Gal3.cpp

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 * 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
 * -------------------------------------------------------------------------- */
 
#include <gtsam/geometry/Gal3.h>
#include <gtsam/geometry/SO3.h>
#include <gtsam/geometry/Event.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/geometry/concepts.h>
 
#include <iostream>
#include <cmath>
#include <functional>
 
namespace gtsam {
 
//------------------------------------------------------------------------------
// Constants and Helper function for Expmap/Logmap
//------------------------------------------------------------------------------
namespace { // Anonymous namespace for internal linkage
  constexpr double kSmallAngleThreshold = 1e-10;
 
  // The type of the Lie algebra (matrix representation)
  using LieAlgebra = Matrix5;
 
  // Helper functions for accessing tangent vector components
  Eigen::Block<Vector10, 3, 1> rho(Vector10& v) { return v.block<3, 1>(0, 0); }
  Eigen::Block<Vector10, 3, 1> nu(Vector10& v) { return v.block<3, 1>(3, 0); }
  Eigen::Block<Vector10, 3, 1> theta(Vector10& v) { return v.block<3, 1>(6, 0); }
  Eigen::Block<Vector10, 1, 1> t_tan(Vector10& v) { return v.block<1, 1>(9, 0); }
  // Const versions
  Eigen::Block<const Vector10, 3, 1> rho(const Vector10& v) { return v.block<3, 1>(0, 0); }
  Eigen::Block<const Vector10, 3, 1> nu(const Vector10& v) { return v.block<3, 1>(3, 0); }
  Eigen::Block<const Vector10, 3, 1> theta(const Vector10& v) { return v.block<3, 1>(6, 0); }
  Eigen::Block<const Vector10, 1, 1> t_tan(const Vector10& v) { return v.block<1, 1>(9, 0); }
 
} // end anonymous namespace
 
//------------------------------------------------------------------------------
// Static Constructor/Create functions
//------------------------------------------------------------------------------
Gal3 Gal3::Create(const Rot3& R, const Point3& r, const Velocity3& v, double t,
                    OptionalJacobian<10, 3> H1, OptionalJacobian<10, 3> H2,
                    OptionalJacobian<10, 3> H3, OptionalJacobian<10, 1> H4) {
      if (H1) {
        H1->setZero();
        H1->block<3, 3>(6, 0) = Matrix3::Identity();
      }
      if (H2) {
        H2->setZero();
        H2->block<3, 3>(0, 0) = R.transpose();
      }
      if (H3) {
        H3->setZero();
        H3->block<3, 3>(3, 0) = R.transpose();
      }
      if (H4) {
        H4->setZero();
        Vector3 drho_dt = -R.transpose() * v;
        H4->block<3, 1>(0, 0) = drho_dt;
        (*H4)(9, 0) = 1.0;
      }
      return Gal3(R, r, v, t);
}
 
//------------------------------------------------------------------------------
Gal3 Gal3::FromPoseVelocityTime(const Pose3& pose, const Velocity3& v, double t,
                                OptionalJacobian<10, 6> H1, OptionalJacobian<10, 3> H2,
                                OptionalJacobian<10, 1> H3) {
    const Rot3& R = pose.rotation();
    const Point3& r = pose.translation();
    if (H1) {
        H1->setZero();
        H1->block<3, 3>(6, 0) = Matrix3::Identity();
        H1->block<3, 3>(0, 3) = Matrix3::Identity();
    }
    if (H2) {
        H2->setZero();
        H2->block<3, 3>(3, 0) = R.transpose();
    }
    if (H3) {
        H3->setZero();
        Vector3 drho_dt = -R.transpose() * v;
        H3->block<3, 1>(0, 0) = drho_dt;
        (*H3)(9, 0) = 1.0;
    }
    return Gal3(R, r, v, t);
}
 
//------------------------------------------------------------------------------
// Constructors
//------------------------------------------------------------------------------
Gal3::Gal3(const Matrix5& M) {
    // Constructor from 5x5 matrix representation (Equation 9, Page 5)
    if (std::abs(M(3, 3) - 1.0) > 1e-9 || std::abs(M(4, 4) - 1.0) > 1e-9 ||
        M.row(4).head(4).norm() > 1e-9 || M.row(3).head(3).norm() > 1e-9) {
        throw std::invalid_argument("Invalid Gal3 matrix structure: Check zero blocks and diagonal ones.");
    }
    R_ = Rot3(M.block<3, 3>(0, 0));
    v_ = M.block<3, 1>(0, 3);
    r_ = Point3(M.block<3, 1>(0, 4));
    t_ = M(3, 4);
}
 
//------------------------------------------------------------------------------
// Component Access
//------------------------------------------------------------------------------
const Rot3& Gal3::rotation(OptionalJacobian<3, 10> H) const {
    if (H) {
        H->setZero();
        H->block<3, 3>(0, 6) = Matrix3::Identity();
    }
    return R_;
}
 
//------------------------------------------------------------------------------
const Point3& Gal3::translation(OptionalJacobian<3, 10> H) const {
     if (H) {
        H->setZero();
        H->block<3,3>(0, 0) = R_.matrix();
        H->block<3,1>(0, 9) = v_;
    }
    return r_;
}
 
//------------------------------------------------------------------------------
const Velocity3& Gal3::velocity(OptionalJacobian<3, 10> H) const {
     if (H) {
        H->setZero();
        H->block<3, 3>(0, 3) = R_.matrix();
     }
    return v_;
}
 
//------------------------------------------------------------------------------
const double& Gal3::time(OptionalJacobian<1, 10> H) const {
    if (H) {
        H->setZero();
        (*H)(0, 9) = 1.0;
    }
    return t_;
}
 
//------------------------------------------------------------------------------
// Matrix Representation
//------------------------------------------------------------------------------
Matrix5 Gal3::matrix() const {
    // Returns 5x5 matrix representation as in Equation 9, Page 5
    Matrix5 M = Matrix5::Identity();
    M.block<3, 3>(0, 0) = R_.matrix();
    M.block<3, 1>(0, 3) = v_;
    M.block<3, 1>(0, 4) = Vector3(r_);
    M(3, 4) = t_;
    M.block<1,3>(3,0).setZero();
    M.block<1,4>(4,0).setZero();
    return M;
}
 
//------------------------------------------------------------------------------
// Stream operator
//------------------------------------------------------------------------------
std::ostream& operator<<(std::ostream& os, const Gal3& state) {
    os << "R: " << state.R_ << "\n";
    os << "r: " << state.r_.transpose() << "\n";
    os << "v: " << state.v_.transpose() << "\n";
    os << "t: " << state.t_;
    return os;
}
 
//------------------------------------------------------------------------------
// Testable Requirements
//------------------------------------------------------------------------------
void Gal3::print(const std::string& s) const {
    std::cout << (s.empty() ? "" : s + " ");
    std::cout << *this << std::endl;
}
 
//------------------------------------------------------------------------------
bool Gal3::equals(const Gal3& other, double tol) const {
    return R_.equals(other.R_, tol) &&
           traits<Point3>::Equals(r_, other.r_, tol) &&
           traits<Velocity3>::Equals(v_, other.v_, tol) &&
           std::abs(t_ - other.t_) < tol;
}
 
//------------------------------------------------------------------------------
// Group Operations
//------------------------------------------------------------------------------
Gal3 Gal3::inverse() const {
    // Implements inverse formula from Equation 10, Page 5
    const Rot3 Rinv = R_.inverse();
    const Velocity3 v_inv = -(Rinv.rotate(v_));
    const Point3 r_inv = -(Rinv.rotate(Vector3(r_) - t_ * v_));
    const double t_inv = -t_;
    return Gal3(Rinv, r_inv, v_inv, t_inv);
}
 
//------------------------------------------------------------------------------
Gal3 Gal3::operator*(const Gal3& other) const {
    // Implements group composition through matrix multiplication
    const Gal3& g1 = *this;
    const Gal3& g2 = other;
 
    const Rot3 R_comp = g1.R_.compose(g2.R_);
    const Vector3 r1_vec(g1.r_);
    const Vector3 r2_vec(g2.r_);
    const Vector3 r_comp_vec = g1.R_.rotate(r2_vec) + g2.t_ * g1.v_ + r1_vec;
    const Velocity3 v_comp = g1.R_.rotate(g2.v_) + g1.v_;
    const double t_comp = g1.t_ + g2.t_;
 
    return Gal3(R_comp, Point3(r_comp_vec), v_comp, t_comp);
}
 
//------------------------------------------------------------------------------
// Lie Group Static Functions
//------------------------------------------------------------------------------
gtsam::Gal3 gtsam::Gal3::Expmap(const Vector10& xi, OptionalJacobian<10, 10> Hxi) {
    // Implements exponential map from Equations 16-19, Pages 7-8
    const Vector3 rho_tan = rho(xi);
    const Vector3 nu_tan = nu(xi);
    const Vector3 theta_tan = theta(xi);
    const double t_tan_val = t_tan(xi)(0);
 
    const gtsam::so3::DexpFunctor dexp_functor(theta_tan);
    const Rot3 R = Rot3::Expmap(theta_tan);
    const Matrix3 Jl_theta = dexp_functor.leftJacobian();
 
    Matrix3 E;
    if (dexp_functor.nearZero) {
         // Small angle approximation for E matrix (from Equation 19, Page 8)
         E = 0.5 * Matrix3::Identity() + (1.0 / 6.0) * dexp_functor.W + (1.0 / 24.0) * dexp_functor.WW;
    } else {
         // Closed form for E matrix (from Equation 19, Page 8)
         const double B_E = (1.0 - 2.0 * dexp_functor.B) / (2.0 * dexp_functor.theta2);
         E = 0.5 * Matrix3::Identity() + dexp_functor.C * dexp_functor.W + B_E * dexp_functor.WW;
    }
 
    const Point3 r_final = Point3(Jl_theta * rho_tan + E * (t_tan_val * nu_tan));
    const Velocity3 v_final = Jl_theta * nu_tan;
    const double t_final = t_tan_val;
 
    Gal3 result(R, r_final, v_final, t_final);
 
    if (Hxi) {
        *Hxi = Gal3::ExpmapDerivative(xi);
    }
 
    return result;
}
 
//------------------------------------------------------------------------------
Vector10 Gal3::Logmap(const Gal3& g, OptionalJacobian<10, 10> Hg) {
    // Implements logarithmic map from Equations 20-23, Page 8
    const Vector3 theta_vec = Rot3::Logmap(g.R_);
    const gtsam::so3::DexpFunctor dexp_functor_log(theta_vec);
    const Matrix3 Jl_theta_inv = dexp_functor_log.leftJacobianInverse();
 
    Matrix3 E;
    if (dexp_functor_log.nearZero) {
         // Small angle approximation for E matrix
         E = 0.5 * Matrix3::Identity() + (1.0 / 6.0) * dexp_functor_log.W + (1.0 / 24.0) * dexp_functor_log.WW;
    } else {
         // Closed form for E matrix (from Equation 19, Page 8)
         const double B_E = (1.0 - 2.0 * dexp_functor_log.B) / (2.0 * dexp_functor_log.theta2);
         E = 0.5 * Matrix3::Identity() + dexp_functor_log.C * dexp_functor_log.W + B_E * dexp_functor_log.WW;
    }
 
    const Vector3 r_vec = Vector3(g.r_);
    const Velocity3& v_vec = g.v_;
    const double& t_val = g.t_;
 
    // Implementation of Equation 23, Page 8
    const Vector3 nu_tan = Jl_theta_inv * v_vec;
    const Vector3 rho_tan = Jl_theta_inv * (r_vec - E * (t_val * nu_tan));
    const double t_tan_val = t_val;
 
    Vector10 xi;
    rho(xi) = rho_tan;
    nu(xi) = nu_tan;
    theta(xi) = theta_vec;
    t_tan(xi)(0) = t_tan_val;
 
    if (Hg) {
        *Hg = Gal3::LogmapDerivative(g);
    }
 
    return xi;
}
 
//------------------------------------------------------------------------------
Matrix10 Gal3::AdjointMap() const {
    // Implements adjoint map as in Equation 26, Page 9
    const Matrix3 Rmat = R_.matrix();
    const Vector3 v_vec = v_;
    const Vector3 r_minus_tv = Vector3(r_) - t_ * v_;
 
    Matrix10 Ad = Matrix10::Zero();
 
    Ad.block<3,3>(0,0) = Rmat;
    Ad.block<3,3>(0,3) = -t_ * Rmat;
    Ad.block<3,3>(0,6) = skewSymmetric(r_minus_tv) * Rmat;
    Ad.block<3,1>(0,9) = v_vec;
 
    Ad.block<3,3>(3,3) = Rmat;
    Ad.block<3,3>(3,6) = skewSymmetric(v_vec) * Rmat;
 
    Ad.block<3,3>(6,6) = Rmat;
 
    Ad(9,9) = 1.0;
 
    return Ad;
}
 
//------------------------------------------------------------------------------
Vector10 Gal3::Adjoint(const Vector10& xi, OptionalJacobian<10, 10> H_g, OptionalJacobian<10, 10> H_xi) const {
    Matrix10 Ad = AdjointMap();
    Vector10 y = Ad * xi;
 
    if (H_xi) {
        *H_xi = Ad;
    }
 
    if (H_g) {
        // NOTE: Using numerical derivative for the Jacobian with respect to
        // the group element instead of deriving the analytical expression.
        // Future work to use analytical instead.
        std::function<Vector10(const Gal3&, const Vector10&)> adjoint_action_wrt_g =
          [&](const Gal3& g_in, const Vector10& xi_in) {
              return g_in.Adjoint(xi_in);
          };
        *H_g = numericalDerivative21(adjoint_action_wrt_g, *this, xi, 1e-7);
    }
    return y;
}
 
//------------------------------------------------------------------------------
Matrix10 Gal3::adjointMap(const Vector10& xi) {
    // Implements adjoint representation as in Equation 28, Page 10
    const Matrix3 Theta_hat = skewSymmetric(theta(xi));
    const Matrix3 Nu_hat = skewSymmetric(nu(xi));
    const Matrix3 Rho_hat = skewSymmetric(rho(xi));
    const double t_val = t_tan(xi)(0);
    const Vector3 nu_vec = nu(xi);
 
    Matrix10 ad = Matrix10::Zero();
 
    ad.block<3,3>(0,0) = Theta_hat;
    ad.block<3,3>(0,3) = -t_val * Matrix3::Identity();
    ad.block<3,3>(0,6) = Rho_hat;
    ad.block<3,1>(0,9) = nu_vec;
 
    ad.block<3,3>(3,3) = Theta_hat;
    ad.block<3,3>(3,6) = Nu_hat;
 
    ad.block<3,3>(6,6) = Theta_hat;
 
    return ad;
}
 
//------------------------------------------------------------------------------
Vector10 Gal3::adjoint(const Vector10& xi, const Vector10& y, OptionalJacobian<10, 10> Hxi, OptionalJacobian<10, 10> Hy) {
    Matrix10 ad_xi = adjointMap(xi);
    if (Hy) *Hy = ad_xi;
    if (Hxi) {
         *Hxi = -adjointMap(y);
    }
    return ad_xi * y;
}
 
//------------------------------------------------------------------------------
Matrix10 Gal3::ExpmapDerivative(const Vector10& xi) {
    // Related to the left Jacobian in Equations 31-36, Pages 10-11
    // NOTE: Using numerical approximation instead of implementing the analytical
    // expression for the Jacobian. Future work to replace this
    // with analytical derivative.
    if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity();
    std::function<Gal3(const Vector10&)> fn =
        [](const Vector10& v) { return Gal3::Expmap(v); };
    return numericalDerivative11<Gal3, Vector10>(fn, xi, 1e-5);
}
 
//------------------------------------------------------------------------------
Matrix10 Gal3::LogmapDerivative(const Gal3& g) {
    // Related to the inverse of left Jacobian in Equations 31-36, Pages 10-11
    // NOTE: Using numerical approximation instead of implementing the analytical
    // expression for the inverse Jacobian. Future work to replace this
    // with analytical derivative.
    Vector10 xi = Gal3::Logmap(g);
    if (xi.norm() < kSmallAngleThreshold) return Matrix10::Identity();
    std::function<Vector10(const Gal3&)> fn =
        [](const Gal3& g_in) { return Gal3::Logmap(g_in); };
    return numericalDerivative11<Vector10, Gal3>(fn, g, 1e-5);
}
 
//------------------------------------------------------------------------------
// Lie Algebra (Hat/Vee maps)
//------------------------------------------------------------------------------
Matrix5 Gal3::Hat(const Vector10& xi) {
    // Implements hat operator as in Equation 13, Page 6
    const Vector3 rho_tan = rho(xi);
    const Vector3 nu_tan = nu(xi);
    const Vector3 theta_tan = theta(xi);
    const double t_tan_val = t_tan(xi)(0);
 
    Matrix5 X = Matrix5::Zero();
    X.block<3, 3>(0, 0) = skewSymmetric(theta_tan);
    X.block<3, 1>(0, 3) = nu_tan;
    X.block<3, 1>(0, 4) = rho_tan;
    X(3, 4) = t_tan_val;
    return X;
}
 
//------------------------------------------------------------------------------
Vector10 Gal3::Vee(const Matrix5& X) {
    // Implements vee operator (inverse of hat operator in Equation 13, Page 6)
    if (X.row(4).norm() > 1e-9 || X.row(3).head(3).norm() > 1e-9 || std::abs(X(3,3)) > 1e-9) {
     throw std::invalid_argument("Matrix is not in sgal(3)");
    }
 
    Vector10 xi;
    rho(xi) = X.block<3, 1>(0, 4);
    nu(xi) = X.block<3, 1>(0, 3);
    const Matrix3& S = X.block<3, 3>(0, 0);
    theta(xi) << S(2, 1), S(0, 2), S(1, 0);
    t_tan(xi)(0) = X(3, 4);
    return xi;
}
 
//------------------------------------------------------------------------------
// ChartAtOrigin
//------------------------------------------------------------------------------
Gal3 Gal3::ChartAtOrigin::Retract(const Vector10& xi, ChartJacobian Hxi) {
  return Gal3::Expmap(xi, Hxi);
}
 
//------------------------------------------------------------------------------
Vector10 Gal3::ChartAtOrigin::Local(const Gal3& g, ChartJacobian Hg) {
  return Gal3::Logmap(g, Hg);
}
 
//------------------------------------------------------------------------------
Event Gal3::act(const Event& e, OptionalJacobian<4, 10> Hself,
                OptionalJacobian<4, 4> He) const {
  // Implements group action on events (spacetime points) as described in Section 4.1, Page 3-4
  const double& t_in = e.time();
  const Point3& p_in = e.location();
 
  const double t_out = t_in + t_;
  const Point3 p_out = R_.rotate(p_in) + v_ * t_in + r_;
 
  if (He) {
    He->setZero();
    (*He)(0, 0) = 1.0;
    He->block<3, 1>(1, 0) = v_;
    He->block<3, 3>(1, 1) = R_.matrix();
  }
 
  if (Hself) {
    Hself->setZero();
    const Matrix3 Rmat = R_.matrix();
 
    (*Hself)(0, 9) = 1.0;
    Hself->block<3, 3>(1, 0) = Rmat;
    Hself->block<3, 3>(1, 3) = Rmat * t_in;
    Hself->block<3, 3>(1, 6) = -Rmat * skewSymmetric(p_in);
    Hself->block<3, 1>(1, 9) = v_;
  }
 
  return Event(t_out, p_out);
}
 
} // namespace gtsam