SOT(3) Implementation - eqVIO 1/3

gtsam/geometry/SOT3.cpp

@@ -0,0 +1,126 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    SOT3.cpp
+ * @brief   The scaled orthogonal transforms SOT(3) = SO(3) x R>0
+ * @author  Rohan Bansal
+ */
+
+#include <gtsam/geometry/SOT3.h>
+
+#include <cmath>
+#include <iostream>
+
+namespace gtsam {
+
+void SOT3::print(const std::string& s) const {
+  if (!s.empty()) std::cout << s << "\n";
+  std::cout << "SOT3:\n";
+  R_.print("  R: ");
+  std::cout << "  c: " << c_ << "\n";
+}
+
+bool SOT3::equals(const SOT3& other, double tol) const {
+  return R_.equals(other.R_, tol) && std::abs(c_ - other.c_) < tol;
+}
+
+// MatrixLieGroup interface
+
+SOT3::MatrixNN SOT3::matrix() const {
+  MatrixNN M = MatrixNN::Zero();
+  M.topLeftCorner<3, 3>() = R_.matrix();
+  M(3, 3) = c_;
+  return M;
+}
+
+SOT3::MatrixNN SOT3::Hat(const TangentVector& xi) {
+  // (Omega, s) -> [[Omega^x, 0], [0, s]]
+  MatrixNN X = MatrixNN::Zero();
+  X.topLeftCorner<3, 3>() = SO3::Hat(Vector3(xi.head<3>()));
+  X(3, 3) = xi(3);
+  return X;
+}
+
+SOT3::TangentVector SOT3::Vee(const MatrixNN& X) {
+  // [[Omega^x, 0], [0, s]] -> (Omega, s)
+  TangentVector xi;
+  xi.head<3>() = SO3::Vee(Matrix3(X.topLeftCorner<3, 3>()));
+  xi(3) = X(3, 3);
+  return xi;
+}
+
+//******************************************************************************
+// Lie Group
+
+SOT3 SOT3::Expmap(const TangentVector& xi, ChartJacobian H) {
+  // SOT(3) = SO(3) x R>0 is direct product, so:
+  // exp(Omega, s) = (SO3::Expmap(Omega), exp(s))
+  const Vector3 Omega = xi.head<3>();
+  const double s = xi(3);
+  const double c = std::exp(s);
+
+  if (H) {
+    // block-diagonal Jacobian where top-left 3x3: right Jacobian of 
+    // SO3::Expmap at Omega, and bottom-right: identity, since 
+    // Local(exp(s), exp(s + ds)) = ds
+    Matrix3 H_R;
+    const SO3 R = SO3::Expmap(Omega, H ? &H_R : nullptr);
+    H->setZero();
+    H->topLeftCorner<3, 3>() = H_R;
+    (*H)(3, 3) = 1.0;
+    return SOT3(R, c);
+  }
+
+  return SOT3(SO3::Expmap(Omega), c);
+}
+
+SOT3::TangentVector SOT3::Logmap(const SOT3& Q, ChartJacobian H) {
+  // log(R, c) = (SO3::Logmap(R), log(c))
+  const double s = std::log(Q.c_);
+
+  if (H) {
+    // block-diagonal Jacobian where top-left 3x3: derivative of 
+    // SO3::Logmap at R, and bottom-right: identity, since 
+    // Local(exp(s), exp(s + ds)) = ds
+    Matrix3 H_R;
+    const Vector3 Omega = SO3::Logmap(Q.R_, H ? &H_R : nullptr);
+    H->setZero();
+    H->topLeftCorner<3, 3>() = H_R;
+    (*H)(3, 3) = 1.0;
+    TangentVector xi;
+    xi.head<3>() = Omega;
+    xi(3) = s;
+    return xi;
+  }
+
+  TangentVector xi;
+  xi.head<3>() = SO3::Logmap(Q.R_);
+  xi(3) = s;
+  return xi;
+}
+
+SOT3::Jacobian SOT3::AdjointMap() const {
+  Jacobian Ad = Jacobian::Identity();
+  Ad.topLeftCorner<3, 3>() = R_.matrix();
+  return Ad;
+}
+
+SOT3 SOT3::ChartAtOrigin::Retract(const TangentVector& xi, ChartJacobian H) {
+  return SOT3::Expmap(xi, H);
+}
+
+SOT3::TangentVector SOT3::ChartAtOrigin::Local(const SOT3& Q,
+                                                ChartJacobian H) {
+  return SOT3::Logmap(Q, H);
+}
+
+}  // namespace gtsam

gtsam/geometry/SOT3.h

