Use Euler parameters in QuaternionPlusImpl
Define the relation between the axis-angle representation of a rotation
and the corresponding unit quaternion as
q(๐) = cos(๐/2) + sin(๐/2)/๐ยท๐
where ๐ = ๐๐ is the rotation vector given by the rotation angle ๐ and
the unit axis of rotation ๐ instead of
q(๐) = cos(๐) + sin(๐)/๐ยท๐ .
This brings the former relation closer to the available functionality
provided by ceres/rotation.h.
Fixes #941
Change-Id: Id40c065f78593887ecc52e67a068737d7eb5bda6
diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst
index be87149..9d33b51 100644
--- a/docs/source/nnls_modeling.rst
+++ b/docs/source/nnls_modeling.rst
@@ -1769,26 +1769,25 @@
struct QuaternionFunctor {
template <typename T>
bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
- const T squared_norm_delta =
- delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
+ const T norm_delta = hypot(delta[0], delta[1], delta[2]);
T q_delta[4];
- if (squared_norm_delta > T(0.0)) {
- T norm_delta = sqrt(squared_norm_delta);
- const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
- q_delta[0] = cos(norm_delta);
- q_delta[1] = sin_delta_by_delta * delta[0];
- q_delta[2] = sin_delta_by_delta * delta[1];
- q_delta[3] = sin_delta_by_delta * delta[2];
+ if (norm_delta > T(0.0)) {
+ T half_norm_delta = norm_delta / T(2.0);
+ const T sin_half_delta_by_delta = sin(half_norm_delta) / norm_delta;
+ q_delta[0] = cos(half_norm_delta);
+ q_delta[1] = sin_half_delta_by_delta * delta[0];
+ q_delta[2] = sin_half_delta_by_delta * delta[1];
+ q_delta[3] = sin_half_delta_by_delta * delta[2];
} else {
// We do not just use q_delta = [1,0,0,0] 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[0] = T(1.0);
- q_delta[1] = delta[0];
- q_delta[2] = delta[1];
- q_delta[3] = delta[2];
+ q_delta[1] = delta[0] / T(2);
+ q_delta[2] = delta[1] / T(2);
+ q_delta[3] = delta[2] / T(2);
}
QuaternionProduct(q_delta, x, x_plus_delta);
@@ -1800,20 +1799,19 @@
T minus_x[4] = {x[0], -x[1], -x[2], -x[3]};
T ambient_y_minus_x[4];
QuaternionProduct(y, minus_x, ambient_y_minus_x);
- T u_norm = sqrt(ambient_y_minus_x[1] * ambient_y_minus_x[1] +
- ambient_y_minus_x[2] * ambient_y_minus_x[2] +
- ambient_y_minus_x[3] * ambient_y_minus_x[3]);
+ T u_norm = hypot(ambient_y_minus_x[1], ambient_y_minus_x[2],
+ ambient_y_minus_x[3]);
if (u_norm > 0.0) {
- T theta = atan2(u_norm, ambient_y_minus_x[0]);
- y_minus_x[0] = theta * ambient_y_minus_x[1] / u_norm;
- y_minus_x[1] = theta * ambient_y_minus_x[2] / u_norm;
- y_minus_x[2] = theta * ambient_y_minus_x[3] / u_norm;
+ T theta = T(2) * atan2(u_norm, ambient_y_minus_x[0]);
+ y_minus_x[0] = theta * ambient_y_minus_x[1] / u_norm;
+ y_minus_x[1] = theta * ambient_y_minus_x[2] / u_norm;
+ y_minus_x[2] = theta * ambient_y_minus_x[3] / u_norm;
} else {
- We do not use [0,0,0] here because even though the value part is
- a constant, the derivative part is not.
- y_minus_x[0] = ambient_y_minus_x[1];
- y_minus_x[1] = ambient_y_minus_x[2];
- y_minus_x[2] = ambient_y_minus_x[3];
+ // We do not use [0,0,0] here because even though the value part is
+ // a constant, the derivative part is not.
+ y_minus_x[0] = T(2) * ambient_y_minus_x[1];
+ y_minus_x[1] = T(2) * ambient_y_minus_x[2];
+ y_minus_x[2] = T(2) * ambient_y_minus_x[3];
}
return true;
}
diff --git a/include/ceres/autodiff_manifold.h b/include/ceres/autodiff_manifold.h
index 4bf7e56..c3169f7 100644
--- a/include/ceres/autodiff_manifold.h
+++ b/include/ceres/autodiff_manifold.h
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2023 Google Inc. All rights reserved.
+// Copyright 2025 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -89,26 +89,25 @@
// struct QuaternionFunctor {
// template <typename T>
// bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
-// const T squared_norm_delta =
-// delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
+// const T norm_delta = hypot(delta[0], delta[1], delta[2]);
//
// T q_delta[4];
-// if (squared_norm_delta > T(0.0)) {
-// T norm_delta = sqrt(squared_norm_delta);
-// const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
-// q_delta[0] = cos(norm_delta);
-// q_delta[1] = sin_delta_by_delta * delta[0];
-// q_delta[2] = sin_delta_by_delta * delta[1];
-// q_delta[3] = sin_delta_by_delta * delta[2];
+// if (norm_delta > T(0.0)) {
+// T half_norm_delta = norm_delta / T(2.0);
+// const T sin_half_delta_by_delta = sin(half_norm_delta) / norm_delta;
+// q_delta[0] = cos(half_norm_delta);
+// q_delta[1] = sin_half_delta_by_delta * delta[0];
+// q_delta[2] = sin_half_delta_by_delta * delta[1];
+// q_delta[3] = sin_half_delta_by_delta * delta[2];
// } else {
// // We do not just use q_delta = [1,0,0,0] 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[0] = T(1.0);
-// q_delta[1] = delta[0];
-// q_delta[2] = delta[1];
-// q_delta[3] = delta[2];
+// q_delta[1] = delta[0] / T(2);
+// q_delta[2] = delta[1] / T(2);
+// q_delta[3] = delta[2] / T(2);
// }
//
// QuaternionProduct(q_delta, x, x_plus_delta);
@@ -120,20 +119,18 @@
// T minus_x[4] = {x[0], -x[1], -x[2], -x[3]};
// T ambient_y_minus_x[4];
// QuaternionProduct(y, minus_x, ambient_y_minus_x);
-// T u_norm = sqrt(ambient_y_minus_x[1] * ambient_y_minus_x[1] +
-// ambient_y_minus_x[2] * ambient_y_minus_x[2] +
-// ambient_y_minus_x[3] * ambient_y_minus_x[3]);
-// if (u_norm > 0.0) {
-// T theta = atan2(u_norm, ambient_y_minus_x[0]);
+// T u_norm = hypot(ambient_y_minus_x[1], ambient_y_minus_x[2],
+// ambient_y_minus_x[3]); if (u_norm > 0.0) {
+// T theta = T(2) * atan2(u_norm, ambient_y_minus_x[0]);
// y_minus_x[0] = theta * ambient_y_minus_x[1] / u_norm;
// y_minus_x[1] = theta * ambient_y_minus_x[2] / u_norm;
// y_minus_x[2] = theta * ambient_y_minus_x[3] / u_norm;
// } else {
// // We do not use [0,0,0] here because even though the value part is
// // a constant, the derivative part is not.
-// y_minus_x[0] = ambient_y_minus_x[1];
-// y_minus_x[1] = ambient_y_minus_x[2];
-// y_minus_x[2] = ambient_y_minus_x[3];
+// y_minus_x[0] = T(2) * ambient_y_minus_x[1];
+// y_minus_x[1] = T(2) * ambient_y_minus_x[2];
+// y_minus_x[2] = T(2) * ambient_y_minus_x[3];
// }
// return true;
// }
diff --git a/internal/ceres/manifold.cc b/internal/ceres/manifold.cc
index 6bd320a..f2cd378 100644
--- a/internal/ceres/manifold.cc
+++ b/internal/ceres/manifold.cc
@@ -37,12 +37,14 @@
return;
}
- const double sin_delta_by_delta = (std::sin(norm_delta) / norm_delta);
+ const double half_norm_delta = norm_delta / 2;
+ const double sin_half_delta_by_delta =
+ (std::sin(half_norm_delta) / norm_delta);
double q_delta[4];
- q_delta[Order::kW] = std::cos(norm_delta);
- q_delta[Order::kX] = sin_delta_by_delta * delta[0];
- q_delta[Order::kY] = sin_delta_by_delta * delta[1];
- q_delta[Order::kZ] = sin_delta_by_delta * delta[2];
+ q_delta[Order::kW] = std::cos(half_norm_delta);
+ q_delta[Order::kX] = sin_half_delta_by_delta * delta[0];
+ q_delta[Order::kY] = sin_half_delta_by_delta * delta[1];
+ q_delta[Order::kZ] = sin_half_delta_by_delta * delta[2];
x_plus_delta[Order::kW] =
q_delta[Order::kW] * x[Order::kW] - q_delta[Order::kX] * x[Order::kX] -
@@ -75,6 +77,7 @@
jacobian(Order::kZ, 0) = x[Order::kY];
jacobian(Order::kZ, 1) = -x[Order::kX];
jacobian(Order::kZ, 2) = x[Order::kW];
+ jacobian /= 2;
}
template <typename Order>
@@ -101,7 +104,7 @@
ambient_y_minus_x[Order::kY],
ambient_y_minus_x[Order::kZ]);
if (std::fpclassify(u_norm) != FP_ZERO) {
- const double theta = std::atan2(u_norm, ambient_y_minus_x[Order::kW]);
+ const double theta = 2 * std::atan2(u_norm, ambient_y_minus_x[Order::kW]);
y_minus_x[0] = theta * ambient_y_minus_x[Order::kX] / u_norm;
y_minus_x[1] = theta * ambient_y_minus_x[Order::kY] / u_norm;
y_minus_x[2] = theta * ambient_y_minus_x[Order::kZ] / u_norm;
@@ -127,6 +130,7 @@
jacobian(2, Order::kX) = -x[Order::kY];
jacobian(2, Order::kY) = x[Order::kX];
jacobian(2, Order::kZ) = x[Order::kW];
+ jacobian *= 2;
}
} // namespace
diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc
index 019d648..1341120 100644
--- a/internal/ceres/manifold_test.cc
+++ b/internal/ceres/manifold_test.cc
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
-// Copyright 2023 Google Inc. All rights reserved.
+// Copyright 2025 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -497,14 +497,14 @@
EXPECT_EQ(manifold1.TangentSize(), manifold2.TangentSize());
}
-TEST(QuaternionManifold, PlusPiBy2) {
+TEST(QuaternionManifold, PlusPi) {
QuaternionManifold manifold;
Vector x = Vector::Zero(4);
x[0] = 1.0;
for (int i = 0; i < 3; ++i) {
Vector delta = Vector::Zero(3);
- delta[i] = constants::pi / 2;
+ delta[i] = constants::pi;
Vector x_plus_delta = Vector::Zero(4);
EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), x_plus_delta.data()));
@@ -534,12 +534,8 @@
// Compute the expected value of QuaternionManifold::Plus via functions in
// rotation.h and compares it to the one computed by QuaternionManifold::Plus.
MATCHER_P2(QuaternionManifoldPlusIsCorrectAt, x, delta, "") {
- // This multiplication by 2 is needed because AngleAxisToQuaternion uses
- // |delta|/2 as the angle of rotation where as in the implementation of
- // QuaternionManifold for historical reasons we use |delta|.
- const Vector two_delta = delta * 2;
Vector delta_q(4);
- AngleAxisToQuaternion(two_delta.data(), delta_q.data());
+ AngleAxisToQuaternion(delta.data(), delta_q.data());
Vector expected(4);
QuaternionProduct(delta_q.data(), x.data(), expected.data());
@@ -607,12 +603,8 @@
// Compute the expected value of EigenQuaternionManifold::Plus using Eigen and
// compares it to the one computed by QuaternionManifold::Plus.
MATCHER_P2(EigenQuaternionManifoldPlusIsCorrectAt, x, delta, "") {
- // This multiplication by 2 is needed because AngleAxisToQuaternion uses
- // |delta|/2 as the angle of rotation where as in the implementation of
- // Quaternion for historical reasons we use |delta|.
- const Vector two_delta = delta * 2;
Vector delta_q(4);
- AngleAxisToQuaternion(two_delta.data(), delta_q.data());
+ AngleAxisToQuaternion(delta.data(), delta_q.data());
Eigen::Quaterniond delta_eigen_q(
delta_q[0], delta_q[1], delta_q[2], delta_q[3]);
