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
