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 {
