fix(geometry): validate rotation matrix in Rot3 constructor

gtsam/geometry/Rot3.cpp

@@ -30,6 +30,16 @@ using namespace std;
 
 namespace gtsam {
 
+/* ************************************************************************* */
+bool Rot3::IsValid(const Matrix3& R, double tol) {
+  // Check orthogonality: R^T * R should be identity
+  const Matrix3 RtR = R.transpose() * R;
+  if (!RtR.isApprox(Matrix3::Identity(), tol)) return false;
+  // Check determinant is +1 (not a reflection)
+  if (std::abs(R.determinant() - 1.0) > tol) return false;
+  return true;
+}
+
 /* ************************************************************************* */
 void Rot3::print(const std::string& s) const {
   cout << (s.empty() ? "R: " : s + " ");

gtsam/geometry/Rot3.h

@@ -29,6 +29,8 @@
 #include <gtsam/base/concepts.h>
 #include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
 
+#include <stdexcept>
+
 #include <random>
 
 // You can override the default coordinate mode using this flag
@@ -97,10 +99,13 @@ class GTSAM_EXPORT Rot3 : public MatrixLieGroup<Rot3, 3, 3> {
     template <typename Derived>
 #ifdef GTSAM_USE_QUATERNIONS
     explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
-      quaternion_ = Matrix3(R);
+      const Matrix3 M(R);
+      throwIfInvalid(M);
+      quaternion_ = M;
     }
 #else
     explicit Rot3(const Eigen::MatrixBase<Derived>& R) : rot_(R) {
+      throwIfInvalid(rot_.matrix());
     }
 #endif
 
@@ -109,9 +114,14 @@ class GTSAM_EXPORT Rot3 : public MatrixLieGroup<Rot3, 3, 3> {
      * Overload version for Matrix3 to avoid casting in quaternion mode.
      */
 #ifdef GTSAM_USE_QUATERNIONS
-    explicit Rot3(const Matrix3& R) : quaternion_(R) {}
+    explicit Rot3(const Matrix3& R) : quaternion_(Quaternion::Identity()) {
+      throwIfInvalid(R);
+      quaternion_ = R;
+    }
 #else
-    explicit Rot3(const Matrix3& R) : rot_(R) {}
+    explicit Rot3(const Matrix3& R) : rot_(R) {
+      throwIfInvalid(R);
+    }
 #endif
 
     /**
@@ -265,11 +275,19 @@ class GTSAM_EXPORT Rot3 : public MatrixLieGroup<Rot3, 3, 3> {
     static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p,  //
                               const Unit3& a_q, const Unit3& b_q);
 
+    /**
+     * Check if a 3x3 matrix is a valid rotation matrix (orthogonal with det +1).
+     * @param R The matrix to check
+     * @param tol Tolerance for the checks (default 1e-9)
+     * @return true if the matrix is a valid rotation matrix
+     */
+    static bool IsValid(const Matrix3& R, double tol = 1e-9);
+
     /**
      * Static, named constructor that finds Rot3 element closest to M in Frobenius norm.
-     * 
+     *
      * Uses Full SVD to compute the orthogonal matrix, thus is highly accurate and robust.
-     * 
+     *
      * N. J. Higham. Matrix nearness problems and applications.
      * In M. J. C. Gover and S. Barnett, editors, Applications of Matrix Theory, pages 1–27.
      * Oxford University Press, 1989.
@@ -547,6 +565,17 @@ class GTSAM_EXPORT Rot3 : public MatrixLieGroup<Rot3, 3, 3> {
     /// @}
 
    private:
+    /// Throw std::invalid_argument if R is not a valid rotation matrix.
+    /// Uses a tolerance of 1e-6 to accommodate numerically computed matrices
+    /// (e.g., from matrix exponential) while still catching clearly invalid input.
+    static void throwIfInvalid(const Matrix3& R) {
+      constexpr double kConstructorTol = 1e-6;
+      if (!IsValid(R, kConstructorTol))
+        throw std::invalid_argument(
+            "Rot3 constructor: input matrix is not a valid rotation matrix "
+            "(must be orthogonal with determinant +1).");
+    }
+
 #if GTSAM_ENABLE_BOOST_SERIALIZATION
     /** Serialization function */
     friend class boost::serialization::access;