More FrobeniusFactors

gtsam/base/Lie.h

@@ -229,7 +229,7 @@ struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> {
 /// Both LieGroupTraits and Testable
 template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
 
-/// Adds LieAlgebra, Hat, and Vie to LieGroupTraits
+/// Adds LieAlgebra, Hat, and Vee to LieGroupTraits
 template<class Class> struct MatrixLieGroupTraits: LieGroupTraits<Class> {
   using LieAlgebra = typename Class::LieAlgebra;
   using TangentVector = typename LieGroupTraits<Class>::TangentVector;
@@ -351,7 +351,7 @@ template <class T> Matrix wedge(const Vector& x);
  */
 template <class T>
 T expm(const Vector& x, int K = 7) {
-  auto xhat = T::Hat(x);
+  const Matrix xhat = T::Hat(x);
   return T(expm(xhat, K));
 }
 

gtsam/geometry/Pose2.cpp

@@ -323,6 +323,32 @@ double Pose2::range(const Pose2& pose,
   return r;
 }
 
+/* ************************************************************************* */
+// Compute vectorized Lie algebra generators for SE(2)
+static Matrix93 VectorizedGenerators() {
+  Matrix93 G;
+  for (size_t j = 0; j < 3; j++) {
+    const Matrix3 X = Pose2::Hat(Vector::Unit(3, j));
+    G.col(j) = Eigen::Map<const Vector9>(X.data());
+  }
+  return G;
+}
+
+Vector9 Pose2::vec(OptionalJacobian<9, 3> H) const {
+  // Vectorize
+  const Matrix3 M = matrix();
+  const Vector9 X = Eigen::Map<const Vector9>(M.data());
+
+  // If requested, calculate H as (I_3 \oplus M) * G.
+  if (H) {
+    static const Matrix93 G = VectorizedGenerators(); // static to compute only once
+    for (size_t i = 0; i < 3; i++)
+      H->block(i * 3, 0, 3, dimension) = M * G.block(i * 3, 0, 3, dimension);
+  }
+
+  return X;
+}
+
 /* *************************************************************************
  * Align finds the angle using a linear method:
  * a = Pose2::transformFrom(b) = t + R*b

gtsam/geometry/Pose2.h

@@ -328,6 +328,9 @@ class GTSAM_EXPORT Pose2: public LieGroup<Pose2, 3> {
    */
   static std::pair<size_t, size_t> rotationInterval() { return {2, 2}; }
 
+  /// Return vectorized SE(2) matrix in column order.
+  Vector9 vec(OptionalJacobian<9, 3> H = {}) const;
+
   /// Output stream operator
   GTSAM_EXPORT
   friend std::ostream &operator<<(std::ostream &os, const Pose2& p);

gtsam/geometry/Pose3.cpp

@@ -534,6 +534,34 @@ Pose3 Pose3::slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, Opti
   return interpolate(*this, other, t, Hx, Hy);
 }
 
+/* ************************************************************************* */
+// Compute vectorized Lie algebra generators for SE(3)
+using Matrix16x6 = Eigen::Matrix<double, 16, 6>;
+using Vector16 = Eigen::Matrix<double, 16, 1>;
+static Matrix16x6 VectorizedGenerators() {
+  Matrix16x6 G;
+  for (size_t j = 0; j < 6; j++) {
+    const Matrix4 X = Pose3::Hat(Vector::Unit(6, j));
+    G.col(j) = Eigen::Map<const Vector16>(X.data());
+  }
+  return G;
+}
+
+Vector Pose3::vec(OptionalJacobian<16, 6> H) const {
+  // Vectorize
+  const Matrix4 M = matrix();
+  const Vector X = Eigen::Map<const Vector16>(M.data());
+
+  // If requested, calculate H as (I_4 \oplus M) * G.
+  if (H) {
+    static const Matrix16x6 G = VectorizedGenerators(); // static to compute only once
+    for (size_t i = 0; i < 4; i++)
+      H->block(i * 4, 0, 4, dimension) = M * G.block(i * 4, 0, 4, dimension);
+  }
+
+  return X;
+}
+
 /* ************************************************************************* */
 std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
   // Both Rot3 and Point3 have ostream definitions so we use them.

gtsam/geometry/Pose3.h

@@ -392,6 +392,9 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
   Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = {},
                                              OptionalJacobian<6, 6> Hy = {}) const;
 
+  /// Return vectorized SE(3) matrix in column order.
+  Vector vec(OptionalJacobian<16, 6> H = {}) const;
+
   /// Output stream operator
   GTSAM_EXPORT
   friend std::ostream &operator<<(std::ostream &os, const Pose3& p);

gtsam/geometry/Rot2.cpp

@@ -157,6 +157,22 @@ Rot2 Rot2::ClosestTo(const Matrix2& M) {
   return Rot2::fromCosSin(c, s);
 }
 
+/* ************************************************************************* */
+Vector4 Rot2::vec(OptionalJacobian<4, 1> H) const {
+  // Vectorize
+  const Matrix2 M = matrix();
+  const Vector4 X = Eigen::Map<const Vector4>(M.data());
+
+  // If requested, calculate H as (I_3 \oplus M) * G.
+  if (H) {
+    static const Matrix41 G = (Matrix41() << 0, 1, -1, 0).finished();
+    for (size_t i = 0; i < 2; i++)
+      H->block(i * 2, 0, 2, dimension) = M * G.block(i * 2, 0, 2, dimension);
+  }
+
+  return X;
+}
+
 /* ************************************************************************* */
 
 } // gtsam

