Add line local parameterization. This CL adds a local parameterization for a n-dimensional line, which is represented as an origin point and a direction. The line direction is updated in the same way as a homogeneous vector and the origin point is updated perpendicular to the line direction. Change-Id: I733f395e5cc4250abf9778c26fe0a5ae1de6b624
diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst index 2604027..0883c6d 100644 --- a/docs/source/nnls_modeling.rst +++ b/docs/source/nnls_modeling.rst
@@ -1339,6 +1339,7 @@ same update as :class:`QuaternionParameterization` but takes into account Eigen's internal memory element ordering. +.. _`homogeneous_vector_parameterization`: .. class:: HomogeneousVectorParameterization In computer vision, homogeneous vectors are commonly used to @@ -1362,6 +1363,27 @@ last element of :math:`x` is the scalar component of the homogeneous vector. +.. class:: LineParameterization + + This class provides a parameterization for lines, where the line is + over-parameterized by an origin point and a direction vector. So the + parameter vector size needs to be two times the ambient space dimension, + where the first half is interpreted as the origin point and the second + half as the direction. + + To give an example: Given n distinct points in 3D (measurements) we search + for the line which has the closest distance to all of these. We parameterize + the line with a 3D origin point and a 3D direction vector. As a cost + function the distance between the line and the given points is used. + We use six parameters for the line (two 3D vectors) but a line in 3D only + has four degrees of freedom. To make the over-parameterization visible to + the optimizer and covariance estimator this line parameterization can be + used. + + The plus operator for the line direction is the same as for the + :ref:`HomogeneousVectorParameterization <homogeneous_vector_parameterization>`. + The update of the origin point is perpendicular to the line direction + before the update. .. class:: ProductParameterization
diff --git a/internal/ceres/householder_vector.h b/include/ceres/internal/householder_vector.h similarity index 89% rename from internal/ceres/householder_vector.h rename to include/ceres/internal/householder_vector.h index 6d85217..051a88d 100644 --- a/internal/ceres/householder_vector.h +++ b/include/ceres/internal/householder_vector.h
@@ -28,8 +28,8 @@ // // Author: vitus@google.com (Michael Vitus) -#ifndef CERES_PUBLIC_HOUSEHOLDER_VECTOR_H_ -#define CERES_PUBLIC_HOUSEHOLDER_VECTOR_H_ +#ifndef CERES_PUBLIC_INTERNAL_HOUSEHOLDER_VECTOR_H_ +#define CERES_PUBLIC_INTERNAL_HOUSEHOLDER_VECTOR_H_ #include "Eigen/Core" #include "glog/logging.h" @@ -42,9 +42,9 @@ // vector as pivot instead of first. This computes the vector v with v(n) = 1 // and beta such that H = I - beta * v * v^T is orthogonal and // H * x = ||x||_2 * e_n. -template <typename Scalar> -void ComputeHouseholderVector(const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& x, - Eigen::Matrix<Scalar, Eigen::Dynamic, 1>* v, +template <typename Derived, typename Scalar, int N> +void ComputeHouseholderVector(const Eigen::DenseBase<Derived>& x, + Eigen::Matrix<Scalar, N, 1>* v, Scalar* beta) { CHECK(beta != nullptr); CHECK(v != nullptr); @@ -82,4 +82,4 @@ } // namespace internal } // namespace ceres -#endif // CERES_PUBLIC_HOUSEHOLDER_VECTOR_H_ +#endif // CERES_PUBLIC_INTERNAL_HOUSEHOLDER_VECTOR_H_
diff --git a/include/ceres/internal/line_parameterization.h b/include/ceres/internal/line_parameterization.h new file mode 100644 index 0000000..b2ec9e1 --- /dev/null +++ b/include/ceres/internal/line_parameterization.h
@@ -0,0 +1,172 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2020 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: jodebo_beck@gmx.de (Johannes Beck) +// + +#ifndef CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_ +#define CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_ + +#include "householder_vector.h" + +namespace ceres { + +template <int AmbientSpaceDimension> +bool LineParameterization<AmbientSpaceDimension>::Plus( + const double* x_ptr, + const double* delta_ptr, + double* x_plus_delta_ptr) const { + // We seek a box plus operator of the form + // + // [o*, d*] = Plus([o, d], [delta_o, delta_d]) + // + // where o is the origin point, d is the direction vector, delta_o is + // the delta of the origin point and delta_d the delta of the direction and + // o* and d* is the updated origin point and direction. + // + // We separate the Plus operator into the origin point and directional part + // d* = Plus_d(d, delta_d) + // o* = Plus_o(o, d, delta_o) + // + // The direction update function Plus_d is the same as for the homogeneous vector + // parameterization: + // + // d* = H_{v(d)} [0.5 sinc(0.5 |delta_d|) delta_d, cos(0.5 |delta_d|)]^T + // + // where H is the householder matrix + // H_{v} = I - (2 / |v|^2) v v^T + // and + // v(d) = d - sign(d_n) |d| e_n. + // + // The origin point update function Plus_o is defined as + // + // o* = o + H_{v(d)} [0.5 delta_o, 0]^T. + + static constexpr int kDim = AmbientSpaceDimension; + using AmbientVector = Eigen::Matrix<double, kDim, 1>; + using AmbientVectorRef = Eigen::Map<Eigen::Matrix<double, kDim, 1>>; + using ConstAmbientVectorRef = Eigen::Map<const Eigen::Matrix<double, kDim, 1>>; + using ConstTangentVectorRef = + Eigen::Map<const Eigen::Matrix<double, kDim - 1, 1>>; + + + ConstAmbientVectorRef o(x_ptr); + ConstAmbientVectorRef d(x_ptr + kDim); + + ConstTangentVectorRef delta_o(delta_ptr); + ConstTangentVectorRef delta_d(delta_ptr + kDim - 1); + AmbientVectorRef o_plus_delta(x_plus_delta_ptr); + AmbientVectorRef d_plus_delta(x_plus_delta_ptr + kDim); + + const double norm_delta_d = delta_d.norm(); + + o_plus_delta = o; + + // Shortcut for zero delta direction. + if (norm_delta_d == 0.0) { + d_plus_delta = d; + + if (delta_o.isZero(0.0)) { + return true; + } + } + + // Calculate the householder transformation which is needed for f_d and f_o. + AmbientVector v; + double beta; + internal::ComputeHouseholderVector(d, &v, &beta); + + if (norm_delta_d != 0.0) { + // Map the delta from the minimum representation to the over parameterized + // homogeneous vector. See section A6.9.2 on page 624 of Hartley & Zisserman + // (2nd Edition) for a detailed description. Note there is a typo on Page + // 625, line 4 so check the book errata. + const double norm_delta_div_2 = 0.5 * norm_delta_d; + const double sin_delta_by_delta = + std::sin(norm_delta_div_2) / norm_delta_div_2; + + // Apply the delta update to remain on the unit sphere. See section A6.9.3 + // on page 625 of Hartley & Zisserman (2nd Edition) for a detailed + // description. + AmbientVector y; + y.template head<kDim - 1>() = 0.5 * sin_delta_by_delta * delta_d; + y[kDim - 1] = std::cos(norm_delta_div_2); + + d_plus_delta = d.norm() * (y - v * (beta * (v.transpose() * y))); + } + + // The null space is in the direction of the line, so the tangent space is + // perpendicular to the line direction. This is achieved by using the + // householder matrix of the direction and allow only movements + // perpendicular to e_n. + // + // The factor of 0.5 is used to be consistent with the line direction + // update. + AmbientVector y; + y << 0.5 * delta_o, 0; + o_plus_delta += y - v * (beta * (v.transpose() * y)); + + return true; +} + +template <int AmbientSpaceDimension> +bool LineParameterization<AmbientSpaceDimension>::ComputeJacobian( + const double* x_ptr, double* jacobian_ptr) const { + static constexpr int kDim = AmbientSpaceDimension; + using AmbientVector = Eigen::Matrix<double, kDim, 1>; + using ConstAmbientVectorRef = Eigen::Map<const Eigen::Matrix<double, kDim, 1>>; + using MatrixRef = Eigen::Map< + Eigen::Matrix<double, 2 * kDim, 2 * (kDim - 1), Eigen::RowMajor>>; + + ConstAmbientVectorRef d(x_ptr + kDim); + MatrixRef jacobian(jacobian_ptr); + + // Clear the Jacobian as only half of the matrix is not zero. + jacobian.setZero(); + + AmbientVector v; + double beta; + internal::ComputeHouseholderVector(d, &v, &beta); + + // The Jacobian is equal to J = 0.5 * H.leftCols(kDim - 1) where H is + // the Householder matrix (H = I - beta * v * v') for the origin point. For + // the line direction part the Jacobian is scaled by the norm of the + // direction. + for (int i = 0; i < kDim - 1; ++i) { + jacobian.block(0, i, kDim, 1) = -0.5 * beta * v(i) * v; + jacobian.col(i)(i) += 0.5; + } + + jacobian.template block<kDim, kDim - 1>(kDim, kDim - 1) = + jacobian.template block<kDim, kDim - 1>(0, 0) * d.norm(); + return true; +} + +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_LINE_PARAMETERIZATION_H_
diff --git a/include/ceres/local_parameterization.h b/include/ceres/local_parameterization.h index 93c7973..0d7b507 100644 --- a/include/ceres/local_parameterization.h +++ b/include/ceres/local_parameterization.h
@@ -262,6 +262,33 @@ const int size_; }; +// This provides a parameterization for lines, where the line is +// over-parameterized by an origin point and a direction vector. So the +// parameter vector size needs to be two times the ambient space dimension, +// where the first half is interpreted as the origin point and the second half +// as the direction. +// +// The plus operator for the line direction is the same as for the +// HomogeneousVectorParameterization. The update of the origin point is +// perpendicular to the line direction before the update. +// +// This local parameterization is a special case of the affine Grassmannian +// manifold (see https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold)) +// for the case Graff_1(R^n). +template <int AmbientSpaceDimension> +class CERES_EXPORT LineParameterization : public LocalParameterization { + public: + static_assert(AmbientSpaceDimension >= 2, + "The ambient space must be at least 2"); + + bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const override; + bool ComputeJacobian(const double* x, double* jacobian) const override; + int GlobalSize() const override { return 2 * AmbientSpaceDimension; } + int LocalSize() const override { return 2 * (AmbientSpaceDimension - 1); } +}; + // Construct a local parameterization by taking the Cartesian product // of a number of other local parameterizations. This is useful, when // a parameter block is the cartesian product of two or more @@ -328,5 +355,7 @@ } // namespace ceres #include "ceres/internal/reenable_warnings.h" +#include "ceres/internal/line_parameterization.h" #endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ +
diff --git a/internal/ceres/householder_vector_test.cc b/internal/ceres/householder_vector_test.cc index 69a6d3c..9eaca75 100644 --- a/internal/ceres/householder_vector_test.cc +++ b/internal/ceres/householder_vector_test.cc
@@ -28,7 +28,7 @@ // // Author: vitus@google.com (Michael Vitus) -#include "ceres/householder_vector.h" +#include "ceres/internal/householder_vector.h" #include "ceres/internal/eigen.h" #include "glog/logging.h" #include "gtest/gtest.h"
diff --git a/internal/ceres/local_parameterization.cc b/internal/ceres/local_parameterization.cc index b6316f1..5fedefa 100644 --- a/internal/ceres/local_parameterization.cc +++ b/internal/ceres/local_parameterization.cc
@@ -32,7 +32,7 @@ #include <algorithm> #include "Eigen/Geometry" -#include "ceres/householder_vector.h" +#include "ceres/internal/householder_vector.h" #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" #include "ceres/rotation.h" @@ -248,16 +248,16 @@ // (2nd Edition) for a detailed description. Note there is a typo on Page // 625, line 4 so check the book errata. const double norm_delta_div_2 = 0.5 * norm_delta; - const double sin_delta_by_delta = sin(norm_delta_div_2) / + const double sin_delta_by_delta = std::sin(norm_delta_div_2) / norm_delta_div_2; Vector y(size_); y.head(size_ - 1) = 0.5 * sin_delta_by_delta * delta; - y(size_ - 1) = cos(norm_delta_div_2); + y(size_ - 1) = std::cos(norm_delta_div_2); Vector v(size_); double beta; - internal::ComputeHouseholderVector<double>(x, &v, &beta); + internal::ComputeHouseholderVector(x, &v, &beta); // Apply the delta update to remain on the unit sphere. See section A6.9.3 // on page 625 of Hartley & Zisserman (2nd Edition) for a detailed @@ -274,7 +274,7 @@ Vector v(size_); double beta; - internal::ComputeHouseholderVector<double>(x, &v, &beta); + internal::ComputeHouseholderVector(x, &v, &beta); // The Jacobian is equal to J = 0.5 * H.leftCols(size_ - 1) where H is the // Householder matrix (H = I - beta * v * v').
diff --git a/internal/ceres/local_parameterization_test.cc b/internal/ceres/local_parameterization_test.cc index 9334287..f165049 100644 --- a/internal/ceres/local_parameterization_test.cc +++ b/internal/ceres/local_parameterization_test.cc
@@ -36,9 +36,9 @@ #include "Eigen/Geometry" #include "ceres/autodiff_local_parameterization.h" -#include "ceres/householder_vector.h" #include "ceres/internal/autodiff.h" #include "ceres/internal/eigen.h" +#include "ceres/internal/householder_vector.h" #include "ceres/random.h" #include "ceres/rotation.h" #include "gtest/gtest.h" @@ -418,45 +418,41 @@ } // Functor needed to implement automatically differentiated Plus for -// homogeneous vectors. Note this explicitly defined for vectors of size 4. +// homogeneous vectors. +template <int Dim> struct HomogeneousVectorParameterizationPlus { template <typename Scalar> bool operator()(const Scalar* p_x, const Scalar* p_delta, Scalar* p_x_plus_delta) const { - Eigen::Map<const Eigen::Matrix<Scalar, 4, 1>> x(p_x); - Eigen::Map<const Eigen::Matrix<Scalar, 3, 1>> delta(p_delta); - Eigen::Map<Eigen::Matrix<Scalar, 4, 1>> x_plus_delta(p_x_plus_delta); + Eigen::Map<const Eigen::Matrix<Scalar, Dim, 1>> x(p_x); + Eigen::Map<const Eigen::Matrix<Scalar, Dim - 1, 1>> delta(p_delta); + Eigen::Map<Eigen::Matrix<Scalar, Dim, 1>> x_plus_delta(p_x_plus_delta); - const Scalar squared_norm_delta = - delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; + const Scalar squared_norm_delta = delta.squaredNorm(); - Eigen::Matrix<Scalar, 4, 1> y; + Eigen::Matrix<Scalar, Dim, 1> y; Scalar one_half(0.5); if (squared_norm_delta > Scalar(0.0)) { Scalar norm_delta = sqrt(squared_norm_delta); Scalar norm_delta_div_2 = 0.5 * norm_delta; const Scalar sin_delta_by_delta = sin(norm_delta_div_2) / norm_delta_div_2; - y[0] = sin_delta_by_delta * delta[0] * one_half; - y[1] = sin_delta_by_delta * delta[1] * one_half; - y[2] = sin_delta_by_delta * delta[2] * one_half; - y[3] = cos(norm_delta_div_2); + y.template head<Dim - 1>() = sin_delta_by_delta * one_half * delta; + y[Dim - 1] = cos(norm_delta_div_2); } else { // We do not just use y = [0,0,0,1] here because that is a // constant and when used for automatic differentiation will // lead to a zero derivative. Instead we take a first order // approximation and evaluate it at zero. - y[0] = delta[0] * one_half; - y[1] = delta[1] * one_half; - y[2] = delta[2] * one_half; - y[3] = Scalar(1.0); + y.template head<Dim - 1>() = delta * one_half; + y[Dim - 1] = Scalar(1.0); } - Eigen::Matrix<Scalar, Eigen::Dynamic, 1> v(4); + Eigen::Matrix<Scalar, Dim, 1> v; Scalar beta; - internal::ComputeHouseholderVector<Scalar>(x, &v, &beta); + internal::ComputeHouseholderVector(x, &v, &beta); x_plus_delta = x.norm() * (y - v * (beta * v.dot(y))); @@ -484,7 +480,7 @@ EXPECT_NEAR(x_plus_delta_norm, x_norm, kTolerance); // Autodiff jacobian at delta_x = 0. - AutoDiffLocalParameterization<HomogeneousVectorParameterizationPlus, 4, 3> + AutoDiffLocalParameterization<HomogeneousVectorParameterizationPlus<4>, 4, 3> autodiff_jacobian; double jacobian_autodiff[12]; @@ -565,6 +561,177 @@ EXPECT_DEATH_IF_SUPPORTED(HomogeneousVectorParameterization x(1), "size"); } +// Functor needed to implement automatically differentiated Plus for +// line parameterization. +template <int AmbientSpaceDim> +struct LineParameterizationPlus { + template <typename Scalar> + bool operator()(const Scalar* p_x, + const Scalar* p_delta, + Scalar* p_x_plus_delta) const { + static constexpr int kTangetSpaceDim = AmbientSpaceDim - 1; + Eigen::Map<const Eigen::Matrix<Scalar, AmbientSpaceDim, 1>> origin_point( + p_x); + Eigen::Map<const Eigen::Matrix<Scalar, AmbientSpaceDim, 1>> dir( + p_x + AmbientSpaceDim); + Eigen::Map<const Eigen::Matrix<Scalar, kTangetSpaceDim, 1>> + delta_origin_point(p_delta); + Eigen::Map<Eigen::Matrix<Scalar, AmbientSpaceDim, 1>> + origin_point_plus_delta(p_x_plus_delta); + + HomogeneousVectorParameterizationPlus<AmbientSpaceDim> dir_plus; + dir_plus(dir.data(), + p_delta + kTangetSpaceDim, + p_x_plus_delta + AmbientSpaceDim); + + Eigen::Matrix<Scalar, AmbientSpaceDim, 1> v; + Scalar beta; + internal::ComputeHouseholderVector(dir, &v, &beta); + + Eigen::Matrix<Scalar, AmbientSpaceDim, 1> y; + y << 0.5 * delta_origin_point, Scalar(0.0); + origin_point_plus_delta = origin_point + y - v * (beta * v.dot(y)); + + return true; + } +}; + +template <int AmbientSpaceDim> +static void LineParameterizationHelper(const double* x_ptr, + const double* delta) { + const double kTolerance = 1e-14; + + static constexpr int ParameterDim = 2 * AmbientSpaceDim; + static constexpr int TangientParameterDim = 2 * (AmbientSpaceDim - 1); + + LineParameterization<AmbientSpaceDim> line_parameterization; + + using ParameterVector = Eigen::Matrix<double, ParameterDim, 1>; + ParameterVector x_plus_delta = ParameterVector::Zero(); + line_parameterization.Plus(x_ptr, delta, x_plus_delta.data()); + + // Ensure the update maintains the norm for the line direction. + Eigen::Map<const ParameterVector> x(x_ptr); + const double dir_plus_delta_norm = + x_plus_delta.template tail<AmbientSpaceDim>().norm(); + const double dir_norm = x.template tail<AmbientSpaceDim>().norm(); + EXPECT_NEAR(dir_plus_delta_norm, dir_norm, kTolerance); + + // Ensure the update of the origin point is perpendicular to the line + // direction. + const double dot_prod_val = x.template tail<AmbientSpaceDim>().dot( + x_plus_delta.template head<AmbientSpaceDim>() - + x.template head<AmbientSpaceDim>()); + EXPECT_NEAR(dot_prod_val, 0.0, kTolerance); + + // Autodiff jacobian at delta_x = 0. + AutoDiffLocalParameterization<LineParameterizationPlus<AmbientSpaceDim>, + ParameterDim, + TangientParameterDim> + autodiff_jacobian; + + using JacobianMatrix = Eigen:: + Matrix<double, ParameterDim, TangientParameterDim, Eigen::RowMajor>; + constexpr double kNaN = std::numeric_limits<double>::quiet_NaN(); + JacobianMatrix jacobian_autodiff = JacobianMatrix::Constant(kNaN); + JacobianMatrix jacobian_analytic = JacobianMatrix::Constant(kNaN); + + autodiff_jacobian.ComputeJacobian(x_ptr, jacobian_autodiff.data()); + line_parameterization.ComputeJacobian(x_ptr, jacobian_analytic.data()); + + EXPECT_FALSE(jacobian_autodiff.hasNaN()); + EXPECT_FALSE(jacobian_analytic.hasNaN()); + EXPECT_TRUE(jacobian_autodiff.isApprox(jacobian_analytic)) + << "auto diff:\n" + << jacobian_autodiff << "\n" + << "analytic diff:\n" + << jacobian_analytic; +} + +TEST(LineParameterization, ZeroTest3D) { + double x[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 1.0}; + double delta[4] = {0.0, 0.0, 0.0, 0.0}; + + LineParameterizationHelper<3>(x, delta); +} + +TEST(LineParameterization, ZeroTest4D) { + double x[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}; + double delta[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + + LineParameterizationHelper<4>(x, delta); +} + +TEST(LineParameterization, ZeroOriginPointTest3D) { + double x[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 1.0}; + double delta[4] = {0.0, 0.0, 1.0, 2.0}; + + LineParameterizationHelper<3>(x, delta); +} + +TEST(LineParameterization, ZeroOriginPointTest4D) { + double x[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}; + double delta[6] = {0.0, 0.0, 0.0, 1.0, 2.0, 3.0}; + + LineParameterizationHelper<4>(x, delta); +} + +TEST(LineParameterization, ZeroDirTest3D) { + double x[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 1.0}; + double delta[4] = {3.0, 2.0, 0.0, 0.0}; + + LineParameterizationHelper<3>(x, delta); +} + +TEST(LineParameterization, ZeroDirTest4D) { + double x[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0}; + double delta[6] = {3.0, 2.0, 1.0, 0.0, 0.0, 0.0}; + + LineParameterizationHelper<4>(x, delta); +} + +TEST(LineParameterization, AwayFromZeroTest3D1) { + Eigen::Matrix<double, 6, 1> x; + x.head<3>() << 1.54, 2.32, 1.34; + x.tail<3>() << 0.52, 0.25, 0.15; + x.tail<3>().normalize(); + + double delta[4] = {4.0, 7.0, 1.0, -0.5}; + + LineParameterizationHelper<3>(x.data(), delta); +} + +TEST(LineParameterization, AwayFromZeroTest4D1) { + Eigen::Matrix<double, 8, 1> x; + x.head<4>() << 1.54, 2.32, 1.34, 3.23; + x.tail<4>() << 0.52, 0.25, 0.15, 0.45; + x.tail<4>().normalize(); + + double delta[6] = {4.0, 7.0, -3.0, 0.0, 1.0, -0.5}; + + LineParameterizationHelper<4>(x.data(), delta); +} + +TEST(LineParameterization, AwayFromZeroTest3D2) { + Eigen::Matrix<double, 6, 1> x; + x.head<3>() << 7.54, -2.81, 8.63; + x.tail<3>() << 2.52, 5.25, 4.15; + + double delta[4] = {4.0, 7.0, 1.0, -0.5}; + + LineParameterizationHelper<3>(x.data(), delta); +} + +TEST(LineParameterization, AwayFromZeroTest4D2) { + Eigen::Matrix<double, 8, 1> x; + x.head<4>() << 7.54, -2.81, 8.63, 6.93; + x.tail<4>() << 2.52, 5.25, 4.15, 1.45; + + double delta[6] = {4.0, 7.0, -3.0, 2.0, 1.0, -0.5}; + + LineParameterizationHelper<4>(x.data(), delta); +} + class ProductParameterizationTest : public ::testing::Test { protected: void SetUp() final {