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

Moved code into appropriate headers (SOt instead of SO3 for now)

release/4.3a0
Frank Dellaert 2019-05-06 11:07:09 -04:00 committed by Fan Jiang
parent 96392c74c0
commit 6cff36975f
7 changed files with 1014 additions and 1059 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/SO3.h
View File

@ -20,8 +20,6 @@
#pragma once
#include <gtsam/geometry/SOn.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>
@ -35,7 +33,7 @@ namespace gtsam {
* We guarantee (all but first) constructors only generate from sub-manifold.
* However, round-off errors in repeated composition could move off it...
*/
class SO3 : public SOnBase<SO3>, public Matrix3, public LieGroup<SO3, 3> {
class SO3 : public Matrix3, public LieGroup<SO3, 3> {
public:
enum { N = 3 };
enum { dimension = 3 };

345
gtsam/geometry/SO4.cpp
View File

@ -31,196 +31,199 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
static Vector3 randomOmega(boost::mt19937 &rng) {
static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
return Unit3::Random(rng).unitVector() * randomAngle(rng);
}
// /* ************************************************************************* */
// static Vector3 randomOmega(boost::mt19937 &rng) {
// static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
// return Unit3::Random(rng).unitVector() * randomAngle(rng);
// }
/* ************************************************************************* */
// Create random SO(4) element using direct product of lie algebras.
SO4 SO4::Random(boost::mt19937 &rng) {
Vector6 delta;
delta << randomOmega(rng), randomOmega(rng);
return SO4::Expmap(delta);
}
// /* ************************************************************************* */
// // Create random SO(4) element using direct product of lie algebras.
// SO4 SO4::Random(boost::mt19937 &rng) {
// Vector6 delta;
// delta << randomOmega(rng), randomOmega(rng);
// return SO4::Expmap(delta);
// }
/* ************************************************************************* */
void SO4::print(const string &s) const { cout << s << *this << endl; }
// /* ************************************************************************* */
// void SO4::print(const string &s) const { cout << s << *this << endl; }
/* ************************************************************************* */
bool SO4::equals(const SO4 &R, double tol) const {
return equal_with_abs_tol(*this, R, tol);
}
// /* ************************************************************************* */
// bool SO4::equals(const SO4 &R, double tol) const {
// return equal_with_abs_tol(*this, R, tol);
// }
//******************************************************************************
Matrix4 SO4::Hat(const Vector6 &xi) {
// skew symmetric matrix X = xi^
// Unlike Luca, makes upper-left the SO(3) subgroup.
// See also
// http://scipp.ucsc.edu/~haber/archives/physics251_13/groups13_sol4.pdf
Matrix4 Y = Z_4x4;
Y(0, 1) = -xi(2);
Y(0, 2) = +xi(1);
Y(1, 2) = -xi(0);
Y(0, 3) = -xi(3);
Y(1, 3) = -xi(4);
Y(2, 3) = -xi(5);
return Y - Y.transpose();
}
/* ************************************************************************* */
Vector6 SO4::Vee(const Matrix4 &X) {
Vector6 xi;
xi(2) = -X(0, 1);
xi(1) = X(0, 2);
xi(0) = -X(1, 2);
xi(3) = -X(0, 3);
xi(4) = -X(1, 3);
xi(5) = -X(2, 3);
return xi;
}
// //******************************************************************************
// Matrix4 SO4::Hat(const Vector6 &xi) {
// // skew symmetric matrix X = xi^
// // Unlike Luca, makes upper-left the SO(3) subgroup.
// // See also
// // http://scipp.ucsc.edu/~haber/archives/physics251_13/groups13_sol4.pdf
// Matrix4 Y = Z_4x4;
// Y(0, 1) = -xi(2);
// Y(0, 2) = +xi(1);
// Y(1, 2) = -xi(0);
// Y(0, 3) = -xi(3);
// Y(1, 3) = -xi(4);
// Y(2, 3) = -xi(5);
// return Y - Y.transpose();
// }
// /* ************************************************************************* */
// Vector6 SO4::Vee(const Matrix4 &X) {
// Vector6 xi;
// xi(2) = -X(0, 1);
// xi(1) = X(0, 2);
// xi(0) = -X(1, 2);
// xi(3) = -X(0, 3);
// xi(4) = -X(1, 3);
// xi(5) = -X(2, 3);
// return xi;
// }
//******************************************************************************
/* Exponential map, porting MATLAB implementation by Luca, which follows
* "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
* Ramona-Andreaa Rohan */
SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H) {
if (H) throw std::runtime_error("SO4::Expmap Jacobian");
gttic(SO4_Expmap);
// //******************************************************************************
// /* Exponential map, porting MATLAB implementation by Luca, which follows
// * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
// * Ramona-Andreaa Rohan */
// template <>
// SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
// using namespace std;
// if (H) throw std::runtime_error("SO4::Expmap Jacobian");
// skew symmetric matrix X = xi^
const Matrix4 X = Hat(xi);
// // skew symmetric matrix X = xi^
// const Matrix4 X = Hat(xi);
// do eigen-decomposition
auto eig = Eigen::EigenSolver<Matrix4>(X);
Eigen::Vector4cd e = eig.eigenvalues();
using std::abs;
sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
return abs(a.imag()) > abs(b.imag());
});
// // do eigen-decomposition
// auto eig = Eigen::EigenSolver<Matrix4>(X);
// Eigen::Vector4cd e = eig.eigenvalues();
// using std::abs;
// sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
// return abs(a.imag()) > abs(b.imag());
// });
// Get a and b from eigenvalues +/i ai and +/- bi
double a = e[0].imag(), b = e[2].imag();
if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
throw runtime_error("SO4::Expmap: wrong eigenvalues.");
}
// // Get a and b from eigenvalues +/i ai and +/- bi
// double a = e[0].imag(), b = e[2].imag();
// if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
// throw runtime_error("SO4::Expmap: wrong eigenvalues.");
// }
// Build expX = exp(xi^)
Matrix4 expX;
using std::cos;
using std::sin;
const auto X2 = X * X;
const auto X3 = X2 * X;
double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
if (a != 0 && b == 0) {
double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
return I_4x4 + X + c2 * X2 + c3 * X3;
} else if (a == b && b != 0) {
double sin_a = sin(a), cos_a = cos(a);
double c0 = (a * sin_a + 2 * cos_a) / 2,
c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a),
c3 = (sin_a - a * cos_a) / (2 * a3);
return c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3;
} else if (a != b) {
double sin_a = sin(a), cos_a = cos(a);
double sin_b = sin(b), cos_b = cos(b);
double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2),
c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)),
c2 = (cos_a - cos_b) / (b2 - a2),
c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2));
return c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3;
} else {
return I_4x4;
}
}
// // Build expX = exp(xi^)
// Matrix4 expX;
// using std::cos;
// using std::sin;
// const auto X2 = X * X;
// const auto X3 = X2 * X;
// double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
// if (a != 0 && b == 0) {
// double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
// return SO4(I_4x4 + X + c2 * X2 + c3 * X3);
// } else if (a == b && b != 0) {
// double sin_a = sin(a), cos_a = cos(a);
// double c0 = (a * sin_a + 2 * cos_a) / 2,
// c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a),
// c3 = (sin_a - a * cos_a) / (2 * a3);
// return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
// } else if (a != b) {
// double sin_a = sin(a), cos_a = cos(a);
// double sin_b = sin(b), cos_b = cos(b);
// double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2),
// c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)),
// c2 = (cos_a - cos_b) / (b2 - a2),
// c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2));
// return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
// } else {
// return SO4();
// }
// }
//******************************************************************************
Vector6 SO4::Logmap(const SO4 &Q, ChartJacobian H) {
if (H) throw std::runtime_error("SO4::Logmap Jacobian");
throw std::runtime_error("SO4::Logmap");
}
// //******************************************************************************
// static SO4::VectorN2 vec4(const Matrix4& Q) {
// return Eigen::Map<const SO4::VectorN2>(Q.data());
// }
/* ************************************************************************* */
SO4 SO4::ChartAtOrigin::Retract(const Vector6 &xi, ChartJacobian H) {
if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
gttic(SO4_Retract);
const Matrix4 X = Hat(xi / 2);
return (I_4x4 + X) * (I_4x4 - X).inverse();
}
// static const std::vector<const Matrix4> G4(
// {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)),
// SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
// SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});
/* ************************************************************************* */
Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H) {
if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
const Matrix4 X = (I_4x4 - Q) * (I_4x4 + Q).inverse();
return -2 * Vee(X);
}
// static const Eigen::Matrix<double, 16, 6> P4 =
// (Eigen::Matrix<double, 16, 6>() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]),
// vec4(G4[3]), vec4(G4[4]), vec4(G4[5]))
// .finished();
/* ************************************************************************* */
static SO4::Vector16 vec(const SO4 &Q) {
return Eigen::Map<const SO4::Vector16>(Q.data());
}
// //******************************************************************************
// template <>
// Matrix6 SO4::AdjointMap() const {
// // Elaborate way of calculating the AdjointMap
// // TODO(frank): find a closed form solution. In SO(3) is just R :-/
// const Matrix4& Q = matrix_;
// const Matrix4 Qt = Q.transpose();
// Matrix6 A;
// for (size_t i = 0; i < 6; i++) {
// // Calculate column i of linear map for coeffcient of Gi
// A.col(i) = SO4::Vee(Q * G4[i] * Qt);
// }
// return A;
// }
static const std::vector<const Matrix4> G(
{SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)),
SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});
// //******************************************************************************
// template <>
// SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const {
// const Matrix& Q = matrix_;
// if (H) {
// // As Luca calculated, this is (I4 \oplus Q) * P4
// *H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0),
// Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0);
// }
// return gtsam::vec4(Q);
// }
static const Eigen::Matrix<double, 16, 6> P =
(Eigen::Matrix<double, 16, 6>() << vec(G[0]), vec(G[1]), vec(G[2]),
vec(G[3]), vec(G[4]), vec(G[5]))
.finished();
/* ************************************************************************* */
Matrix6 SO4::AdjointMap() const {
gttic(SO4_AdjointMap);
// Elaborate way of calculating the AdjointMap
// TODO(frank): find a closed form solution. In SO(3) is just R :-/
const SO4 &Q = *this;
const SO4 Qt = this->inverse();
Matrix6 A;
for (size_t i = 0; i < 6; i++) {
// Calculate column i of linear map for coeffcient of Gi
A.col(i) = SO4::Vee(Q * G[i] * Qt);
}
return A;
}
// //******************************************************************************
// Vector6 SO4::Logmap(const SO4 &Q, ChartJacobian H) {
// if (H) throw std::runtime_error("SO4::Logmap Jacobian");
// throw std::runtime_error("SO4::Logmap");
// }
/* ************************************************************************* */
SO4::Vector16 SO4::vec(OptionalJacobian<16, 6> H) const {
const SO4 &Q = *this;
if (H) {
// As Luca calculated, this is (I4 \oplus Q) * P
*H << Q * P.block<4, 6>(0, 0), Q * P.block<4, 6>(4, 0),
Q * P.block<4, 6>(8, 0), Q * P.block<4, 6>(12, 0);
}
return gtsam::vec(Q);
};
// /* ************************************************************************* */
// SO4 SO4::ChartAtOrigin::Retract(const Vector6 &xi, ChartJacobian H) {
// if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
// gttic(SO4_Retract);
// const Matrix4 X = Hat(xi / 2);
// return (I_4x4 + X) * (I_4x4 - X).inverse();
// }
/* ************************************************************************* */
Matrix3 SO4::topLeft(OptionalJacobian<9, 6> H) const {
const Matrix3 M = this->topLeftCorner<3, 3>();
const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
q = this->topRightCorner<3, 1>();
if (H) {
*H << Z_3x1, -m3, m2, q, Z_3x1, Z_3x1, //
m3, Z_3x1, -m1, Z_3x1, q, Z_3x1, //
-m2, m1, Z_3x1, Z_3x1, Z_3x1, q;
}
return M;
}
// /* ************************************************************************* */
// Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H) {
// if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
// const Matrix4 X = (I_4x4 - Q) * (I_4x4 + Q).inverse();
// return -2 * Vee(X);
// }
/* ************************************************************************* */
Matrix43 SO4::stiefel(OptionalJacobian<12, 6> H) const {
const Matrix43 M = this->leftCols<3>();
const auto &m1 = col(0), m2 = col(1), m3 = col(2), q = col(3);
if (H) {
*H << Z_4x1, -m3, m2, q, Z_4x1, Z_4x1, //
m3, Z_4x1, -m1, Z_4x1, q, Z_4x1, //
-m2, m1, Z_4x1, Z_4x1, Z_4x1, q;
}
return M;
}
// /* ************************************************************************* */
// Matrix3 SO4::topLeft(OptionalJacobian<9, 6> H) const {
// const Matrix3 M = this->topLeftCorner<3, 3>();
// const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
// q = this->topRightCorner<3, 1>();
// if (H) {
// *H << Z_3x1, -m3, m2, q, Z_3x1, Z_3x1, //
// m3, Z_3x1, -m1, Z_3x1, q, Z_3x1, //
// -m2, m1, Z_3x1, Z_3x1, Z_3x1, q;
// }
// return M;
// }
/* ************************************************************************* */
// /* ************************************************************************* */
// Matrix43 SO4::stiefel(OptionalJacobian<12, 6> H) const {
// const Matrix43 M = this->leftCols<3>();
// const auto &m1 = col(0), m2 = col(1), m3 = col(2), q = col(3);
// if (H) {
// *H << Z_4x1, -m3, m2, q, Z_4x1, Z_4x1, //
// m3, Z_4x1, -m1, Z_4x1, q, Z_4x1, //
// -m2, m1, Z_4x1, Z_4x1, Z_4x1, q;
// }
// return M;
// }
// /* ************************************************************************* */
} // end namespace gtsam

