12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Made constructor explicit, expose SOn in wrapper

release/4.3a0
Frank Dellaert 2019-04-20 16:49:20 -04:00 committed by Fan Jiang
parent ad0bb79533
commit 3e1db48ced
3 changed files with 30 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam.h
View File

@ -591,6 +591,17 @@ class SO4 {
Matrix matrix() const;
};
#include <gtsam/geometry/SOn.h>
class SOn {
// Standard Constructors
SOn(size_t n);
SOn(Matrix R);
// Other methods
Vector vec() const;
Matrix matrix() const;
};
#include <gtsam/geometry/Rot3.h>
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<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
T at(size_t j);
/// version for double

2
gtsam/geometry/SO4.h
View File

@ -107,7 +107,7 @@ class SO4 : public Matrix4, public LieGroup<SO4, 6> {
/// Return matrix (for wrapper)
const Matrix4 &matrix() const { return *this; }
/// @
/// @}
private:
/** Serialization function */

20
gtsam/geometry/SOn.h
View File

@ -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 <typename Derived>
SOn(const Eigen::MatrixBase<Derived>& R) : Eigen::MatrixXd(R.eval()) {}
explicit SOn(const Eigen::MatrixBase<Derived>& 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<SOn> {
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);
}