Fixed discrepancy in QuaternionRotatePoint for different orders
Fixes #1178
Change-Id: I0430ffb384ea53a863ba72f076043f0f772db4e1
diff --git a/include/ceres/rotation.h b/include/ceres/rotation.h
index addd598..7c95172 100644
--- a/include/ceres/rotation.h
+++ b/include/ceres/rotation.h
@@ -46,7 +46,9 @@
#define CERES_PUBLIC_ROTATION_H_
#include <algorithm>
+#include <array>
#include <cmath>
+#include <type_traits>
#include "absl/log/check.h"
#include "ceres/constants.h"
@@ -95,6 +97,39 @@
static constexpr int kZ = 2;
};
+// Constructs a quaternion with the Ceres coefficient order (w, x, y, z).
+template <typename Order, typename T>
+constexpr auto MakeQuaternion(T w, T x, T y, T z)
+ -> std::enable_if_t<std::is_same_v<Order, CeresQuaternionOrder>,
+ std::array<T, 4>> {
+ return {w, x, y, z};
+}
+
+// Constructs a quaternion with the Ceres coefficient order (x, y, z, w).
+template <typename Order, typename T>
+constexpr auto MakeQuaternion(T w, T x, T y, T z)
+ -> std::enable_if_t<std::is_same_v<Order, EigenQuaternionOrder>,
+ std::array<T, 4>> {
+ return {x, y, z, w};
+}
+
+// Changes the quaternion coefficients order. This specialization is used
+// whenever the target and source coefficient order is the same.
+template <typename ToOrder, typename FromOrder, typename T>
+constexpr auto ConvertQuaternion(const std::array<T, 4>& q)
+ -> std::enable_if_t<std::is_same_v<FromOrder, ToOrder>, std::array<T, 4>> {
+ return q;
+}
+
+// Changes the quaternion coefficients order. This specialization is used
+// whenever the target and source coefficient order is different.
+template <typename ToOrder, typename FromOrder, typename T>
+constexpr auto ConvertQuaternion(const std::array<T, 4>& q)
+ -> std::enable_if_t<!std::is_same_v<FromOrder, ToOrder>, std::array<T, 4>> {
+ return MakeQuaternion<ToOrder>(
+ q[FromOrder::kW], q[FromOrder::kX], q[FromOrder::kY], q[FromOrder::kZ]);
+}
+
// Convert a value in combined axis-angle representation to a quaternion.
// The value angle_axis is a triple whose norm is an angle in radians,
// and whose direction is aligned with the axis of rotation,
@@ -780,14 +815,12 @@
q[Order::kY] * q[Order::kY] + q[Order::kZ] * q[Order::kZ]);
// Make unit-norm version of q.
- const T unit[4] = {
- scale * q[Order::kW],
- scale * q[Order::kX],
- scale * q[Order::kY],
- scale * q[Order::kZ],
- };
+ const std::array<T, 4> unit = MakeQuaternion<Order>(scale * q[Order::kW],
+ scale * q[Order::kX],
+ scale * q[Order::kY],
+ scale * q[Order::kZ]);
- UnitQuaternionRotatePoint<Order>(unit, pt, result);
+ UnitQuaternionRotatePoint<Order>(unit.data(), pt, result);
}
template <typename Order, typename T>
diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc
index 28b34ef..2328114 100644
--- a/internal/ceres/rotation_test.cc
+++ b/internal/ceres/rotation_test.cc
@@ -36,7 +36,6 @@
#include <limits>
#include <random>
#include <string>
-#include <type_traits>
#include <utility>
#include "absl/log/log.h"
@@ -202,20 +201,6 @@
return true;
}
-template <typename Order, typename T>
-constexpr auto MakeQuaternion(T w, T x, T y, T z)
- -> std::enable_if_t<std::is_same_v<Order, CeresQuaternionOrder>,
- std::array<T, 4>> {
- return {w, x, y, z};
-}
-
-template <typename Order, typename T>
-constexpr auto MakeQuaternion(T w, T x, T y, T z)
- -> std::enable_if_t<std::is_same_v<Order, EigenQuaternionOrder>,
- std::array<T, 4>> {
- return {x, y, z, w};
-}
-
template <typename T>
class QuaternionTest : public testing::Test {};
@@ -1696,6 +1681,56 @@
ExpectArraysClose(9, R[0], Rq[0], kTolerance);
}
+TEST(Quaternion, RotatePointGivesSameAnswerForDifferentQuaternionOrders) {
+ // Rotation defined by a unit quaternion.
+ const std::array<double, 4> q1 =
+ MakeQuaternion<CeresQuaternionOrder>(+0.2318160216097109,
+ -0.0178430356832060,
+ +0.9044300776717159,
+ -0.3576998641394597);
+ const std::array<double, 4> q2 =
+ ConvertQuaternion<EigenQuaternionOrder, CeresQuaternionOrder>(q1);
+
+ constexpr double p[3] = {
+ +0.11,
+ -13.15,
+ 1.17,
+ };
+
+ double result1[3];
+ QuaternionRotatePoint<CeresQuaternionOrder>(q1.data(), p, result1);
+
+ double result2[3];
+ QuaternionRotatePoint<EigenQuaternionOrder>(q2.data(), p, result2);
+
+ ExpectArraysClose(3, result1, result2, kTolerance);
+}
+
+TEST(Quaternion, RotatePointGivesSameAnswerForDifferentUnitQuaternionOrders) {
+ // Rotation defined by a unit quaternion.
+ const std::array<double, 4> q1 =
+ MakeQuaternion<CeresQuaternionOrder>(+0.2318160216097109,
+ -0.0178430356832060,
+ +0.9044300776717159,
+ -0.3576998641394597);
+ const std::array<double, 4> q2 =
+ ConvertQuaternion<EigenQuaternionOrder, CeresQuaternionOrder>(q1);
+
+ constexpr double p[3] = {
+ +0.11,
+ -13.15,
+ 1.17,
+ };
+
+ double result1[3];
+ UnitQuaternionRotatePoint<CeresQuaternionOrder>(q1.data(), p, result1);
+
+ double result2[3];
+ UnitQuaternionRotatePoint<EigenQuaternionOrder>(q2.data(), p, result2);
+
+ ExpectArraysClose(3, result1, result2, kTolerance);
+}
+
TYPED_TEST(QuaternionTest, RotatePointGivesSameAnswerAsRotationByMatrix) {
using Order = TypeParam;
// Rotation defined by a unit quaternion.