238
gtsam/geometry/SO4.h
View File

@ -33,111 +33,155 @@
namespace gtsam {
/**
* True SO(4), i.e., 4*4 matrix subgroup
*/
class SO4 : public SOnBase<SO4>, public Matrix4, public LieGroup<SO4, 6> {
public:
enum { N = 4 };
enum { dimension = 6 };
using SO4 = SO<4>;
typedef Eigen::Matrix<double, 16, 1> Vector16;
// /// Random SO(4) element (no big claims about uniformity)
// static SO4 Random(boost::mt19937 &rng);
/// @name Constructors
/// @{
// static Matrix4 Hat(const Vector6 &xi); ///< make skew symmetric matrix
// static Vector6 Vee(const Matrix4 &X); ///< inverse of Hat
// static SO4 Expmap(const Vector6 &xi,
// ChartJacobian H = boost::none); ///< exponential map
// static Vector6 Logmap(const SO4 &Q,
// ChartJacobian H = boost::none); ///< and its inverse
// Matrix6 AdjointMap() const;
/// Default constructor creates identity
SO4() : Matrix4(I_4x4) {}
//******************************************************************************
/* Exponential map, porting MATLAB implementation by Luca, which follows
* "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
* Ramona-Andreaa Rohan */
template <>
SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
using namespace std;
if (H) throw std::runtime_error("SO4::Expmap Jacobian");
/// Constructor from Eigen Matrix
template <typename Derived>
SO4(const MatrixBase<Derived> &R) : Matrix4(R.eval()) {}
// skew symmetric matrix X = xi^
const Matrix4 X = Hat(xi);
/// Random SO(4) element (no big claims about uniformity)
static SO4 Random(boost::mt19937 &rng);
// do eigen-decomposition
auto eig = Eigen::EigenSolver<Matrix4>(X);
Eigen::Vector4cd e = eig.eigenvalues();
using std::abs;
sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
return abs(a.imag()) > abs(b.imag());
});
/// @}
/// @name Testable
/// @{
void print(const std::string &s) const;
bool equals(const SO4 &R, double tol) const;
/// @}
/// @name Group
/// @{
/// identity rotation for group operation
static SO4 identity() { return I_4x4; }
/// inverse of a rotation = transpose
SO4 inverse() const { return this->transpose(); }
/// @}
/// @name Lie Group
/// @{
static Matrix4 Hat(const Vector6 &xi); ///< make skew symmetric matrix
static Vector6 Vee(const Matrix4 &X); ///< inverse of Hat
static SO4 Expmap(const Vector6 &xi,
ChartJacobian H = boost::none); ///< exponential map
static Vector6 Logmap(const SO4 &Q,
ChartJacobian H = boost::none); ///< and its inverse
Matrix6 AdjointMap() const;
// Chart at origin
struct ChartAtOrigin {
static SO4 Retract(const Vector6 &omega, ChartJacobian H = boost::none);
static Vector6 Local(const SO4 &R, ChartJacobian H = boost::none);
};
using LieGroup<SO4, 6>::inverse;
/// @}
/// @name Other methods
/// @{
/// Vectorize
Vector16 vec(OptionalJacobian<16, 6> H = boost::none) const;
/// Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3).
Matrix3 topLeft(OptionalJacobian<9, 6> H = boost::none) const;
/// Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> S \in St(3,4).
Matrix43 stiefel(OptionalJacobian<12, 6> H = boost::none) const;
/// Return matrix (for wrapper)
const Matrix4 &matrix() const { return *this; }
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
ar &boost::serialization::make_nvp("Q11", (*this)(0, 0));
ar &boost::serialization::make_nvp("Q12", (*this)(0, 1));
ar &boost::serialization::make_nvp("Q13", (*this)(0, 2));
ar &boost::serialization::make_nvp("Q14", (*this)(0, 3));
ar &boost::serialization::make_nvp("Q21", (*this)(1, 0));
ar &boost::serialization::make_nvp("Q22", (*this)(1, 1));
ar &boost::serialization::make_nvp("Q23", (*this)(1, 2));
ar &boost::serialization::make_nvp("Q24", (*this)(1, 3));
ar &boost::serialization::make_nvp("Q31", (*this)(2, 0));
ar &boost::serialization::make_nvp("Q32", (*this)(2, 1));
ar &boost::serialization::make_nvp("Q33", (*this)(2, 2));
ar &boost::serialization::make_nvp("Q34", (*this)(2, 3));
ar &boost::serialization::make_nvp("Q41", (*this)(3, 0));
ar &boost::serialization::make_nvp("Q42", (*this)(3, 1));
ar &boost::serialization::make_nvp("Q43", (*this)(3, 2));
ar &boost::serialization::make_nvp("Q44", (*this)(3, 3));
// Get a and b from eigenvalues +/i ai and +/- bi
double a = e[0].imag(), b = e[2].imag();
if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
throw runtime_error("SO4::Expmap: wrong eigenvalues.");
}
}; // SO4
// Build expX = exp(xi^)
Matrix4 expX;
using std::cos;
using std::sin;
const auto X2 = X * X;
const auto X3 = X2 * X;
double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
if (a != 0 && b == 0) {
double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
return SO4(I_4x4 + X + c2 * X2 + c3 * X3);
} else if (a == b && b != 0) {
double sin_a = sin(a), cos_a = cos(a);
double c0 = (a * sin_a + 2 * cos_a) / 2,
c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a),
c3 = (sin_a - a * cos_a) / (2 * a3);
return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
} else if (a != b) {
double sin_a = sin(a), cos_a = cos(a);
double sin_b = sin(b), cos_b = cos(b);
double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2),
c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)),
c2 = (cos_a - cos_b) / (b2 - a2),
c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2));
return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
} else {
return SO4();
}
}
//******************************************************************************
static SO4::VectorN2 vec4(const Matrix4& Q) {
return Eigen::Map<const SO4::VectorN2>(Q.data());
}
static const std::vector<const Matrix4> G4(
{SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)),
SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});
static const Eigen::Matrix<double, 16, 6> P4 =
(Eigen::Matrix<double, 16, 6>() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]),
vec4(G4[3]), vec4(G4[4]), vec4(G4[5]))
.finished();
//******************************************************************************
template <>
Matrix6 SO4::AdjointMap() const {
// Elaborate way of calculating the AdjointMap
// TODO(frank): find a closed form solution. In SO(3) is just R :-/
const Matrix4& Q = matrix_;
const Matrix4 Qt = Q.transpose();
Matrix6 A;
for (size_t i = 0; i < 6; i++) {
// Calculate column i of linear map for coeffcient of Gi
A.col(i) = SO4::Vee(Q * G4[i] * Qt);
}
return A;
}
//******************************************************************************
template <>
SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const {
const Matrix& Q = matrix_;
if (H) {
// As Luca calculated, this is (I4 \oplus Q) * P4
*H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0),
Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0);
}
return gtsam::vec4(Q);
}
// /// Vectorize
// Vector16 vec(OptionalJacobian<16, 6> H = boost::none) const;
// /// Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3).
// Matrix3 topLeft(OptionalJacobian<9, 6> H = boost::none) const;
// /// Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e.,
// pi(Q) -> S \in St(3,4). Matrix43 stiefel(OptionalJacobian<12, 6> H =
// boost::none) const;
// private:
// /** Serialization function */
// friend class boost::serialization::access;
// template <class ARCHIVE>
// void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
// ar &boost::serialization::make_nvp("Q11", (*this)(0, 0));
// ar &boost::serialization::make_nvp("Q12", (*this)(0, 1));
// ar &boost::serialization::make_nvp("Q13", (*this)(0, 2));
// ar &boost::serialization::make_nvp("Q14", (*this)(0, 3));
// ar &boost::serialization::make_nvp("Q21", (*this)(1, 0));
// ar &boost::serialization::make_nvp("Q22", (*this)(1, 1));
// ar &boost::serialization::make_nvp("Q23", (*this)(1, 2));
// ar &boost::serialization::make_nvp("Q24", (*this)(1, 3));
// ar &boost::serialization::make_nvp("Q31", (*this)(2, 0));
// ar &boost::serialization::make_nvp("Q32", (*this)(2, 1));
// ar &boost::serialization::make_nvp("Q33", (*this)(2, 2));
// ar &boost::serialization::make_nvp("Q34", (*this)(2, 3));
// ar &boost::serialization::make_nvp("Q41", (*this)(3, 0));
// ar &boost::serialization::make_nvp("Q42", (*this)(3, 1));
// ar &boost::serialization::make_nvp("Q43", (*this)(3, 2));
// ar &boost::serialization::make_nvp("Q44", (*this)(3, 3));
// }
/*
* Define the traits. internal::LieGroup provides both Lie group and Testable
*/
template <>
struct traits<SO4> : Testable<SO4>, internal::LieGroupTraits<SO4> {};

