saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-29 08:34:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Restore quaternion group action

Browse Source 
Commit 370631f01f rescaled the quaternion
manifold's group action so the tangent vector norm matches the full
rotation angle, addressing issue #941. While both conventions are
mathematically valid, issue #311 already concluded that the half-angle
one should be kept because too much code depends on it. The rescaling
effectively broke that expectation for existing callers.

Restore the pre-370631f01f71db36b580e5a65790b2a8d9559821 scaling, keep
the rest of that commit's cleanup, and update the documentation and
tests to match.

Fixes #1190

Change-Id: I1dd189c51153ed62cc8c871472aacb87673ec351
This commit is contained in:
Sergiu Deitsch
2026-05-01 13:36:43 +02:00
parent 76b431ef04
commit 06de7240c4
4 changed files with 86 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/source/nnls_modeling.rst
+24 -22
View File
@@ -1769,25 +1769,26 @@ coordinate.
struct QuaternionFunctor {
template <typename T>
bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
const T norm_delta = hypot(delta[0], delta[1], delta[2]);
const T squared_norm_delta =
delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
T q_delta[4];
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];
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];
} 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] / T(2);
q_delta[2] = delta[1] / T(2);
q_delta[3] = delta[2] / T(2);
q_delta[1] = delta[0];
q_delta[2] = delta[1];
q_delta[3] = delta[2];
}
QuaternionProduct(q_delta, x, x_plus_delta);
@@ -1799,19 +1800,20 @@ coordinate.
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 = hypot(ambient_y_minus_x[1], ambient_y_minus_x[2],
ambient_y_minus_x[3]);
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 = 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;
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;
} 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] = 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];
// 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];
}
return true;
}
include/ceres/autodiff_manifold.h
+21 -18
View File
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2025 Google Inc. All rights reserved.
// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -89,25 +89,26 @@ namespace ceres {
// struct QuaternionFunctor {
// template <typename T>
// bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
// const T norm_delta = hypot(delta[0], delta[1], delta[2]);
// const T squared_norm_delta =
// delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
//
// T q_delta[4];
// 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];
// 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];
// } 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] / T(2);
// q_delta[2] = delta[1] / T(2);
// q_delta[3] = delta[2] / T(2);
// q_delta[1] = delta[0];
// q_delta[2] = delta[1];
// q_delta[3] = delta[2];
// }
//
// QuaternionProduct(q_delta, x, x_plus_delta);
@@ -119,18 +120,20 @@ namespace ceres {
// 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 = 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]);
// 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]);
// 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] = 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];
// 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];
// }
// return true;
// }
internal/ceres/manifold.cc
+28 -4
View File
@@ -16,8 +16,22 @@ inline void QuaternionPlusImpl(const double* x,
double* x_plus_delta) {
// x_plus_delta = QuaternionProduct(q_delta, x), where q_delta is the
// quaternion constructed from delta.
const double norm_delta = std::hypot(delta[0], delta[1], delta[2]);
if (std::fpclassify(norm_delta) == FP_ZERO) {
// No change in rotation: return the quaternion as is.
std::copy_n(x, 4, x_plus_delta);
return;
}
const double sin_delta_by_delta = std::sin(norm_delta) / norm_delta;
double q_delta[4];
AngleAxisToQuaternion<Order>(delta, q_delta);
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];
QuaternionProduct<Order>(q_delta, x, x_plus_delta);
}
@@ -38,7 +52,6 @@ inline void QuaternionPlusJacobianImpl(const double* x, double* jacobian_ptr) {
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>
@@ -52,7 +65,19 @@ inline void QuaternionMinusImpl(const double* y,
double ambient_y_minus_x[4];
QuaternionProduct<Order>(y, x_conj, ambient_y_minus_x);
QuaternionToAngleAxis<Order>(ambient_y_minus_x, y_minus_x);
const double u_norm = std::hypot(ambient_y_minus_x[Order::kX],
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]);
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;
} else {
std::fill_n(y_minus_x, 3, 0.0);
}
}
template <typename Order>
@@ -72,7 +97,6 @@ inline void QuaternionMinusJacobianImpl(const double* x, double* jacobian_ptr) {
jacobian(2, Order::kX) = -x[Order::kY];
jacobian(2, Order::kY) = x[Order::kX];
jacobian(2, Order::kZ) = x[Order::kW];
jacobian *= 2;
}
} // namespace
internal/ceres/manifold_test.cc
+13 -5
View File
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2025 Google Inc. All rights reserved.
// Copyright 2023 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -497,14 +497,14 @@ TEST(ProductManifold, Pointers) {
EXPECT_EQ(manifold1.TangentSize(), manifold2.TangentSize());
}
TEST(QuaternionManifold, PlusPi) {
TEST(QuaternionManifold, PlusPiBy2) {
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;
delta[i] = constants::pi / 2;
Vector x_plus_delta = Vector::Zero(4);
EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), x_plus_delta.data()));
@@ -534,8 +534,12 @@ TEST(QuaternionManifold, PlusPi) {
// 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(delta.data(), delta_q.data());
AngleAxisToQuaternion(two_delta.data(), delta_q.data());
Vector expected(4);
QuaternionProduct(delta_q.data(), x.data(), expected.data());
@@ -603,8 +607,12 @@ TEST(QuaternionManifold, DeltaJustBelowPi) {
// 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(delta.data(), delta_q.data());
AngleAxisToQuaternion(two_delta.data(), delta_q.data());
Eigen::Quaterniond delta_eigen_q(
delta_q[0], delta_q[1], delta_q[2], delta_q[3]);