@@ -0,0 +1,153 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    SOT3.h
+ * @brief   The scaled orthogonal transforms SOT(3) = SO(3) x R>0
+ *
+ * This file implements the SOT(3) group as defined by eqVIO (van Goor et al., https://arxiv.org/pdf/2205.01980)
+ *
+ * @author  Rohan Bansal
+ * @date    2026
+ */
+
+#pragma once
+
+#include <gtsam/base/MatrixLieGroup.h>
+#include <gtsam/dllexport.h>
+#include <gtsam/geometry/SO3.h>
+
+#include <string>
+
+namespace gtsam {
+
+/**
+ * @class SOT3
+ * @brief Scaled orthogonal transforms: SOT(3) = SO(3) x R>0.
+ *
+ * Represents group elements Q = (R_Q, c_Q) with R_Q in SO(3) and c_Q > 0.
+ * The 4x4 matrix representation is block-diagonal: [[R, 0], [0, c]].
+ * The Lie algebra has dimension 4: tangent vector xi = (Omega, s) in R^4.
+ */
+class GTSAM_EXPORT SOT3 : public MatrixLieGroup<SOT3, 4, 4> {
+ public:
+  /// @name Type definitions
+  /// @{
+
+  static constexpr int dimension = 4;
+  using MatrixNN = Eigen::Matrix4d;
+  using LieAlgebra = MatrixNN;
+  using TangentVector = Eigen::Matrix<double, 4, 1>;
+  using ChartJacobian = OptionalJacobian<4, 4>;
+  using Jacobian = Eigen::Matrix<double, 4, 4>;
+
+  /// @}
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor: identity element (R = I_3, c = 1).
+  SOT3() : R_(), c_(1.0) {}
+
+  /// Construct from rotation and positive scale.
+  SOT3(const SO3& R, double c) : R_(R), c_(c) {}
+
+  /// Construct 4x4 [[R, 0], [0, c]].
+  explicit SOT3(const MatrixNN& M)
+      : R_(SO3(M.topLeftCorner<3, 3>())), c_(M(3, 3)) {}
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  void print(const std::string& s = "") const;
+  bool equals(const SOT3& other, double tol = 1e-9) const;
+
+  /// @}
+  /// @name Group
+  /// @{
+
+  /// Identity element.
+  static SOT3 Identity() { return SOT3(); }
+
+  /// Inverse element
+  SOT3 inverse() const { return SOT3(R_.inverse(), 1.0 / c_); }
+
+  /// Group multiplication: (R1,c1)*(R2,c2) = (R1*R2, c1*c2).
+  SOT3 operator*(const SOT3& other) const {
+    return SOT3(R_ * other.R_, c_ * other.c_);
+  }
+
+  /// @}
+  /// @name MatrixLieGroup interface
+  /// @{
+
+  /// Return 4x4 [[R, 0], [0, c]].
+  MatrixNN matrix() const;
+
+  /// Hat: (Omega, s) -> [[Omega^x, 0], [0, s]].
+  static MatrixNN Hat(const TangentVector& xi);
+
+  /// Vee: inverse of Hat.
+  static TangentVector Vee(const MatrixNN& X);
+
+  /// Exponential map at identity.
+  static SOT3 Expmap(const TangentVector& xi, ChartJacobian H = {});
+
+  /// Logarithmic map at identity.
+  static TangentVector Logmap(const SOT3& Q, ChartJacobian H = {});
+
+  /// Adjoint map Ad_Q: block-diagonal [[R, 0], [0, 1]].
+  Jacobian AdjointMap() const;
+
+  // Chart at origin uses Expmap/Logmap directly.
+  struct ChartAtOrigin {
+    static SOT3 Retract(const TangentVector& xi, ChartJacobian H = {});
+    static TangentVector Local(const SOT3& Q, ChartJacobian H = {});
+  };
+
+  using LieGroup<SOT3, 4>::inverse;
+
+  /// @}
+  /// @name Group action on R^3
+  /// @{
+
+  /// Apply Q to point p in R^3: Qp = c * R * p.
+  Vector3 operator*(const Vector3& p) const { return c_ * (R_.matrix() * p); }
+
+  /// Apply inverse of Q to point p: Q^{-1}p = (1/c) * R^T * p.
+  Vector3 applyInverse(const Vector3& p) const {
+    return (1.0 / c_) * (R_.matrix().transpose() * p);
+  }
+
+  /// @}
+  /// @name Accessors
+  /// @{
+
+  /// Return R_Q, the SO(3) rotation component.
+  const SO3& rotation() const { return R_; }
+
+  /// Return c_Q, the positive scale component.
+  double scalar() const { return c_; }
+
+  /// @}
+
+ private:
+  SO3 R_;       ///< Rotation component R_Q in SO(3).
+  double c_;    ///< Scale component c_Q > 0.
+};
+
+template <>
+struct traits<SOT3> : public internal::MatrixLieGroup<SOT3, 4> {};
+
+template <>
+struct traits<const SOT3> : public internal::MatrixLieGroup<SOT3, 4> {};
+
+}  // namespace gtsam