327
gtsam/geometry/SOn.h
View File

@ -11,94 +11,168 @@
/**
* @file SOn.h
* @brief n*n matrix representation of SO(n)
* @brief n*n matrix representation of SO(n), template on N, which can be
* Eigen::Dynamic
* @author Frank Dellaert
* @date March 2019
*/
#pragma once
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <Eigen/Core>
#include <boost/random.hpp>
#include <iostream>
#include <string>
namespace gtsam {
namespace internal {
/// Calculate dimensionality of SO<N> manifold, or return Dynamic if so
constexpr int DimensionSO(int N) {
return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2;
}
// Calculate N^2 at compile time, or return Dynamic if so
constexpr int VecSize(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
} // namespace internal
/**
* Base class for rotation group objects. Template arguments:
* Derived_: derived class
* Must have:
* N : dimension of ambient space, or Eigen::Dynamic, e.g. 4 for SO4
* dimension: manifold dimension, or Eigen::Dynamic, e.g. 6 for SO4
* Manifold of special orthogonal rotation matrices SO<N>.
* Template paramater N can be a fizxed integer or can be Eigen::Dynamic
*/
template <class Derived_>
class SOnBase {
template <int N>
class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
public:
/// @name Basic functionality
/// @{
enum { dimension = internal::DimensionSO(N) };
using MatrixNN = Eigen::Matrix<double, N, N>;
using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
/// @}
/// @name Manifold
/// @{
protected:
MatrixNN matrix_; ///< Rotation matrix
/// @}
/// @name Lie Group
/// @{
// enable_if_t aliases, used to specialize constructors/methods, see
// https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty-sfinae/
template <int N_>
using IsDynamic = boost::enable_if_t<N_ == Eigen::Dynamic, void>;
template <int N_>
using IsFixed = boost::enable_if_t<N_ >= 2, void>;
template <int N_>
using IsSO3 = boost::enable_if_t<N_ == 3, void>;
/// @}
/// @name Other methods
/// @{
/// @}
};
/**
* Variable size rotations
*/
class SOn : public Eigen::MatrixXd, public SOnBase<SOn> {
public:
enum { N = Eigen::Dynamic };
enum { dimension = Eigen::Dynamic };
/// @name Constructors
/// @{
/// Default constructor: only used for serialization and wrapping
SOn() {}
/// Construct SO<N> identity for N >= 2
template <int N_ = N, typename = IsFixed<N_>>
SO() : matrix_(MatrixNN::Identity()) {}
/// Construct SO(n) identity
explicit SOn(size_t n) : Eigen::MatrixXd(n, n) {
*this << Eigen::MatrixXd::Identity(n, n);
/// Construct SO<N> identity for N == Eigen::Dynamic
template <int N_ = N, typename = IsDynamic<N_>>
explicit SO(size_t n = 0) {
if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
matrix_ = Eigen::MatrixXd::Identity(n, n);
}
/// Constructor from Eigen Matrix
template <typename Derived_>
explicit SOn(const Eigen::MatrixBase<Derived_>& R)
: Eigen::MatrixXd(R.eval()) {}
template <typename Derived>
explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
/// Constructor from AngleAxisd
template <int N_ = N, typename = IsSO3<N_>>
SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {}
/// Random SO(n) element (no big claims about uniformity)
static SOn Random(boost::mt19937& rng, size_t n) {
template <int N_ = N, typename = IsDynamic<N_>>
static SO Random(boost::mt19937& rng, size_t n = 0) {
if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
// This needs to be re-thought!
static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
const size_t d = n * (n - 1) / 2;
const size_t d = SO::Dimension(n);
Vector xi(d);
for (size_t j = 0; j < d; j++) {
xi(j) = randomAngle(rng);
}
return SOn::Retract(n, xi);
return SO::Retract(xi);
}
/// Random SO(N) element (no big claims about uniformity)
template <int N_ = N, typename = IsFixed<N_>>
static SO Random(boost::mt19937& rng) {
// By default, use dynamic implementation above. Specialized for SO(3).
return SO(SO<Eigen::Dynamic>::Random(rng, N).matrix());
}
/// @}
/// @name Standard methods
/// @{
/// Return matrix
const MatrixNN& matrix() const { return matrix_; }
size_t rows() const { return matrix_.rows(); }
size_t cols() const { return matrix_.cols(); }
/// @}
/// @name Testable
/// @{
void print(const std::string& s) const {
std::cout << s << matrix_ << std::endl;
}
bool equals(const SO& other, double tol) const {
return equal_with_abs_tol(matrix_, other.matrix_, tol);
}
/// @}
/// @name Group
/// @{
/// Multiplication
SO operator*(const SO& other) const { return SO(matrix_ * other.matrix_); }
/// SO<N> identity for N >= 2
template <int N_ = N, typename = IsFixed<N_>>
static SO identity() {
return SO();
}
/// SO<N> identity for N == Eigen::Dynamic
template <int N_ = N, typename = IsDynamic<N_>>
static SO identity(size_t n = 0) {
return SO(n);
}
/// inverse of a rotation = transpose
SO inverse() const { return SO(matrix_.transpose()); }
/// @}
/// @name Manifold
/// @{
using TangentVector = Eigen::Matrix<double, dimension, 1>;
using ChartJacobian = OptionalJacobian<dimension, dimension>;
/// Return compile-time dimensionality: fixed size N or Eigen::Dynamic
static int Dim() { return dimension; }
// Calculate manifold dimensionality for SO(n).
// Available as dimension or Dim() for fixed N.
static size_t Dimension(size_t n) { return n * (n - 1) / 2; }
// Calculate ambient dimension n from manifold dimensionality d.
static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; }
// Calculate run-time dimensionality of manifold.
// Available as dimension or Dim() for fixed N.
size_t dim() const { return Dimension(matrix_.rows()); }
/**
* Hat operator creates Lie algebra element corresponding to d-vector, where d
* is the dimensionality of the manifold. This function is implemented
@ -114,7 +188,8 @@ class SOn : public Eigen::MatrixXd, public SOnBase<SOn> {
* -d c -b a 0
* This scheme behaves exactly as expected for SO(2) and SO(3).
*/
static Matrix Hat(size_t n, const Vector& xi) {
static Matrix Hat(const Vector& xi) {
size_t n = AmbientDim(xi.size());
if (n < 2) throw std::invalid_argument("SOn::Hat: n<2 not supported");
Matrix X(n, n); // allocate space for n*n skew-symmetric matrix
@ -126,7 +201,7 @@ class SOn : public Eigen::MatrixXd, public SOnBase<SOn> {
} else {
// Recursively call SO(n-1) call for top-left block
const size_t dmin = (n - 1) * (n - 2) / 2;
X.topLeftCorner(n - 1, n - 1) = Hat(n - 1, xi.tail(dmin));
X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin));
// Now fill last row and column
double sign = 1.0;
@ -172,126 +247,114 @@ class SOn : public Eigen::MatrixXd, public SOnBase<SOn> {
}
}
/**
* Retract uses Cayley map. See note about xi element order in Hat.
*/
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 SOn((I + X) * (I - X).inverse());
}
// Chart at origin
struct ChartAtOrigin {
/**
* Retract uses Cayley map. See note about xi element order in Hat.
* Deafault implementation has no Jacobian implemented
*/
static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none) {
const Matrix X = Hat(xi / 2.0);
size_t n = AmbientDim(xi.size());
const auto I = Eigen::MatrixXd::Identity(n, n);
return SO((I + X) * (I - X).inverse());
}
/**
* Inverse of Retract. See note about xi element order in Hat.
*/
static TangentVector Local(const SO& R, ChartJacobian H = boost::none) {
const size_t n = R.rows();
const auto I = Eigen::MatrixXd::Identity(n, n);
const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse();
return -2 * Vee(X);
}
};
/**
* Inverse of Retract. See note about xi element order in Hat.
*/
static Vector Local(const SOn& R) {
const size_t n = R.rows();
const auto I = Eigen::MatrixXd::Identity(n, n);
const Matrix X = (I - R) * (I + R).inverse();
return -2 * Vee(X);
// Return dynamic identity DxD Jacobian for given SO(n)
template <int N_ = N, typename = IsDynamic<N_>>
static MatrixDD IdentityJacobian(size_t n) {
const size_t d = Dimension(n);
return MatrixDD::Identity(d, d);
}
/// @}
/// @name Lie group
/// @name Lie Group
/// @{
MatrixDD AdjointMap() const {
throw std::runtime_error(
"SO<N>::AdjointMap only implemented for SO3 and SO4.");
}
/**
* Exponential map at identity - create a rotation from canonical coordinates
*/
static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none) {
throw std::runtime_error("SO<N>::Expmap only implemented for SO3 and SO4.");
}
/**
* Log map at identity - returns the canonical coordinates of this rotation
*/
static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none) {
throw std::runtime_error("SO<N>::Logmap only implemented for SO3 and SO4.");
}
// inverse with optional derivative
using LieGroup<SO<N>, internal::DimensionSO(N)>::inverse;
/// @}
/// @name Other methods
/// @{
/// Return matrix (for wrapper)
const Matrix& matrix() const { return *this; }
/**
* Return vectorized rotation matrix in column order.
* Will use dynamic matrices as intermediate results, but returns a fixed size
* X and fixed-size Jacobian if dimension is known at compile time.
* */
Vector vec(OptionalJacobian<-1, -1> H = boost::none) const {
const size_t n = rows(), n2 = n * n;
const size_t d = (n2 - n) / 2; // manifold dimension
// Calculate G matrix of vectorized generators
Matrix G(n2, d);
for (size_t j = 0; j < d; j++) {
// TODO(frank): this can't be right. Think about fixed vs dynamic.
const auto X = Hat(n, Eigen::VectorXd::Unit(d, j));
G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
}
VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
boost::none) const {
const size_t n = rows();
const size_t n2 = n * n;
// Vectorize
Vector X(n2);
X << Eigen::Map<const Matrix>(data(), n2, 1);
VectorN2 X(n2);
X << Eigen::Map<const Matrix>(matrix_.data(), n2, 1);
// If requested, calculate H as (I \oplus Q) * P
if (H) {
// Calculate P matrix of vectorized generators
const size_t d = dim();
Matrix P(n2, d);
for (size_t j = 0; j < d; j++) {
const auto X = Hat(Eigen::VectorXd::Unit(d, j));
P.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
}
H->resize(n2, d);
for (size_t i = 0; i < n; i++) {
H->block(i * n, 0, n, d) = derived() * G.block(i * n, 0, n, d);
H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);
}
}
return X;
}
/// @}
};
template <>
struct traits<SOn> {
typedef manifold_tag structure_category;
/*
* Fully specialize compose and between, because the derivative is unknowable by
* the LieGroup implementations, who return a fixed-size matrix for H2.
*/
/// @name Testable
/// @{
static void Print(SOn R, const std::string& str = "") {
gtsam::print(R, str);
}
static bool Equals(SOn R1, SOn R2, double tol = 1e-8) {
return equal_with_abs_tol(R1, R2, tol);
}
/// @}
using SOn = SO<Eigen::Dynamic>;
/// @name Manifold
/// @{
enum { dimension = Eigen::Dynamic };
typedef Eigen::VectorXd TangentVector;
// typedef Eigen::MatrixXd Jacobian;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
// typedef SOn ManifoldType;
/*
* Define the traits. internal::LieGroup provides both Lie group and Testable
*/
/**
* Calculate manifold dimension, e.g.,
* n = 3 -> 3*2/2 = 3
* n = 4 -> 4*3/2 = 6
* n = 5 -> 5*4/2 = 10
*/
static int GetDimension(const SOn& R) {
const size_t n = R.rows();
return n * (n - 1) / 2;
}
template <int N>
struct traits<SO<N>> : public internal::LieGroup<SO<N>> {};
// static Jacobian Eye(const SOn& R) {
// int dim = GetDimension(R);
// return Eigen::Matrix<double, dimension, dimension>::Identity(dim,
// dim);
// }
static SOn Retract(const SOn& R, const TangentVector& xi, //
ChartJacobian H1 = boost::none,
ChartJacobian H2 = boost::none) {
if (H1 || H2) throw std::runtime_error("SOn::Retract");
const size_t n = R.rows();
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");
const SOn between = SOn(R.inverse() * other);
return SOn::Local(between);
}
/// @}
};
template <int N>
struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
} // namespace gtsam

