SL4.h

Go to the documentation of this file.

 
#pragma once
 
#include <gtsam/base/Matrix.h>
#include <gtsam/base/MatrixLieGroup.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/Vector.h>
#include <gtsam/config.h>
#include <gtsam/dllexport.h>
 
#if GTSAM_ENABLE_BOOST_SERIALIZATION
#include <gtsam/base/MatrixSerialization.h>
#endif
 
#include <string>
 
using SL4Jacobian = gtsam::OptionalJacobian<15, 15>;
 
using Matrix15x15 = Eigen::Matrix<double, 15, 15>;
using Matrix16x16 = Eigen::Matrix<double, 16, 16>;
 
namespace gtsam {
// NOTE(hlim): Strictly speaking, it should be expressed as SL(4, ℝ),
// but for simplicity, we omit ℝ, assuming our target is over the real numbers.
// And the variable `sl4` represents SL(4, ℝ).
class GTSAM_EXPORT SL4 : public MatrixLieGroup<SL4, 15, 4> {
 public:
  static const size_t dimension = 15;
 
 protected:
  Matrix44 T_;
 
 public:

  SL4() : T_(Matrix44::Identity()) {}

  SL4(const Matrix44& pose);
 
  SL4(const SL4& pose) = default;
 
  SL4& operator=(const SL4& pose) = default;

  void print(const std::string& s = "") const;

  bool equals(const SL4& sl4, double tol = 1e-9) const;

  inline const Matrix44& matrix() const { return T_; }


  static SL4 Identity() { return SL4(); }

  SL4 inverse() const { return SL4(T_.inverse()); }

  SL4 operator*(const SL4& other) const { return SL4(T_ * other.T_); }

 
  // compose and between provided by LieGroup

  Matrix15x15 AdjointMap() const;

  using LieGroup<SL4, 15>::inverse;

  static SL4 Expmap(const Vector& xi);

  static Vector Logmap(const SL4& p);
 
  // Chart at origin
  struct GTSAM_EXPORT ChartAtOrigin {
    static SL4 Retract(const Vector15& xi);
    static Vector15 Local(const SL4& pose);
  };
 
  // retract and localCoordinates provided by LieGroup

 
  using LieAlgebra = Matrix44;
  static Matrix44 Hat(const Vector& xi);
  static Vector Vee(const Matrix44& X);

 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class Archive>
  void serialize(Archive& ar, const unsigned int /*version*/) {
    ar& BOOST_SERIALIZATION_NVP(T_);
  }
#endif
};  // \class SL4
 
template <>
struct traits<SL4> : public internal::MatrixLieGroup<SL4, 4> {};
 
template <>
struct traits<const SL4> : public internal::MatrixLieGroup<SL4, 4> {};
 
}  // namespace gtsam