gtsam/geometry/Rot2.h

@@ -223,6 +223,9 @@ namespace gtsam {
     /** Find closest valid rotation matrix, given a 2x2 matrix */
     static Rot2 ClosestTo(const Matrix2& M);
 
+    /// Return vectorized SO(2) matrix in column order.
+    Vector4 vec(OptionalJacobian<4, 1> H = {}) const;
+
     /// @}
 
     private:

gtsam/geometry/Rot3.cpp

@@ -146,6 +146,7 @@ Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
 }
 
 /* ************************************************************************* */
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
 Point3 Rot3::column(int index) const{
   if(index == 3)
     return r3();
@@ -156,6 +157,7 @@ Point3 Rot3::column(int index) const{
   else
     throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
 }
+#endif
 
 /* ************************************************************************* */
 Vector3 Rot3::xyz(OptionalJacobian<3, 3> H) const {

gtsam/geometry/Rot3.h

@@ -459,9 +459,6 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
      */
     Matrix3 transpose() const;
 
-    /// @deprecated, this is base 1, and was just confusing
-    Point3 column(int index) const;
-
     Point3 r1() const; ///< first column
     Point3 r2() const; ///< second column
     Point3 r3() const; ///< third column
@@ -530,14 +527,26 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
     /**
      * @brief Spherical Linear intERPolation between *this and other
      * @param t a value between 0 and 1
-     * @param other final point of iterpolation geodesic on manifold
+     * @param other final point of interpolation geodesic on manifold
      */
     Rot3 slerp(double t, const Rot3& other) const;
 
+    /// Vee maps from Lie algebra to tangent vector
+    inline Vector9 vec(OptionalJacobian<9, 3> H = {}) const { return SO3(matrix()).vec(H); }
+
     /// Output stream operator
     GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
 
     /// @}
+    /// @name deprecated
+    /// @{
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
+    /// @deprecated, this is base 1, and was just confusing
+    Point3 column(int index) const;
+#endif
+
+    /// @}
 
    private:
 #if GTSAM_ENABLE_BOOST_SERIALIZATION

gtsam/geometry/tests/testPose2.cpp

@@ -958,6 +958,23 @@ TEST(Pose2, Print) {
   EXPECT(assert_print_equal(expected2, pose, s));
 }
 
+/* ************************************************************************* */
+TEST(Pose2, vec) {
+  // Test a simple pose
+  Pose2 pose(Rot2::fromAngle(M_PI / 4), Point2(1, 2));
+
+  // Test the 'vec' method
+  Vector9 expected_vec = Eigen::Map<Vector9>(pose.matrix().data());
+  Matrix93 actualH;
+  Vector9 actual_vec = pose.vec(actualH);
+  EXPECT(assert_equal(expected_vec, actual_vec));
+
+  // Verify Jacobian with numerical derivatives
+  std::function<Vector9(const Pose2&)> f = [](const Pose2& p) { return p.vec(); };
+  Matrix93 numericalH = numericalDerivative11<Vector9, Pose2>(f, pose);
+  EXPECT(assert_equal(numericalH, actualH, 1e-9));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/tests/testPose3.cpp

@@ -1401,6 +1401,21 @@ TEST(Pose3, ExpmapChainRule) {
   EXPECT(assert_equal<Matrix6>(expected2, analytic, 1e-5)); // note tolerance
 }
 
+/* ************************************************************************* */
+TEST(Pose3, vec) {
+  // Test the 'vec' method
+  using Vector16 = Eigen::Matrix<double, 16, 1>;
+  Vector16 expected_vec = Eigen::Map<Vector16>(T.matrix().data());
+  Matrix actualH;
+  Vector16 actual_vec = T.vec(actualH);
+  EXPECT(assert_equal(expected_vec, actual_vec));
+
+  // Verify Jacobian with numerical derivatives
+  std::function<Vector16(const Pose3&)> f = [](const Pose3& p) { return p.vec(); };
+  Matrix numericalH = numericalDerivative11<Vector16, Pose3>(f, T);
+  EXPECT(assert_equal(numericalH, actualH, 1e-9));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/geometry/tests/testRot2.cpp

@@ -174,6 +174,20 @@ TEST( Rot2, relativeBearing )
   CHECK(assert_equal(expectedH,actualH));
 }
 
+/* ************************************************************************* */
+TEST(Rot2, vec) {
+  // Test the 'vec' method
+  Vector4 expected_vec = Eigen::Map<Vector4>(R.matrix().data());
+  Matrix41 actualH;
+  Vector4 actual_vec = R.vec(actualH);
+  EXPECT(assert_equal(expected_vec, actual_vec));
+
+  // Verify Jacobian with numerical derivatives
+  std::function<Vector4(const Rot2&)> f = [](const Rot2& p) { return p.vec(); };
+  Matrix41 numericalH = numericalDerivative11<Vector4, Rot2>(f, R);
+  EXPECT(assert_equal(numericalH, actualH, 1e-9));
+}
+
 //******************************************************************************
 namespace {
 Rot2 id;

gtsam/geometry/tests/testUnit3.cpp

@@ -67,7 +67,7 @@ static Unit3 rotate_(const Rot3& R, const Unit3& p) {
 TEST(Unit3, rotate) {
   Rot3 R = Rot3::Yaw(0.5);
   Unit3 p(1, 0, 0);
-  Unit3 expected = Unit3(R.column(1));
+  Unit3 expected = Unit3(R.matrix().col(0));
   Unit3 actual = R * p;
   EXPECT(assert_equal(expected, actual, 1e-5));
   Matrix actualH, expectedH;