281
gtsam/geometry/SOt.cpp Normal file
View File

@ -0,0 +1,281 @@
/* ----------------------------------------------------------------------------
* 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 SO3.cpp
* @brief 3*3 matrix representation of SO(3)
* @author Frank Dellaert
* @author Luca Carlone
* @author Duy Nguyen Ta
* @date December 2014
*/
#include <gtsam/base/concepts.h>
#include <gtsam/geometry/SOt.h>
#include <Eigen/SVD>
#include <cmath>
#include <iostream>
#include <limits>
namespace gtsam {
//******************************************************************************
namespace sot {
Matrix99 Dcompose(const SO3& Q) {
Matrix99 H;
auto R = Q.matrix();
H << I_3x3 * R(0, 0), I_3x3 * R(1, 0), I_3x3 * R(2, 0), //
I_3x3 * R(0, 1), I_3x3 * R(1, 1), I_3x3 * R(2, 1), //
I_3x3 * R(0, 2), I_3x3 * R(1, 2), I_3x3 * R(2, 2);
return H;
}
Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
Matrix3 MR = M * R.matrix();
if (H) *H = Dcompose(R);
return MR;
}
void ExpmapFunctor::init(bool nearZeroApprox) {
nearZero =
nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
if (!nearZero) {
theta = std::sqrt(theta2); // rotation angle
sin_theta = std::sin(theta);
const double s2 = std::sin(theta / 2.0);
one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
}
}
ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
: theta2(omega.dot(omega)) {
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
init(nearZeroApprox);
if (!nearZero) {
K = W / theta;
KK = K * K;
}
}
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
bool nearZeroApprox)
: theta2(angle * angle) {
const double ax = axis.x(), ay = axis.y(), az = axis.z();
K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
W = K * angle;
init(nearZeroApprox);
if (!nearZero) {
KK = K * K;
}
}
SO3 ExpmapFunctor::expmap() const {
if (nearZero)
return SO3(I_3x3 + W);
else
return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
}
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
if (nearZero)
dexp_ = I_3x3 - 0.5 * W;
else {
a = one_minus_cos / theta;
b = 1.0 - sin_theta / theta;
dexp_ = I_3x3 - a * K + b * KK;
}
}
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
if (H1) {
if (nearZero) {
*H1 = 0.5 * skewSymmetric(v);
} else {
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
const Vector3 Kv = K * v;
const double Da = (sin_theta - 2.0 * a) / theta2;
const double Db = (one_minus_cos - 3.0 * b) / theta2;
*H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
skewSymmetric(Kv * b / theta) +
(a * I_3x3 - b * K) * skewSymmetric(v / theta);
}
}
if (H2) *H2 = dexp_;
return dexp_ * v;
}
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 invDexp = dexp_.inverse();
const Vector3 c = invDexp * v;
if (H1) {
Matrix3 D_dexpv_omega;
applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping
*H1 = -invDexp * D_dexpv_omega;
}
if (H2) *H2 = invDexp;
return c;
}
} // namespace sot
/* ************************************************************************* */
// SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
// return sot::ExpmapFunctor(axis, theta).expmap();
// }
/* ************************************************************************* */
// SO3 SO3::ClosestTo(const Matrix3& M) {
// Eigen::JacobiSVD<Matrix3> svd(M, Eigen::ComputeFullU |
// Eigen::ComputeFullV); const auto& U = svd.matrixU(); const auto& V =
// svd.matrixV(); const double det = (U * V.transpose()).determinant(); return
// U * Vector3(1, 1, det).asDiagonal() * V.transpose();
// }
/* ************************************************************************* */
// SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
// // See Hartley13ijcv:
// // Cost function C(R) = \sum sqr(|R-R_i|_F)
// // Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!!
// Matrix3 C_e{Z_3x3};
// for (const auto& R_i : rotations) {
// C_e += R_i;
// }
// return ClosestTo(C_e);
// }
//******************************************************************************
// Matrix3 SO3::Hat(const Vector3& xi) { return skewSymmetric(xi); }
// /* *************************************************************************
// */ Vector3 SO3::Vee(const Matrix3& X) {
// Vector3 xi;
// xi(0) = -X(1, 2);
// xi(1) = X(0, 2);
// xi(2) = -X(0, 1);
// return xi;
// }
template <>
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) {
sot::DexpFunctor impl(omega);
*H = impl.dexp();
return impl.expmap();
} else {
return sot::ExpmapFunctor(omega).expmap();
}
}
// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
// return sot::DexpFunctor(omega).dexp();
// }
/* ************************************************************************* */
// Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
// using std::sin;
// using std::sqrt;
// // note switch to base 1
// const double &R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
// const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
// const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
// // Get trace(R)
// const double tr = R.trace();
// Vector3 omega;
// // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// // we do something special
// if (std::abs(tr + 1.0) < 1e-10) {
// if (std::abs(R33 + 1.0) > 1e-10)
// omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
// else if (std::abs(R22 + 1.0) > 1e-10)
// omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
// else
// // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit
// omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
// } else {
// double magnitude;
// const double tr_3 = tr - 3.0; // always negative
// if (tr_3 < -1e-7) {
// double theta = acos((tr - 1.0) / 2.0);
// magnitude = theta / (2.0 * sin(theta));
// } else {
// // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
// magnitude = 0.5 - tr_3 * tr_3 / 12.0;
// }
// omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
// }
// if (H) *H = LogmapDerivative(omega);
// return omega;
// }
/* ************************************************************************* */
// Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
// using std::cos;
// using std::sin;
// double theta2 = omega.dot(omega);
// if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
// double theta = std::sqrt(theta2); // rotation angle
// /** Right Jacobian for Log map in SO(3) - equation (10.86) and following
// * equations in G.S. Chirikjian, "Stochastic Models, Information Theory,
// and
// * Lie Groups", Volume 2, 2008. logmap( Rhat * expmap(omega) ) \approx
// logmap(
// * Rhat ) + Jrinv * omega where Jrinv = LogmapDerivative(omega); This maps
// a
// * perturbation on the manifold (expmap(omega)) to a perturbation in the
// * tangent space (Jrinv * omega)
// */
// const Matrix3 W =
// skewSymmetric(omega); // element of Lie algebra so(3): W = omega^
// return I_3x3 + 0.5 * W +
// (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta)))
// *
// W * W;
// }
/* ************************************************************************* */
// static Vector9 vec(const SO3& R) { return Eigen::Map<const
// Vector9>(R.data()); }
// static const std::vector<const Matrix3> G({SO3::Hat(Vector3::Unit(0)),
// SO3::Hat(Vector3::Unit(1)),
// SO3::Hat(Vector3::Unit(2))});
// static const Matrix93 P =
// (Matrix93() << vec(G[0]), vec(G[1]), vec(G[2])).finished();
/* ************************************************************************* */
// Vector9 SO3::vec(OptionalJacobian<9, 3> H) const {
// const SO3& R = *this;
// if (H) {
// // As Luca calculated (for SO4), this is (I3 \oplus R) * P
// *H << R * P.block<3, 3>(0, 0), R * P.block<3, 3>(3, 0),
// R * P.block<3, 3>(6, 0);
// }
// return gtsam::vec(R);
// };
/* ************************************************************************* */
} // end namespace gtsam

