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]);