Fixed discrepancy in QuaternionRotatePoint for different orders

Fixes #1178

Change-Id: I0430ffb384ea53a863ba72f076043f0f772db4e1

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 @@ struct EigenQuaternionOrder {
   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 @@ inline void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
                   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>

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 @@ MATCHER_P(IsNear3x3Matrix, expected, "") {
   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 @@ TYPED_TEST(QuaternionTest, RotatePointGivesSameAnswerAsRotationByMatrixCanned) {
   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.