204
gtsam/geometry/SOt.h Normal file
View File

@ -0,0 +1,204 @@
/* ----------------------------------------------------------------------------
* 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 SO3.h
* @brief 3*3 matrix representation of SO(3)
* @author Frank Dellaert
* @author Luca Carlone
* @author Duy Nguyen Ta
* @date December 2014
*/
#pragma once
#include <gtsam/geometry/SOn.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h>
#include <cmath>
#include <iosfwd>
#include <vector>
namespace gtsam {
using SO3 = SO<3>;
// /// Static, named constructor that finds SO(3) matrix closest to M in
// Frobenius norm. static SO3 ClosestTo(const Matrix3& M);
// /// Static, named constructor that finds chordal mean = argmin_R \sum
// sqr(|R-R_i|_F). static SO3 ChordalMean(const std::vector<SO3>& rotations);
// static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix
// static Vector3 Vee(const Matrix3 &X); ///< inverse of Hat
//******************************************************************************
template <>
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
// template<>
// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
// return sot::DexpFunctor(omega).dexp();
// }
/**
* Exponential map at identity - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
*/
// static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
/// Derivative of Expmap
// static Matrix3 ExpmapDerivative(const Vector3& omega);
/**
* Log map at identity - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation
*/
// static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
/// Derivative of Logmap
// static Matrix3 LogmapDerivative(const Vector3& omega);
// Matrix3 AdjointMap() const {
// return *this;
// }
// // Chart at origin
// struct ChartAtOrigin {
// static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) {
// return Expmap(omega, H);
// }
// static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) {
// return Logmap(R, H);
// }
// };
//******************************************************************************
static Vector9 vec3(const Matrix3& R) {
return Eigen::Map<const Vector9>(R.data());
}
static const std::vector<const Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
SO3::Hat(Vector3::Unit(1)),
SO3::Hat(Vector3::Unit(2))});
static const Matrix93 P3 =
(Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished();
//******************************************************************************
template <>
Vector9 SO3::vec(OptionalJacobian<9, 3> H) const {
const Matrix3& R = matrix_;
if (H) {
// As Luca calculated (for SO4), this is (I3 \oplus R) * P3
*H << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0),
R * P3.block<3, 3>(6, 0);
}
return gtsam::vec3(R);
}
// private:
// /** Serialization function */
// friend class boost::serialization::access;
// template<class ARCHIVE>
// void serialize(ARCHIVE & ar, const unsigned int /*version*/)
// {
// ar & boost::serialization::make_nvp("R11", (*this)(0,0));
// ar & boost::serialization::make_nvp("R12", (*this)(0,1));
// ar & boost::serialization::make_nvp("R13", (*this)(0,2));
// ar & boost::serialization::make_nvp("R21", (*this)(1,0));
// ar & boost::serialization::make_nvp("R22", (*this)(1,1));
// ar & boost::serialization::make_nvp("R23", (*this)(1,2));
// ar & boost::serialization::make_nvp("R31", (*this)(2,0));
// ar & boost::serialization::make_nvp("R32", (*this)(2,1));
// ar & boost::serialization::make_nvp("R33", (*this)(2,2));
// }
namespace sot {
/**
* Compose general matrix with an SO(3) element.
* We only provide the 9*9 derivative in the first argument M.
*/
Matrix3 compose(const Matrix3& M, const SO3& R,
OptionalJacobian<9, 9> H = boost::none);
/// (constant) Jacobian of compose wrpt M
Matrix99 Dcompose(const SO3& R);
// Below are two functors that allow for saving computation when exponential map
// and its derivatives are needed at the same location in so<3>. The second
// functor also implements dedicated methods to apply dexp and/or inv(dexp).
/// Functor implementing Exponential map
class GTSAM_EXPORT ExpmapFunctor {
protected:
const double theta2;
Matrix3 W, K, KK;
bool nearZero;
double theta, sin_theta, one_minus_cos; // only defined if !nearZero
void init(bool nearZeroApprox = false);
public:
/// Constructor with element of Lie algebra so(3)
explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
/// Constructor with axis-angle
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
/// Rodrigues formula
SO3 expmap() const;
};
/// Functor that implements Exponential map *and* its derivatives
class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
const Vector3 omega;
double a, b;
Matrix3 dexp_;
public:
/// Constructor with element of Lie algebra so(3)
explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
// NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
// (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
// Information Theory, and Lie Groups", Volume 2, 2008.
// expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
// This maps a perturbation v in the tangent space to
// a perturbation on the manifold Expmap(dexp * v) */
const Matrix3& dexp() const { return dexp_; }
/// Multiplies with dexp(), with optional derivatives
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// Multiplies with dexp().inverse(), with optional derivatives
Vector3 applyInvDexp(const Vector3& v,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
};
} // namespace sot
/*
* Define the traits. internal::LieGroup provides both Lie group and Testable
*/
template <>
struct traits<SO3> : public internal::LieGroup<SO3> {};
template <>
struct traits<const SO3> : public internal::LieGroup<SO3> {};
} // end namespace gtsam

