|  | @@ -29,7 +29,9 @@
 | 
											
												
													
														|  |  // Author: sameeragarwal@google.com (Sameer Agarwal)
 |  |  // Author: sameeragarwal@google.com (Sameer Agarwal)
 | 
											
												
													
														|  |  
 |  |  
 | 
											
												
													
														|  |  #include <cmath>
 |  |  #include <cmath>
 | 
											
												
													
														|  | 
 |  | +#include "ceres/autodiff_local_parameterization.h"
 | 
											
												
													
														|  |  #include "ceres/fpclassify.h"
 |  |  #include "ceres/fpclassify.h"
 | 
											
												
													
														|  | 
 |  | +#include "ceres/householder_vector.h"
 | 
											
												
													
														|  |  #include "ceres/internal/autodiff.h"
 |  |  #include "ceres/internal/autodiff.h"
 | 
											
												
													
														|  |  #include "ceres/internal/eigen.h"
 |  |  #include "ceres/internal/eigen.h"
 | 
											
												
													
														|  |  #include "ceres/local_parameterization.h"
 |  |  #include "ceres/local_parameterization.h"
 | 
											
										
											
												
													
														|  | @@ -244,6 +246,11 @@ void QuaternionParameterizationTestHelper(const double* x,
 | 
											
												
													
														|  |    EXPECT_EQ((local_matrix - expected_local_matrix).norm(), 0.0);
 |  |    EXPECT_EQ((local_matrix - expected_local_matrix).norm(), 0.0);
 | 
											
												
													
														|  |  }
 |  |  }
 | 
											
												
													
														|  |  
 |  |  
 | 
											
												
													
														|  | 
 |  | +template <int N>
 | 
											
												
													
														|  | 
 |  | +void Normalize(double* x) {
 | 
											
												
													
														|  | 
 |  | +  VectorRef(x, N).normalize();
 | 
											
												
													
														|  | 
 |  | +}
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  |  TEST(QuaternionParameterization, ZeroTest) {
 |  |  TEST(QuaternionParameterization, ZeroTest) {
 | 
											
												
													
														|  |    double x[4] = {0.5, 0.5, 0.5, 0.5};
 |  |    double x[4] = {0.5, 0.5, 0.5, 0.5};
 | 
											
												
													
														|  |    double delta[3] = {0.0, 0.0, 0.0};
 |  |    double delta[3] = {0.0, 0.0, 0.0};
 | 
											
										
											
												
													
														|  | @@ -251,16 +258,9 @@ TEST(QuaternionParameterization, ZeroTest) {
 | 
											
												
													
														|  |    QuaternionParameterizationTestHelper(x, delta, q_delta);
 |  |    QuaternionParameterizationTestHelper(x, delta, q_delta);
 | 
											
												
													
														|  |  }
 |  |  }
 | 
											
												
													
														|  |  
 |  |  
 | 
											
												
													
														|  | -
 |  | 
 | 
											
												
													
														|  |  TEST(QuaternionParameterization, NearZeroTest) {
 |  |  TEST(QuaternionParameterization, NearZeroTest) {
 | 
											
												
													
														|  |    double x[4] = {0.52, 0.25, 0.15, 0.45};
 |  |    double x[4] = {0.52, 0.25, 0.15, 0.45};
 | 
											
												
													
														|  | -  double norm_x = sqrt(x[0] * x[0] +
 |  | 
 | 
											
												
													
														|  | -                       x[1] * x[1] +
 |  | 
 | 
											
												
													
														|  | -                       x[2] * x[2] +
 |  | 
 | 
											
												
													
														|  | -                       x[3] * x[3]);
 |  | 
 | 
											
												
													
														|  | -  for (int i = 0; i < 4; ++i) {
 |  | 
 | 
											
												
													
														|  | -    x[i] = x[i] / norm_x;
 |  | 
 | 
											
												
													
														|  | -  }
 |  | 
 | 
											
												
													
														|  | 
 |  | +  Normalize<4>(x);
 | 
											
												
													
														|  |  
 |  |  
 | 
											
												
													
														|  |    double delta[3] = {0.24, 0.15, 0.10};
 |  |    double delta[3] = {0.24, 0.15, 0.10};
 | 
											
												
													
														|  |    for (int i = 0; i < 3; ++i) {
 |  |    for (int i = 0; i < 3; ++i) {
 | 
											
										
											
												
													
														|  | @@ -278,14 +278,7 @@ TEST(QuaternionParameterization, NearZeroTest) {
 | 
											
												
													
														|  |  
 |  |  
 | 
											
												
													
														|  |  TEST(QuaternionParameterization, AwayFromZeroTest) {
 |  |  TEST(QuaternionParameterization, AwayFromZeroTest) {
 | 
											
												
													
														|  |    double x[4] = {0.52, 0.25, 0.15, 0.45};
 |  |    double x[4] = {0.52, 0.25, 0.15, 0.45};
 | 
											
												
													
														|  | -  double norm_x = sqrt(x[0] * x[0] +
 |  | 
 | 
											
												
													
														|  | -                       x[1] * x[1] +
 |  | 
 | 
											
												
													
														|  | -                       x[2] * x[2] +
 |  | 
 | 
											
												
													
														|  | -                       x[3] * x[3]);
 |  | 
 | 
											
												
													
														|  | -
 |  | 
 | 
											
												
													
														|  | -  for (int i = 0; i < 4; ++i) {
 |  | 
 | 
											
												
													
														|  | -    x[i] = x[i] / norm_x;
 |  | 
 | 
											
												
													
														|  | -  }
 |  | 
 | 
											
												
													
														|  | 
 |  | +  Normalize<4>(x);
 | 
											
												
													
														|  |  
 |  |  
 | 
											
												
													
														|  |    double delta[3] = {0.24, 0.15, 0.10};
 |  |    double delta[3] = {0.24, 0.15, 0.10};
 | 
											
												
													
														|  |    const double delta_norm = sqrt(delta[0] * delta[0] +
 |  |    const double delta_norm = sqrt(delta[0] * delta[0] +
 | 
											
										
											
												
													
														|  | @@ -300,6 +293,154 @@ TEST(QuaternionParameterization, AwayFromZeroTest) {
 | 
											
												
													
														|  |    QuaternionParameterizationTestHelper(x, delta, q_delta);
 |  |    QuaternionParameterizationTestHelper(x, delta, q_delta);
 | 
											
												
													
														|  |  }
 |  |  }
 | 
											
												
													
														|  |  
 |  |  
 | 
											
												
													
														|  | 
 |  | +// Functor needed to implement automatically differentiated Plus for
 | 
											
												
													
														|  | 
 |  | +// homogeneous vectors. Note this explicitly defined for vectors of size 4.
 | 
											
												
													
														|  | 
 |  | +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);
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +    const Scalar squared_norm_delta =
 | 
											
												
													
														|  | 
 |  | +        delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +    Eigen::Matrix<Scalar, 4, 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);
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +    } 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);
 | 
											
												
													
														|  | 
 |  | +    }
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> v(4);
 | 
											
												
													
														|  | 
 |  | +    Scalar beta;
 | 
											
												
													
														|  | 
 |  | +    internal::ComputeHouseholderVector<Scalar>(x, &v, &beta);
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +    x_plus_delta = x.norm() * (y - v * (beta * v.dot(y)));
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +    return true;
 | 
											
												
													
														|  | 
 |  | +  }
 | 
											
												
													
														|  | 
 |  | +};
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +void HomogeneousVectorParameterizationHelper(const double* x,
 | 
											
												
													
														|  | 
 |  | +                                             const double* delta) {
 | 
											
												
													
														|  | 
 |  | +  const double kTolerance = 1e-14;
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  HomogeneousVectorParameterization homogeneous_vector_parameterization(4);
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  // Ensure the update maintains the norm.
 | 
											
												
													
														|  | 
 |  | +  double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};
 | 
											
												
													
														|  | 
 |  | +  homogeneous_vector_parameterization.Plus(x, delta, x_plus_delta);
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  const double x_plus_delta_norm =
 | 
											
												
													
														|  | 
 |  | +      sqrt(x_plus_delta[0] * x_plus_delta[0] +
 | 
											
												
													
														|  | 
 |  | +           x_plus_delta[1] * x_plus_delta[1] +
 | 
											
												
													
														|  | 
 |  | +           x_plus_delta[2] * x_plus_delta[2] +
 | 
											
												
													
														|  | 
 |  | +           x_plus_delta[3] * x_plus_delta[3]);
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  const double x_norm = sqrt(x[0] * x[0] + x[1] * x[1] +
 | 
											
												
													
														|  | 
 |  | +                             x[2] * x[2] + x[3] * x[3]);
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  EXPECT_NEAR(x_plus_delta_norm, x_norm, kTolerance);
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  // Autodiff jacobian at delta_x = 0.
 | 
											
												
													
														|  | 
 |  | +  AutoDiffLocalParameterization<HomogeneousVectorParameterizationPlus, 4, 3>
 | 
											
												
													
														|  | 
 |  | +      autodiff_jacobian;
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  double jacobian_autodiff[12];
 | 
											
												
													
														|  | 
 |  | +  double jacobian_analytic[12];
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  homogeneous_vector_parameterization.ComputeJacobian(x, jacobian_analytic);
 | 
											
												
													
														|  | 
 |  | +  autodiff_jacobian.ComputeJacobian(x, jacobian_autodiff);
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  for (int i = 0; i < 12; ++i) {
 | 
											
												
													
														|  | 
 |  | +    EXPECT_TRUE(ceres::IsFinite(jacobian_analytic[i]));
 | 
											
												
													
														|  | 
 |  | +    EXPECT_NEAR(jacobian_analytic[i], jacobian_autodiff[i], kTolerance)
 | 
											
												
													
														|  | 
 |  | +        << "Jacobian mismatch: i = " << i << ", " << jacobian_analytic[i] << " "
 | 
											
												
													
														|  | 
 |  | +        << jacobian_autodiff[i];
 | 
											
												
													
														|  | 
 |  | +  }
 | 
											
												
													
														|  | 
 |  | +}
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +TEST(HomogeneousVectorParameterization, ZeroTest) {
 | 
											
												
													
														|  | 
 |  | +  double x[4] = {0.0, 0.0, 0.0, 1.0};
 | 
											
												
													
														|  | 
 |  | +  Normalize<4>(x);
 | 
											
												
													
														|  | 
 |  | +  double delta[3] = {0.0, 0.0, 0.0};
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  HomogeneousVectorParameterizationHelper(x, delta);
 | 
											
												
													
														|  | 
 |  | +}
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +TEST(HomogeneousVectorParameterization, NearZeroTest1) {
 | 
											
												
													
														|  | 
 |  | +  double x[4] = {1e-5, 1e-5, 1e-5, 1.0};
 | 
											
												
													
														|  | 
 |  | +  Normalize<4>(x);
 | 
											
												
													
														|  | 
 |  | +  double delta[3] = {0.0, 1.0, 0.0};
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  HomogeneousVectorParameterizationHelper(x, delta);
 | 
											
												
													
														|  | 
 |  | +}
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +TEST(HomogeneousVectorParameterization, NearZeroTest2) {
 | 
											
												
													
														|  | 
 |  | +  double x[4] = {0.001, 0.0, 0.0, 0.0};
 | 
											
												
													
														|  | 
 |  | +  double delta[3] = {0.0, 1.0, 0.0};
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  HomogeneousVectorParameterizationHelper(x, delta);
 | 
											
												
													
														|  | 
 |  | +}
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +TEST(HomogeneousVectorParameterization, AwayFromZeroTest1) {
 | 
											
												
													
														|  | 
 |  | +  double x[4] = {0.52, 0.25, 0.15, 0.45};
 | 
											
												
													
														|  | 
 |  | +  Normalize<4>(x);
 | 
											
												
													
														|  | 
 |  | +  double delta[3] = {0.0, 1.0, -0.5};
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  HomogeneousVectorParameterizationHelper(x, delta);
 | 
											
												
													
														|  | 
 |  | +}
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +TEST(HomogeneousVectorParameterization, AwayFromZeroTest2) {
 | 
											
												
													
														|  | 
 |  | +  double x[4] = {0.87, -0.25, -0.34, 0.45};
 | 
											
												
													
														|  | 
 |  | +  Normalize<4>(x);
 | 
											
												
													
														|  | 
 |  | +  double delta[3] = {0.0, 0.0, -0.5};
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  HomogeneousVectorParameterizationHelper(x, delta);
 | 
											
												
													
														|  | 
 |  | +}
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +TEST(HomogeneousVectorParameterization, AwayFromZeroTest3) {
 | 
											
												
													
														|  | 
 |  | +  double x[4] = {0.0, 0.0, 0.0, 2.0};
 | 
											
												
													
														|  | 
 |  | +  double delta[3] = {0.0, 0.0, 0};
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  HomogeneousVectorParameterizationHelper(x, delta);
 | 
											
												
													
														|  | 
 |  | +}
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +TEST(HomogeneousVectorParameterization, AwayFromZeroTest4) {
 | 
											
												
													
														|  | 
 |  | +  double x[4] = {0.2, -1.0, 0.0, 2.0};
 | 
											
												
													
														|  | 
 |  | +  double delta[3] = {1.4, 0.0, -0.5};
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  HomogeneousVectorParameterizationHelper(x, delta);
 | 
											
												
													
														|  | 
 |  | +}
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +TEST(HomogeneousVectorParameterization, AwayFromZeroTest5) {
 | 
											
												
													
														|  | 
 |  | +  double x[4] = {2.0, 0.0, 0.0, 0.0};
 | 
											
												
													
														|  | 
 |  | +  double delta[3] = {1.4, 0.0, -0.5};
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +  HomogeneousVectorParameterizationHelper(x, delta);
 | 
											
												
													
														|  | 
 |  | +}
 | 
											
												
													
														|  | 
 |  | +
 | 
											
												
													
														|  | 
 |  | +TEST(HomogeneousVectorParameterization, DeathTests) {
 | 
											
												
													
														|  | 
 |  | +  EXPECT_DEATH_IF_SUPPORTED(HomogeneousVectorParameterization x(1), "size");
 | 
											
												
													
														|  | 
 |  | +}
 | 
											
												
													
														|  |  
 |  |  
 | 
											
												
													
														|  |  }  // namespace internal
 |  |  }  // namespace internal
 | 
											
												
													
														|  |  }  // namespace ceres
 |  |  }  // namespace ceres
 |