diff --git a/gtsam.h b/gtsam.h index d06d83a56..efe36a595 100644 --- a/gtsam.h +++ b/gtsam.h @@ -591,6 +591,17 @@ class SO4 { Matrix matrix() const; }; +#include +class SOn { + // Standard Constructors + SOn(size_t n); + SOn(Matrix R); + + // Other methods + Vector vec() const; + Matrix matrix() const; +}; + #include class Rot3 { // Standard Constructors and Named Constructors @@ -2031,6 +2042,7 @@ class Values { void insert(size_t j, const gtsam::Pose2& pose2); void insert(size_t j, const gtsam::SO3& R); void insert(size_t j, const gtsam::SO4& Q); + void insert(size_t j, const gtsam::SOn& P); void insert(size_t j, const gtsam::Rot3& rot3); void insert(size_t j, const gtsam::Pose3& pose3); void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); @@ -2048,6 +2060,7 @@ class Values { void update(size_t j, const gtsam::Pose2& pose2); void update(size_t j, const gtsam::SO3& R); void update(size_t j, const gtsam::SO4& Q); + void update(size_t j, const gtsam::SOn& P); void update(size_t j, const gtsam::Rot3& rot3); void update(size_t j, const gtsam::Pose3& pose3); void update(size_t j, const gtsam::Cal3_S2& cal3_s2); @@ -2058,7 +2071,7 @@ class Values { void update(size_t j, Vector vector); void update(size_t j, Matrix matrix); - template + template T at(size_t j); /// version for double diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index e270c9555..59279b558 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -107,7 +107,7 @@ class SO4 : public Matrix4, public LieGroup { /// Return matrix (for wrapper) const Matrix4 &matrix() const { return *this; } - /// @ + /// @} private: /** Serialization function */ diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 1b5228be0..475e71861 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -107,14 +107,17 @@ class SOn : public Eigen::MatrixXd, /// @name Constructors /// @{ + /// Default constructor: only used for serialization and wrapping + SOn() {} + /// Construct SO(n) identity - SOn(size_t n) : Eigen::MatrixXd(n, n) { + explicit SOn(size_t n) : Eigen::MatrixXd(n, n) { *this << Eigen::MatrixXd::Identity(n, n); } /// Constructor from Eigen Matrix template - SOn(const Eigen::MatrixBase& R) : Eigen::MatrixXd(R.eval()) {} + explicit SOn(const Eigen::MatrixBase& R) : Eigen::MatrixXd(R.eval()) {} /// Random SO(n) element (no big claims about uniformity) static SOn Random(boost::mt19937& rng, size_t n) { @@ -211,7 +214,7 @@ class SOn : public Eigen::MatrixXd, static SOn Retract(size_t n, const Vector& xi) { const Matrix X = Hat(n, xi / 2.0); const auto I = Eigen::MatrixXd::Identity(n, n); - return (I + X) * (I - X).inverse(); + return SOn((I + X) * (I - X).inverse()); } /** @@ -228,6 +231,13 @@ class SOn : public Eigen::MatrixXd, /// @name Lie group /// @{ + /// @} + /// @name Other methods + /// @{ + + /// Return matrix (for wrapper) + const Matrix &matrix() const { return *this; } + /// @} }; @@ -275,14 +285,14 @@ struct traits { ChartJacobian H2 = boost::none) { if (H1 || H2) throw std::runtime_error("SOn::Retract"); const size_t n = R.rows(); - return R * SOn::Retract(n, xi); + return SOn(R * SOn::Retract(n, xi)); } static TangentVector Local(const SOn& R, const SOn& other, // ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { if (H1 || H2) throw std::runtime_error("SOn::Local"); - Matrix between = R.inverse() * other; + const SOn between = SOn(R.inverse() * other); return SOn::Local(between); }