674
gtsam/geometry/tests/testSOn.cpp
View File

@ -10,16 +10,25 @@
* -------------------------------------------------------------------------- */
/**
* @file testSOnBase.cpp
* @brief Unit tests for Base class of SO(n) classes.
* @file testSOn.cpp
* @brief Unit tests for dynamic SO(n) classes.
* @author Frank Dellaert
**/
#include <gtsam/geometry/SO4.h>
#include <gtsam/geometry/SOn.h>
#include <gtsam/geometry/SOt.h>
#include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
#include <gtsam/nonlinear/Values.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/random.hpp>
@ -28,325 +37,6 @@
#include <type_traits>
namespace gtsam {
namespace internal {
/// Calculate dimensionality of SO<N> manifold, or return Dynamic if so
constexpr int DimensionSO(int N) {
return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2;
}
// Calculate N^2 at compile time, or return Dynamic if so
constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
} // namespace internal
/**
* Space of special orthogonal rotation matrices SO<N>
*/
template <int N>
class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
public:
enum { dimension = internal::DimensionSO(N) };
using MatrixNN = Eigen::Matrix<double, N, N>;
using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
protected:
MatrixNN matrix_; ///< Rotation matrix
// enable_if_t aliases, used to specialize constructors/methods, see
// https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty-sfinae/
template <int N_>
using IsDynamic = boost::enable_if_t<N_ == Eigen::Dynamic, void>;
template <int N_>
using IsFixed = boost::enable_if_t<N_ >= 2, void>;
template <int N_>
using IsSO3 = boost::enable_if_t<N_ == 3, void>;
public:
/// @name Constructors
/// @{
/// Construct SO<N> identity for N >= 2
template <int N_ = N, typename = IsFixed<N_>>
SO() : matrix_(MatrixNN::Identity()) {}
/// Construct SO<N> identity for N == Eigen::Dynamic
template <int N_ = N, typename = IsDynamic<N_>>
explicit SO(size_t n = 0) {
if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
matrix_ = Eigen::MatrixXd::Identity(n, n);
}
/// Constructor from Eigen Matrix
template <typename Derived>
explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
/// Constructor from AngleAxisd
template <int N_ = N, typename = IsSO3<N_>>
SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {}
/// Random SO(n) element (no big claims about uniformity)
template <int N_ = N, typename = IsDynamic<N_>>
static SO Random(boost::mt19937& rng, size_t n = 0) {
if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
// This needs to be re-thought!
static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
const size_t d = SO::Dimension(n);
Vector xi(d);
for (size_t j = 0; j < d; j++) {
xi(j) = randomAngle(rng);
}
return SO::Retract(xi);
}
/// Random SO(N) element (no big claims about uniformity)
template <int N_ = N, typename = IsFixed<N_>>
static SO Random(boost::mt19937& rng) {
// By default, use dynamic implementation above. Specialized for SO(3).
return SO(SO<Eigen::Dynamic>::Random(rng, N).matrix());
}
/// @}
/// @name Standard methods
/// @{
/// Return matrix
const MatrixNN& matrix() const { return matrix_; }
size_t rows() const { return matrix_.rows(); }
size_t cols() const { return matrix_.cols(); }
/// @}
/// @name Testable
/// @{
void print(const std::string& s) const {
std::cout << s << matrix_ << std::endl;
}
bool equals(const SO& other, double tol) const {
return equal_with_abs_tol(matrix_, other.matrix_, tol);
}
/// @}
/// @name Group
/// @{
/// Multiplication
SO operator*(const SO& other) const { return SO(matrix_ * other.matrix_); }
/// SO<N> identity for N >= 2
template <int N_ = N, typename = IsFixed<N_>>
static SO identity() {
return SO();
}
/// SO<N> identity for N == Eigen::Dynamic
template <int N_ = N, typename = IsDynamic<N_>>
static SO identity(size_t n = 0) {
return SO(n);
}
/// inverse of a rotation = transpose
SO inverse() const { return SO(matrix_.transpose()); }
/// @}
/// @name Manifold
/// @{
using TangentVector = Eigen::Matrix<double, dimension, 1>;
using ChartJacobian = OptionalJacobian<dimension, dimension>;
/// Return compile-time dimensionality: fixed size N or Eigen::Dynamic
static int Dim() { return dimension; }
// Calculate manifold dimensionality for SO(n).
// Available as dimension or Dim() for fixed N.
static size_t Dimension(size_t n) { return n * (n - 1) / 2; }
// Calculate ambient dimension n from manifold dimensionality d.
static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; }
// Calculate run-time dimensionality of manifold.
// Available as dimension or Dim() for fixed N.
size_t dim() const { return Dimension(matrix_.rows()); }
/**
* Hat operator creates Lie algebra element corresponding to d-vector, where d
* is the dimensionality of the manifold. This function is implemented
* recursively, and the d-vector is assumed to laid out such that the last
* element corresponds to so(2), the last 3 to so(3), the last 6 to so(4)
* etc... For example, the vector-space isomorphic to so(5) is laid out as:
* a b c d | u v w | x y | z
* where the latter elements correspond to "telescoping" sub-algebras:
* 0 -z y -w d
* z 0 -x v -c
* -y x 0 -u b
* w -v u 0 -a
* -d c -b a 0
* This scheme behaves exactly as expected for SO(2) and SO(3).
*/
static Matrix Hat(const Vector& xi) {
size_t n = AmbientDim(xi.size());
if (n < 2) throw std::invalid_argument("SOn::Hat: n<2 not supported");
Matrix X(n, n); // allocate space for n*n skew-symmetric matrix
X.setZero();
if (n == 2) {
// Handle SO(2) case as recursion bottom
assert(xi.size() == 1);
X << 0, -xi(0), xi(0), 0;
} else {
// Recursively call SO(n-1) call for top-left block
const size_t dmin = (n - 1) * (n - 2) / 2;
X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin));
// Now fill last row and column
double sign = 1.0;
for (size_t i = 0; i < n - 1; i++) {
const size_t j = n - 2 - i;
X(n - 1, j) = sign * xi(i);
X(j, n - 1) = -X(n - 1, j);
sign = -sign;
}
}
return X;
}
/**
* Inverse of Hat. See note about xi element order in Hat.
*/
static Vector Vee(const Matrix& X) {
const size_t n = X.rows();
if (n < 2) throw std::invalid_argument("SOn::Hat: n<2 not supported");
if (n == 2) {
// Handle SO(2) case as recursion bottom
Vector xi(1);
xi(0) = X(1, 0);
return xi;
} else {
// Calculate dimension and allocate space
const size_t d = n * (n - 1) / 2;
Vector xi(d);
// Fill first n-1 spots from last row of X
double sign = 1.0;
for (size_t i = 0; i < n - 1; i++) {
const size_t j = n - 2 - i;
xi(i) = sign * X(n - 1, j);
sign = -sign;
}
// Recursively call Vee to fill remainder of x
const size_t dmin = (n - 1) * (n - 2) / 2;
xi.tail(dmin) = Vee(X.topLeftCorner(n - 1, n - 1));
return xi;
}
}
// Chart at origin
struct ChartAtOrigin {
/**
* Retract uses Cayley map. See note about xi element order in Hat.
* Deafault implementation has no Jacobian implemented
*/
static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none) {
const Matrix X = Hat(xi / 2.0);
size_t n = AmbientDim(xi.size());
const auto I = Eigen::MatrixXd::Identity(n, n);
return SO((I + X) * (I - X).inverse());
}
/**
* Inverse of Retract. See note about xi element order in Hat.
*/
static TangentVector Local(const SO& R, ChartJacobian H = boost::none) {
const size_t n = R.rows();
const auto I = Eigen::MatrixXd::Identity(n, n);
const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse();
return -2 * Vee(X);
}
};
// Return dynamic identity DxD Jacobian for given SO(n)
template <int N_ = N, typename = IsDynamic<N_>>
static MatrixDD IdentityJacobian(size_t n) {
const size_t d = Dimension(n);
return MatrixDD::Identity(d, d);
}
/// @}
/// @name Lie Group
/// @{
MatrixDD AdjointMap() const {
throw std::runtime_error(
"SO<N>::AdjointMap only implemented for SO3 and SO4.");
}
/**
* Exponential map at identity - create a rotation from canonical coordinates
*/
static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none) {
throw std::runtime_error("SO<N>::Expmap only implemented for SO3 and SO4.");
}
/**
* Log map at identity - returns the canonical coordinates of this rotation
*/
static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none) {
throw std::runtime_error("SO<N>::Logmap only implemented for SO3 and SO4.");
}
// inverse with optional derivative
using LieGroup<SO<N>, internal::DimensionSO(N)>::inverse;
/// @}
/// @name Other methods
/// @{
/**
* Return vectorized rotation matrix in column order.
* Will use dynamic matrices as intermediate results, but returns a fixed size
* X and fixed-size Jacobian if dimension is known at compile time.
* */
VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
boost::none) const {
const size_t n = rows();
const size_t n2 = n * n;
// Vectorize
VectorN2 X(n2);
X << Eigen::Map<const Matrix>(matrix_.data(), n2, 1);
// If requested, calculate H as (I \oplus Q) * P
if (H) {
// Calculate P matrix of vectorized generators
const size_t d = dim();
Matrix P(n2, d);
for (size_t j = 0; j < d; j++) {
const auto X = Hat(Eigen::VectorXd::Unit(d, j));
P.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
}
H->resize(n2, d);
for (size_t i = 0; i < n; i++) {
H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);
}
}
return X;
}
/// @}
};
/*
* Fully specialize compose and between, because the derivative is unknowable by
* the LieGroup implementations, who return a fixed-size matrix for H2.
*/
using SO3 = SO<3>;
using SO4 = SO<4>;
using SOn = SO<Eigen::Dynamic>;
using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
template <>
@ -365,336 +55,8 @@ SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
if (H2) *H2 = SOn::IdentityJacobian(g.rows());
return result;
}
/*
* Define the traits. internal::LieGroup provides both Lie group and Testable
*/
template <int N>
struct traits<SO<N>> : public internal::LieGroup<SO<N>> {};
template <int N>
struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
namespace so3 {
/**
* Compose general matrix with an SO(3) element.
* We only provide the 9*9 derivative in the first argument M.
*/
Matrix3 compose(const Matrix3& M, const SO3& R,
OptionalJacobian<9, 9> H = boost::none);
/// (constant) Jacobian of compose wrpt M
Matrix99 Dcompose(const SO3& R);
// Below are two functors that allow for saving computation when exponential map
// and its derivatives are needed at the same location in so<3>. The second
// functor also implements dedicated methods to apply dexp and/or inv(dexp).
/// Functor implementing Exponential map
class GTSAM_EXPORT ExpmapFunctor {
protected:
const double theta2;
Matrix3 W, K, KK;
bool nearZero;
double theta, sin_theta, one_minus_cos; // only defined if !nearZero
void init(bool nearZeroApprox = false);
public:
/// Constructor with element of Lie algebra so(3)
ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
/// Constructor with axis-angle
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
/// Rodrigues formula
SO3 expmap() const;
};
/// Functor that implements Exponential map *and* its derivatives
class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor {
const Vector3 omega;
double a, b;
Matrix3 dexp_;
public:
/// Constructor with element of Lie algebra so(3)
DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
// NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
// (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
// Information Theory, and Lie Groups", Volume 2, 2008.
// expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
// This maps a perturbation v in the tangent space to
// a perturbation on the manifold Expmap(dexp * v) */
const Matrix3& dexp() const { return dexp_; }
/// Multiplies with dexp(), with optional derivatives
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
/// Multiplies with dexp().inverse(), with optional derivatives
Vector3 applyInvDexp(const Vector3& v,
OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
};
} // namespace so3
//******************************************************************************
namespace so3 {
Matrix99 Dcompose(const SO3& Q) {
Matrix99 H;
auto R = Q.matrix();
H << I_3x3 * R(0, 0), I_3x3 * R(1, 0), I_3x3 * R(2, 0), //
I_3x3 * R(0, 1), I_3x3 * R(1, 1), I_3x3 * R(2, 1), //
I_3x3 * R(0, 2), I_3x3 * R(1, 2), I_3x3 * R(2, 2);
return H;
}
Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
Matrix3 MR = M * R.matrix();
if (H) *H = Dcompose(R);
return MR;
}
void ExpmapFunctor::init(bool nearZeroApprox) {
nearZero =
nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
if (!nearZero) {
theta = std::sqrt(theta2); // rotation angle
sin_theta = std::sin(theta);
const double s2 = std::sin(theta / 2.0);
one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)]
}
}
ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
: theta2(omega.dot(omega)) {
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
init(nearZeroApprox);
if (!nearZero) {
K = W / theta;
KK = K * K;
}
}
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
bool nearZeroApprox)
: theta2(angle * angle) {
const double ax = axis.x(), ay = axis.y(), az = axis.z();
K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
W = K * angle;
init(nearZeroApprox);
if (!nearZero) {
KK = K * K;
}
}
SO3 ExpmapFunctor::expmap() const {
if (nearZero)
return SO3(I_3x3 + W);
else
return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
}
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
if (nearZero)
dexp_ = I_3x3 - 0.5 * W;
else {
a = one_minus_cos / theta;
b = 1.0 - sin_theta / theta;
dexp_ = I_3x3 - a * K + b * KK;
}
}
Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
if (H1) {
if (nearZero) {
*H1 = 0.5 * skewSymmetric(v);
} else {
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
const Vector3 Kv = K * v;
const double Da = (sin_theta - 2.0 * a) / theta2;
const double Db = (one_minus_cos - 3.0 * b) / theta2;
*H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() -
skewSymmetric(Kv * b / theta) +
(a * I_3x3 - b * K) * skewSymmetric(v / theta);
}
}
if (H2) *H2 = dexp_;
return dexp_ * v;
}
Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
OptionalJacobian<3, 3> H2) const {
const Matrix3 invDexp = dexp_.inverse();
const Vector3 c = invDexp * v;
if (H1) {
Matrix3 D_dexpv_omega;
applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping
*H1 = -invDexp * D_dexpv_omega;
}
if (H2) *H2 = invDexp;
return c;
}
} // namespace so3
//******************************************************************************
template <>
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) {
so3::DexpFunctor impl(omega);
*H = impl.dexp();
return impl.expmap();
} else
return so3::ExpmapFunctor(omega).expmap();
}
// template<>
// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
// return so3::DexpFunctor(omega).dexp();
// }
//******************************************************************************
static Vector9 vec3(const Matrix3& R) {
return Eigen::Map<const Vector9>(R.data());
}
static const std::vector<const Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
SO3::Hat(Vector3::Unit(1)),
SO3::Hat(Vector3::Unit(2))});
static const Matrix93 P3 =
(Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished();
//******************************************************************************
template <>
Vector9 SO3::vec(OptionalJacobian<9, 3> H) const {
const Matrix3& R = matrix_;
if (H) {
// As Luca calculated (for SO4), this is (I3 \oplus R) * P3
*H << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0),
R * P3.block<3, 3>(6, 0);
}
return gtsam::vec3(R);
};
//******************************************************************************
/* Exponential map, porting MATLAB implementation by Luca, which follows
* "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
* Ramona-Andreaa Rohan */
template <>
SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
using namespace std;
if (H) throw std::runtime_error("SO4::Expmap Jacobian");
// skew symmetric matrix X = xi^
const Matrix4 X = Hat(xi);
// do eigen-decomposition
auto eig = Eigen::EigenSolver<Matrix4>(X);
Eigen::Vector4cd e = eig.eigenvalues();
using std::abs;
sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
return abs(a.imag()) > abs(b.imag());
});
// Get a and b from eigenvalues +/i ai and +/- bi
double a = e[0].imag(), b = e[2].imag();
if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
throw runtime_error("SO4::Expmap: wrong eigenvalues.");
}
// Build expX = exp(xi^)
Matrix4 expX;
using std::cos;
using std::sin;
const auto X2 = X * X;
const auto X3 = X2 * X;
double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
if (a != 0 && b == 0) {
double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
return SO4(I_4x4 + X + c2 * X2 + c3 * X3);
} else if (a == b && b != 0) {
double sin_a = sin(a), cos_a = cos(a);
double c0 = (a * sin_a + 2 * cos_a) / 2,
c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a),
c3 = (sin_a - a * cos_a) / (2 * a3);
return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
} else if (a != b) {
double sin_a = sin(a), cos_a = cos(a);
double sin_b = sin(b), cos_b = cos(b);
double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2),
c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)),
c2 = (cos_a - cos_b) / (b2 - a2),
c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2));
return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
} else {
return SO4();
}
}
//******************************************************************************
static SO4::VectorN2 vec4(const Matrix4& Q) {
return Eigen::Map<const SO4::VectorN2>(Q.data());
}
static const std::vector<const Matrix4> G4(
{SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)),
SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});
static const Eigen::Matrix<double, 16, 6> P4 =
(Eigen::Matrix<double, 16, 6>() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]),
vec4(G4[3]), vec4(G4[4]), vec4(G4[5]))
.finished();
//******************************************************************************
template <>
Matrix6 SO4::AdjointMap() const {
// Elaborate way of calculating the AdjointMap
// TODO(frank): find a closed form solution. In SO(3) is just R :-/
const Matrix4& Q = matrix_;
const Matrix4 Qt = Q.transpose();
Matrix6 A;
for (size_t i = 0; i < 6; i++) {
// Calculate column i of linear map for coeffcient of Gi
A.col(i) = SO4::Vee(Q * G4[i] * Qt);
}
return A;
}
//******************************************************************************
template <>
SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const {
const Matrix& Q = matrix_;
if (H) {
// As Luca calculated, this is (I4 \oplus Q) * P4
*H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0),
Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0);
}
return gtsam::vec4(Q);
}
} // namespace gtsam
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/testLie.h>
// #include <gtsam/geometry/SO3.h>
// #include <gtsam/geometry/SO4.h>
// #include <gtsam/geometry/SOn.h>
#include <gtsam/nonlinear/Values.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
@ -1130,11 +492,11 @@ TEST(SO3, ApplyDexp) {
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
return sot::DexpFunctor(omega, nearZeroApprox).applyDexp(v);
};
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
so3::DexpFunctor local(omega, nearZeroApprox);
sot::DexpFunctor local(omega, nearZeroApprox);
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Vector3(0.4, 0.3, 0.2)}) {
EXPECT(assert_equal(Vector3(local.dexp() * v),
@ -1153,11 +515,11 @@ TEST(SO3, ApplyInvDexp) {
for (bool nearZeroApprox : {true, false}) {
boost::function<Vector3(const Vector3&, const Vector3&)> f =
[=](const Vector3& omega, const Vector3& v) {
return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
return sot::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v);
};
for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0),
Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) {
so3::DexpFunctor local(omega, nearZeroApprox);
sot::DexpFunctor local(omega, nearZeroApprox);
Matrix invDexp = local.dexp().inverse();
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
Vector3(0.4, 0.3, 0.2)}) {
@ -1189,10 +551,10 @@ TEST(SO3, vec) {
// SO3 R = SO3::Expmap(Vector3(1, 2, 3));
// const Matrix3 expected = M * R.matrix();
// Matrix actualH;
// const Matrix3 actual = so3::compose(M, R, actualH);
// const Matrix3 actual = sot::compose(M, R, actualH);
// CHECK(assert_equal(expected, actual));
// boost::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
// return so3::compose(M, R);
// return sot::compose(M, R);
// };
// Matrix numericalH = numericalDerivative11(f, M, 1e-2);
// CHECK(assert_equal(numericalH, actualH));