Add a quaternion local parameterization for Eigen's quaternion element convention. Change-Id: I7046e8b24805313c5fb6a767de581d0054fcdb83
diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst index e315e4a..311f8ae 100644 --- a/docs/source/nnls_modeling.rst +++ b/docs/source/nnls_modeling.rst
@@ -1273,6 +1273,19 @@ product. :class:`QuaternionParameterization` is an implementation of :eq:`quaternion`. +.. class:: EigenQuaternionParameterization + + Eigen uses a different internal memory layout for the elements of the + quaternion than what is commonly used. Specifically, Eigen stores the + elements in memory as [x, y, z, w] where the real part is last + whereas it is typically stored first. Note, when creating an Eigen + quaternion through the constructor the elements are accepted in w, x, + y, z order. Since Ceres operates on parameter blocks which are raw + double pointers this difference is important and requires a different + parameterization. :class:`EigenQuaternionParameterization` uses the + same update as :class:`QuaternionParameterization` but takes into + account Eigen's internal memory element ordering. + .. class:: HomogeneousVectorParameterization In computer vision, homogeneous vectors are commonly used to
diff --git a/include/ceres/local_parameterization.h b/include/ceres/local_parameterization.h index 67633de..379fc68 100644 --- a/include/ceres/local_parameterization.h +++ b/include/ceres/local_parameterization.h
@@ -211,6 +211,28 @@ virtual int LocalSize() const { return 3; } }; +// Implements the quaternion local parameterization for Eigen's representation +// of the quaternion. Eigen uses a different internal memory layout for the +// elements of the quaternion than what is commonly used. Specifically, Eigen +// stores the elements in memory as [x, y, z, w] where the real part is last +// whereas it is typically stored first. Note, when creating an Eigen quaternion +// through the constructor the elements are accepted in w, x, y, z order. Since +// Ceres operates on parameter blocks which are raw double pointers this +// difference is important and requires a different parameterization. +// +// Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x +// with * being the quaternion multiplication operator. +class EigenQuaternionParameterization : public ceres::LocalParameterization { + public: + virtual ~EigenQuaternionParameterization() {} + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const; + virtual bool ComputeJacobian(const double* x, + double* jacobian) const; + virtual int GlobalSize() const { return 4; } + virtual int LocalSize() const { return 3; } +}; // This provides a parameterization for homogeneous vectors which are commonly // used in Structure for Motion problems. One example where they are used is
diff --git a/internal/ceres/local_parameterization.cc b/internal/ceres/local_parameterization.cc index 9e05cba..c1e0cda 100644 --- a/internal/ceres/local_parameterization.cc +++ b/internal/ceres/local_parameterization.cc
@@ -31,6 +31,7 @@ #include "ceres/local_parameterization.h" #include <algorithm> +#include "Eigen/Geometry" #include "ceres/householder_vector.h" #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" @@ -185,6 +186,39 @@ return true; } +bool EigenQuaternionParameterization::Plus(const double* x_ptr, + const double* delta, + double* x_plus_delta_ptr) const { + Eigen::Map<Eigen::Quaterniond> x_plus_delta(x_plus_delta_ptr); + Eigen::Map<const Eigen::Quaterniond> x(x_ptr); + + const double norm_delta = + sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); + if (norm_delta > 0.0) { + const double sin_delta_by_delta = sin(norm_delta) / norm_delta; + + // Note, in the constructor w is first. + Eigen::Quaterniond delta_q(cos(norm_delta), + sin_delta_by_delta * delta[0], + sin_delta_by_delta * delta[1], + sin_delta_by_delta * delta[2]); + x_plus_delta = delta_q * x; + } else { + x_plus_delta = x; + } + + return true; +} + +bool EigenQuaternionParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT + jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT + jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT + jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT + return true; +} + HomogeneousVectorParameterization::HomogeneousVectorParameterization(int size) : size_(size) { CHECK_GT(size_, 1) << "The size of the homogeneous vector needs to be "
diff --git a/internal/ceres/local_parameterization_test.cc b/internal/ceres/local_parameterization_test.cc index 6e27abc..d861a95 100644 --- a/internal/ceres/local_parameterization_test.cc +++ b/internal/ceres/local_parameterization_test.cc
@@ -30,6 +30,7 @@ #include <cmath> #include <limits> +#include "Eigen/Geometry" #include "ceres/autodiff_local_parameterization.h" #include "ceres/fpclassify.h" #include "ceres/householder_vector.h" @@ -185,21 +186,20 @@ } }; -void QuaternionParameterizationTestHelper(const double* x, - const double* delta, - const double* q_delta) { +template<typename Parameterization, typename Plus> +void QuaternionParameterizationTestHelper( + const double* x, const double* delta, + const double* x_plus_delta_ref) { const int kGlobalSize = 4; const int kLocalSize = 3; const double kTolerance = 1e-14; - double x_plus_delta_ref[kGlobalSize] = {0.0, 0.0, 0.0, 0.0}; - QuaternionProduct(q_delta, x, x_plus_delta_ref); double x_plus_delta[kGlobalSize] = {0.0, 0.0, 0.0, 0.0}; - QuaternionParameterization parameterization; + Parameterization parameterization; parameterization.Plus(x, delta, x_plus_delta); for (int i = 0; i < kGlobalSize; ++i) { - EXPECT_NEAR(x_plus_delta[i], x_plus_delta_ref[i], kTolerance); + EXPECT_NEAR(x_plus_delta[i], x_plus_delta[i], kTolerance); } const double x_plus_delta_norm = @@ -216,10 +216,10 @@ double* jacobian_array[2] = { NULL, jacobian_ref }; // Autodiff jacobian at delta_x = 0. - internal::AutoDiff<QuaternionPlus, + internal::AutoDiff<Plus, double, kGlobalSize, - kLocalSize>::Differentiate(QuaternionPlus(), + kLocalSize>::Differentiate(Plus(), parameters, kGlobalSize, x_plus_delta, @@ -259,7 +259,10 @@ double x[4] = {0.5, 0.5, 0.5, 0.5}; double delta[3] = {0.0, 0.0, 0.0}; double q_delta[4] = {1.0, 0.0, 0.0, 0.0}; - QuaternionParameterizationTestHelper(x, delta, q_delta); + double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; + QuaternionProduct(q_delta, x, x_plus_delta); + QuaternionParameterizationTestHelper<QuaternionParameterization, + QuaternionPlus>(x, delta, x_plus_delta); } TEST(QuaternionParameterization, NearZeroTest) { @@ -277,7 +280,10 @@ q_delta[2] = delta[1]; q_delta[3] = delta[2]; - QuaternionParameterizationTestHelper(x, delta, q_delta); + double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; + QuaternionProduct(q_delta, x, x_plus_delta); + QuaternionParameterizationTestHelper<QuaternionParameterization, + QuaternionPlus>(x, delta, x_plus_delta); } TEST(QuaternionParameterization, AwayFromZeroTest) { @@ -294,7 +300,88 @@ q_delta[2] = sin(delta_norm) / delta_norm * delta[1]; q_delta[3] = sin(delta_norm) / delta_norm * delta[2]; - QuaternionParameterizationTestHelper(x, delta, q_delta); + double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; + QuaternionProduct(q_delta, x, x_plus_delta); + QuaternionParameterizationTestHelper<QuaternionParameterization, + QuaternionPlus>(x, delta, x_plus_delta); +} + +// Functor needed to implement automatically differentiated Plus for +// Eigen's quaternion. +struct EigenQuaternionPlus { + template<typename T> + bool operator()(const T* x, const T* delta, T* x_plus_delta) const { + const T norm_delta = + sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); + + Eigen::Quaternion<T> q_delta; + if (norm_delta > T(0.0)) { + const T sin_delta_by_delta = sin(norm_delta) / norm_delta; + q_delta.coeffs() << sin_delta_by_delta * delta[0], + sin_delta_by_delta * delta[1], sin_delta_by_delta * delta[2], + cos(norm_delta); + } else { + // We do not just use q_delta = [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. + q_delta.coeffs() << delta[0], delta[1], delta[2], T(1.0); + } + + Eigen::Map<Eigen::Quaternion<T> > x_plus_delta_ref(x_plus_delta); + Eigen::Map<const Eigen::Quaternion<T> > x_ref(x); + x_plus_delta_ref = q_delta * x_ref; + return true; + } +}; + +TEST(EigenQuaternionParameterization, ZeroTest) { + Eigen::Quaterniond x(0.5, 0.5, 0.5, 0.5); + double delta[3] = {0.0, 0.0, 0.0}; + Eigen::Quaterniond q_delta(1.0, 0.0, 0.0, 0.0); + Eigen::Quaterniond x_plus_delta = q_delta * x; + QuaternionParameterizationTestHelper<EigenQuaternionParameterization, + EigenQuaternionPlus>( + x.coeffs().data(), delta, x_plus_delta.coeffs().data()); +} + +TEST(EigenQuaternionParameterization, NearZeroTest) { + Eigen::Quaterniond x(0.52, 0.25, 0.15, 0.45); + x.normalize(); + + double delta[3] = {0.24, 0.15, 0.10}; + for (int i = 0; i < 3; ++i) { + delta[i] = delta[i] * 1e-14; + } + + // Note: w is first in the constructor. + Eigen::Quaterniond q_delta(1.0, delta[0], delta[1], delta[2]); + + Eigen::Quaterniond x_plus_delta = q_delta * x; + QuaternionParameterizationTestHelper<EigenQuaternionParameterization, + EigenQuaternionPlus>( + x.coeffs().data(), delta, x_plus_delta.coeffs().data()); +} + +TEST(EigenQuaternionParameterization, AwayFromZeroTest) { + Eigen::Quaterniond x(0.52, 0.25, 0.15, 0.45); + x.normalize(); + + double delta[3] = {0.24, 0.15, 0.10}; + const double delta_norm = sqrt(delta[0] * delta[0] + + delta[1] * delta[1] + + delta[2] * delta[2]); + + // Note: w is first in the constructor. + Eigen::Quaterniond q_delta(cos(delta_norm), + sin(delta_norm) / delta_norm * delta[0], + sin(delta_norm) / delta_norm * delta[1], + sin(delta_norm) / delta_norm * delta[2]); + + Eigen::Quaterniond x_plus_delta = q_delta * x; + QuaternionParameterizationTestHelper<EigenQuaternionParameterization, + EigenQuaternionPlus>( + x.coeffs().data(), delta, x_plus_delta.coeffs().data()); } // Functor needed to implement automatically differentiated Plus for