Moved code into appropriate headers (SOt instead of SO3 for now)
Frank Dellaert
Fan Jiang
parent
96392c74c0
commit
6cff36975f
|
|
@ -20,8 +20,6 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <gtsam/geometry/SOn.h>
|
|
||||||
|
|
||||||
#include <gtsam/base/Lie.h>
|
#include <gtsam/base/Lie.h>
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
|
|
||||||
|
|
@ -35,7 +33,7 @@ namespace gtsam {
|
||||||
* We guarantee (all but first) constructors only generate from sub-manifold.
|
* We guarantee (all but first) constructors only generate from sub-manifold.
|
||||||
* However, round-off errors in repeated composition could move off it...
|
* 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:
|
public:
|
||||||
enum { N = 3 };
|
enum { N = 3 };
|
||||||
enum { dimension = 3 };
|
enum { dimension = 3 };
|
||||||
|
|
|
||||||
|
|
@ -31,196 +31,199 @@ using namespace std;
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/* ************************************************************************* */
|
// /* ************************************************************************* */
|
||||||
static Vector3 randomOmega(boost::mt19937 &rng) {
|
// static Vector3 randomOmega(boost::mt19937 &rng) {
|
||||||
static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
|
// static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
|
||||||
return Unit3::Random(rng).unitVector() * randomAngle(rng);
|
// return Unit3::Random(rng).unitVector() * randomAngle(rng);
|
||||||
}
|
// }
|
||||||
|
|
||||||
/* ************************************************************************* */
|
// /* ************************************************************************* */
|
||||||
// Create random SO(4) element using direct product of lie algebras.
|
// // Create random SO(4) element using direct product of lie algebras.
|
||||||
SO4 SO4::Random(boost::mt19937 &rng) {
|
// SO4 SO4::Random(boost::mt19937 &rng) {
|
||||||
Vector6 delta;
|
// Vector6 delta;
|
||||||
delta << randomOmega(rng), randomOmega(rng);
|
// delta << randomOmega(rng), randomOmega(rng);
|
||||||
return SO4::Expmap(delta);
|
// 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 {
|
// bool SO4::equals(const SO4 &R, double tol) const {
|
||||||
return equal_with_abs_tol(*this, R, tol);
|
// return equal_with_abs_tol(*this, R, tol);
|
||||||
}
|
// }
|
||||||
|
|
||||||
//******************************************************************************
|
// //******************************************************************************
|
||||||
Matrix4 SO4::Hat(const Vector6 &xi) {
|
// Matrix4 SO4::Hat(const Vector6 &xi) {
|
||||||
// skew symmetric matrix X = xi^
|
// // skew symmetric matrix X = xi^
|
||||||
// Unlike Luca, makes upper-left the SO(3) subgroup.
|
// // Unlike Luca, makes upper-left the SO(3) subgroup.
|
||||||
// See also
|
// // See also
|
||||||
// http://scipp.ucsc.edu/~haber/archives/physics251_13/groups13_sol4.pdf
|
// // http://scipp.ucsc.edu/~haber/archives/physics251_13/groups13_sol4.pdf
|
||||||
Matrix4 Y = Z_4x4;
|
// Matrix4 Y = Z_4x4;
|
||||||
Y(0, 1) = -xi(2);
|
// Y(0, 1) = -xi(2);
|
||||||
Y(0, 2) = +xi(1);
|
// Y(0, 2) = +xi(1);
|
||||||
Y(1, 2) = -xi(0);
|
// Y(1, 2) = -xi(0);
|
||||||
Y(0, 3) = -xi(3);
|
// Y(0, 3) = -xi(3);
|
||||||
Y(1, 3) = -xi(4);
|
// Y(1, 3) = -xi(4);
|
||||||
Y(2, 3) = -xi(5);
|
// Y(2, 3) = -xi(5);
|
||||||
return Y - Y.transpose();
|
// return Y - Y.transpose();
|
||||||
}
|
// }
|
||||||
/* ************************************************************************* */
|
// /* ************************************************************************* */
|
||||||
Vector6 SO4::Vee(const Matrix4 &X) {
|
// Vector6 SO4::Vee(const Matrix4 &X) {
|
||||||
Vector6 xi;
|
// Vector6 xi;
|
||||||
xi(2) = -X(0, 1);
|
// xi(2) = -X(0, 1);
|
||||||
xi(1) = X(0, 2);
|
// xi(1) = X(0, 2);
|
||||||
xi(0) = -X(1, 2);
|
// xi(0) = -X(1, 2);
|
||||||
xi(3) = -X(0, 3);
|
// xi(3) = -X(0, 3);
|
||||||
xi(4) = -X(1, 3);
|
// xi(4) = -X(1, 3);
|
||||||
xi(5) = -X(2, 3);
|
// xi(5) = -X(2, 3);
|
||||||
return xi;
|
// return xi;
|
||||||
}
|
// }
|
||||||
|
|
||||||
//******************************************************************************
|
// //******************************************************************************
|
||||||
/* Exponential map, porting MATLAB implementation by Luca, which follows
|
// /* Exponential map, porting MATLAB implementation by Luca, which follows
|
||||||
* "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
|
// * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
|
||||||
* Ramona-Andreaa Rohan */
|
// * Ramona-Andreaa Rohan */
|
||||||
SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H) {
|
// template <>
|
||||||
if (H) throw std::runtime_error("SO4::Expmap Jacobian");
|
// SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
|
||||||
gttic(SO4_Expmap);
|
// using namespace std;
|
||||||
|
// if (H) throw std::runtime_error("SO4::Expmap Jacobian");
|
||||||
|
|
||||||
// skew symmetric matrix X = xi^
|
// // skew symmetric matrix X = xi^
|
||||||
const Matrix4 X = Hat(xi);
|
// const Matrix4 X = Hat(xi);
|
||||||
|
|
||||||
// do eigen-decomposition
|
// // do eigen-decomposition
|
||||||
auto eig = Eigen::EigenSolver<Matrix4>(X);
|
// auto eig = Eigen::EigenSolver<Matrix4>(X);
|
||||||
Eigen::Vector4cd e = eig.eigenvalues();
|
// Eigen::Vector4cd e = eig.eigenvalues();
|
||||||
using std::abs;
|
// using std::abs;
|
||||||
sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
|
// sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
|
||||||
return abs(a.imag()) > abs(b.imag());
|
// return abs(a.imag()) > abs(b.imag());
|
||||||
});
|
// });
|
||||||
|
|
||||||
// Get a and b from eigenvalues +/i ai and +/- bi
|
// // Get a and b from eigenvalues +/i ai and +/- bi
|
||||||
double a = e[0].imag(), b = e[2].imag();
|
// double a = e[0].imag(), b = e[2].imag();
|
||||||
if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
|
// if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
|
||||||
throw runtime_error("SO4::Expmap: wrong eigenvalues.");
|
// throw runtime_error("SO4::Expmap: wrong eigenvalues.");
|
||||||
}
|
// }
|
||||||
|
|
||||||
// Build expX = exp(xi^)
|
// // Build expX = exp(xi^)
|
||||||
Matrix4 expX;
|
// Matrix4 expX;
|
||||||
using std::cos;
|
// using std::cos;
|
||||||
using std::sin;
|
// using std::sin;
|
||||||
const auto X2 = X * X;
|
// const auto X2 = X * X;
|
||||||
const auto X3 = X2 * X;
|
// const auto X3 = X2 * X;
|
||||||
double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
|
// double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
|
||||||
if (a != 0 && b == 0) {
|
// if (a != 0 && b == 0) {
|
||||||
double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
|
// double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
|
||||||
return I_4x4 + X + c2 * X2 + c3 * X3;
|
// return SO4(I_4x4 + X + c2 * X2 + c3 * X3);
|
||||||
} else if (a == b && b != 0) {
|
// } else if (a == b && b != 0) {
|
||||||
double sin_a = sin(a), cos_a = cos(a);
|
// double sin_a = sin(a), cos_a = cos(a);
|
||||||
double c0 = (a * sin_a + 2 * cos_a) / 2,
|
// double c0 = (a * sin_a + 2 * cos_a) / 2,
|
||||||
c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a),
|
// c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a),
|
||||||
c3 = (sin_a - a * cos_a) / (2 * a3);
|
// c3 = (sin_a - a * cos_a) / (2 * a3);
|
||||||
return c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3;
|
// return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
|
||||||
} else if (a != b) {
|
// } else if (a != b) {
|
||||||
double sin_a = sin(a), cos_a = cos(a);
|
// double sin_a = sin(a), cos_a = cos(a);
|
||||||
double sin_b = sin(b), cos_b = cos(b);
|
// double sin_b = sin(b), cos_b = cos(b);
|
||||||
double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2),
|
// double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2),
|
||||||
c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)),
|
// c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)),
|
||||||
c2 = (cos_a - cos_b) / (b2 - a2),
|
// c2 = (cos_a - cos_b) / (b2 - a2),
|
||||||
c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2));
|
// c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2));
|
||||||
return c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3;
|
// return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
|
||||||
} else {
|
// } else {
|
||||||
return I_4x4;
|
// return SO4();
|
||||||
}
|
// }
|
||||||
}
|
// }
|
||||||
|
|
||||||
//******************************************************************************
|
// //******************************************************************************
|
||||||
Vector6 SO4::Logmap(const SO4 &Q, ChartJacobian H) {
|
// static SO4::VectorN2 vec4(const Matrix4& Q) {
|
||||||
if (H) throw std::runtime_error("SO4::Logmap Jacobian");
|
// return Eigen::Map<const SO4::VectorN2>(Q.data());
|
||||||
throw std::runtime_error("SO4::Logmap");
|
// }
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
// static const std::vector<const Matrix4> G4(
|
||||||
SO4 SO4::ChartAtOrigin::Retract(const Vector6 &xi, ChartJacobian H) {
|
// {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)),
|
||||||
if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
|
// SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
|
||||||
gttic(SO4_Retract);
|
// SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});
|
||||||
const Matrix4 X = Hat(xi / 2);
|
|
||||||
return (I_4x4 + X) * (I_4x4 - X).inverse();
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
// static const Eigen::Matrix<double, 16, 6> P4 =
|
||||||
Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H) {
|
// (Eigen::Matrix<double, 16, 6>() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]),
|
||||||
if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
|
// vec4(G4[3]), vec4(G4[4]), vec4(G4[5]))
|
||||||
const Matrix4 X = (I_4x4 - Q) * (I_4x4 + Q).inverse();
|
// .finished();
|
||||||
return -2 * Vee(X);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
// //******************************************************************************
|
||||||
static SO4::Vector16 vec(const SO4 &Q) {
|
// template <>
|
||||||
return Eigen::Map<const SO4::Vector16>(Q.data());
|
// 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)),
|
// template <>
|
||||||
SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
|
// SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const {
|
||||||
SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});
|
// 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 {
|
// Vector6 SO4::Logmap(const SO4 &Q, ChartJacobian H) {
|
||||||
gttic(SO4_AdjointMap);
|
// if (H) throw std::runtime_error("SO4::Logmap Jacobian");
|
||||||
// Elaborate way of calculating the AdjointMap
|
// throw std::runtime_error("SO4::Logmap");
|
||||||
// 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
// /* ************************************************************************* */
|
||||||
SO4::Vector16 SO4::vec(OptionalJacobian<16, 6> H) const {
|
// SO4 SO4::ChartAtOrigin::Retract(const Vector6 &xi, ChartJacobian H) {
|
||||||
const SO4 &Q = *this;
|
// if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
|
||||||
if (H) {
|
// gttic(SO4_Retract);
|
||||||
// As Luca calculated, this is (I4 \oplus Q) * P
|
// const Matrix4 X = Hat(xi / 2);
|
||||||
*H << Q * P.block<4, 6>(0, 0), Q * P.block<4, 6>(4, 0),
|
// return (I_4x4 + X) * (I_4x4 - X).inverse();
|
||||||
Q * P.block<4, 6>(8, 0), Q * P.block<4, 6>(12, 0);
|
// }
|
||||||
}
|
|
||||||
return gtsam::vec(Q);
|
|
||||||
};
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
// /* ************************************************************************* */
|
||||||
Matrix3 SO4::topLeft(OptionalJacobian<9, 6> H) const {
|
// Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H) {
|
||||||
const Matrix3 M = this->topLeftCorner<3, 3>();
|
// if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
|
||||||
const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
|
// const Matrix4 X = (I_4x4 - Q) * (I_4x4 + Q).inverse();
|
||||||
q = this->topRightCorner<3, 1>();
|
// return -2 * Vee(X);
|
||||||
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 {
|
// Matrix3 SO4::topLeft(OptionalJacobian<9, 6> H) const {
|
||||||
const Matrix43 M = this->leftCols<3>();
|
// const Matrix3 M = this->topLeftCorner<3, 3>();
|
||||||
const auto &m1 = col(0), m2 = col(1), m3 = col(2), q = col(3);
|
// const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
|
||||||
if (H) {
|
// q = this->topRightCorner<3, 1>();
|
||||||
*H << Z_4x1, -m3, m2, q, Z_4x1, Z_4x1, //
|
// if (H) {
|
||||||
m3, Z_4x1, -m1, Z_4x1, q, Z_4x1, //
|
// *H << Z_3x1, -m3, m2, q, Z_3x1, Z_3x1, //
|
||||||
-m2, m1, Z_4x1, Z_4x1, Z_4x1, q;
|
// m3, Z_3x1, -m1, Z_3x1, q, Z_3x1, //
|
||||||
}
|
// -m2, m1, Z_3x1, Z_3x1, Z_3x1, q;
|
||||||
return M;
|
// }
|
||||||
}
|
// 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
|
} // end namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -33,111 +33,155 @@
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
/**
|
using SO4 = SO<4>;
|
||||||
* 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 };
|
|
||||||
|
|
||||||
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
|
// skew symmetric matrix X = xi^
|
||||||
template <typename Derived>
|
const Matrix4 X = Hat(xi);
|
||||||
SO4(const MatrixBase<Derived> &R) : Matrix4(R.eval()) {}
|
|
||||||
|
|
||||||
/// Random SO(4) element (no big claims about uniformity)
|
// do eigen-decomposition
|
||||||
static SO4 Random(boost::mt19937 &rng);
|
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
|
||||||
/// @name Testable
|
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.");
|
||||||
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));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}; // 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 <>
|
template <>
|
||||||
struct traits<SO4> : Testable<SO4>, internal::LieGroupTraits<SO4> {};
|
struct traits<SO4> : Testable<SO4>, internal::LieGroupTraits<SO4> {};
|
||||||
|
|
|
||||||
|
|
@ -11,94 +11,168 @@
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file SOn.h
|
* @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
|
* @author Frank Dellaert
|
||||||
* @date March 2019
|
* @date March 2019
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/base/Lie.h>
|
||||||
#include <gtsam/base/Manifold.h>
|
#include <gtsam/base/Manifold.h>
|
||||||
|
|
||||||
#include <Eigen/Core>
|
#include <Eigen/Core>
|
||||||
#include <boost/random.hpp>
|
#include <boost/random.hpp>
|
||||||
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
namespace gtsam {
|
namespace gtsam {
|
||||||
|
|
||||||
namespace internal {
|
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
|
// 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
|
} // namespace internal
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Base class for rotation group objects. Template arguments:
|
* Manifold of special orthogonal rotation matrices SO<N>.
|
||||||
* Derived_: derived class
|
* Template paramater N can be a fizxed integer or can be Eigen::Dynamic
|
||||||
* 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
|
|
||||||
*/
|
*/
|
||||||
template <class Derived_>
|
template <int N>
|
||||||
class SOnBase {
|
class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
|
||||||
public:
|
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>;
|
||||||
|
|
||||||
/// @}
|
protected:
|
||||||
/// @name Manifold
|
MatrixNN matrix_; ///< Rotation matrix
|
||||||
/// @{
|
|
||||||
|
|
||||||
/// @}
|
// enable_if_t aliases, used to specialize constructors/methods, see
|
||||||
/// @name Lie Group
|
// 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:
|
public:
|
||||||
enum { N = Eigen::Dynamic };
|
|
||||||
enum { dimension = Eigen::Dynamic };
|
|
||||||
|
|
||||||
/// @name Constructors
|
/// @name Constructors
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/// Default constructor: only used for serialization and wrapping
|
/// Construct SO<N> identity for N >= 2
|
||||||
SOn() {}
|
template <int N_ = N, typename = IsFixed<N_>>
|
||||||
|
SO() : matrix_(MatrixNN::Identity()) {}
|
||||||
|
|
||||||
/// Construct SO(n) identity
|
/// Construct SO<N> identity for N == Eigen::Dynamic
|
||||||
explicit SOn(size_t n) : Eigen::MatrixXd(n, n) {
|
template <int N_ = N, typename = IsDynamic<N_>>
|
||||||
*this << Eigen::MatrixXd::Identity(n, 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
|
/// Constructor from Eigen Matrix
|
||||||
template <typename Derived_>
|
template <typename Derived>
|
||||||
explicit SOn(const Eigen::MatrixBase<Derived_>& R)
|
explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
|
||||||
: Eigen::MatrixXd(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)
|
/// 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!
|
// This needs to be re-thought!
|
||||||
static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
|
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);
|
Vector xi(d);
|
||||||
for (size_t j = 0; j < d; j++) {
|
for (size_t j = 0; j < d; j++) {
|
||||||
xi(j) = randomAngle(rng);
|
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
|
/// @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
|
* Hat operator creates Lie algebra element corresponding to d-vector, where d
|
||||||
* is the dimensionality of the manifold. This function is implemented
|
* 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
|
* -d c -b a 0
|
||||||
* This scheme behaves exactly as expected for SO(2) and SO(3).
|
* 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");
|
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
|
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 {
|
} else {
|
||||||
// Recursively call SO(n-1) call for top-left block
|
// Recursively call SO(n-1) call for top-left block
|
||||||
const size_t dmin = (n - 1) * (n - 2) / 2;
|
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
|
// Now fill last row and column
|
||||||
double sign = 1.0;
|
double sign = 1.0;
|
||||||
|
|
@ -172,126 +247,114 @@ class SOn : public Eigen::MatrixXd, public SOnBase<SOn> {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
// Chart at origin
|
||||||
* Retract uses Cayley map. See note about xi element order in Hat.
|
struct ChartAtOrigin {
|
||||||
*/
|
/**
|
||||||
static SOn Retract(size_t n, const Vector& xi) {
|
* Retract uses Cayley map. See note about xi element order in Hat.
|
||||||
const Matrix X = Hat(n, xi / 2.0);
|
* Deafault implementation has no Jacobian implemented
|
||||||
const auto I = Eigen::MatrixXd::Identity(n, n);
|
*/
|
||||||
return SOn((I + X) * (I - X).inverse());
|
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)
|
||||||
* Inverse of Retract. See note about xi element order in Hat.
|
template <int N_ = N, typename = IsDynamic<N_>>
|
||||||
*/
|
static MatrixDD IdentityJacobian(size_t n) {
|
||||||
static Vector Local(const SOn& R) {
|
const size_t d = Dimension(n);
|
||||||
const size_t n = R.rows();
|
return MatrixDD::Identity(d, d);
|
||||||
const auto I = Eigen::MatrixXd::Identity(n, n);
|
|
||||||
const Matrix X = (I - R) * (I + R).inverse();
|
|
||||||
return -2 * Vee(X);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
/// @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
|
/// @name Other methods
|
||||||
/// @{
|
/// @{
|
||||||
|
|
||||||
/// Return matrix (for wrapper)
|
|
||||||
const Matrix& matrix() const { return *this; }
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Return vectorized rotation matrix in column order.
|
* Return vectorized rotation matrix in column order.
|
||||||
* Will use dynamic matrices as intermediate results, but returns a fixed size
|
* Will use dynamic matrices as intermediate results, but returns a fixed size
|
||||||
* X and fixed-size Jacobian if dimension is known at compile time.
|
* X and fixed-size Jacobian if dimension is known at compile time.
|
||||||
* */
|
* */
|
||||||
Vector vec(OptionalJacobian<-1, -1> H = boost::none) const {
|
VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
|
||||||
const size_t n = rows(), n2 = n * n;
|
boost::none) const {
|
||||||
const size_t d = (n2 - n) / 2; // manifold dimension
|
const size_t n = rows();
|
||||||
|
const size_t n2 = n * n;
|
||||||
// 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);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Vectorize
|
// Vectorize
|
||||||
Vector X(n2);
|
VectorN2 X(n2);
|
||||||
X << Eigen::Map<const Matrix>(data(), n2, 1);
|
X << Eigen::Map<const Matrix>(matrix_.data(), n2, 1);
|
||||||
|
|
||||||
// If requested, calculate H as (I \oplus Q) * P
|
// If requested, calculate H as (I \oplus Q) * P
|
||||||
if (H) {
|
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);
|
H->resize(n2, d);
|
||||||
for (size_t i = 0; i < n; i++) {
|
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;
|
return X;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// @}
|
/// @}
|
||||||
};
|
};
|
||||||
|
|
||||||
template <>
|
/*
|
||||||
struct traits<SOn> {
|
* Fully specialize compose and between, because the derivative is unknowable by
|
||||||
typedef manifold_tag structure_category;
|
* the LieGroup implementations, who return a fixed-size matrix for H2.
|
||||||
|
*/
|
||||||
|
|
||||||
/// @name Testable
|
using SOn = SO<Eigen::Dynamic>;
|
||||||
/// @{
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
/// @}
|
|
||||||
|
|
||||||
/// @name Manifold
|
/*
|
||||||
/// @{
|
* Define the traits. internal::LieGroup provides both Lie group and Testable
|
||||||
enum { dimension = Eigen::Dynamic };
|
*/
|
||||||
typedef Eigen::VectorXd TangentVector;
|
|
||||||
// typedef Eigen::MatrixXd Jacobian;
|
|
||||||
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
|
|
||||||
// typedef SOn ManifoldType;
|
|
||||||
|
|
||||||
/**
|
template <int N>
|
||||||
* Calculate manifold dimension, e.g.,
|
struct traits<SO<N>> : public internal::LieGroup<SO<N>> {};
|
||||||
* 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
// static Jacobian Eye(const SOn& R) {
|
template <int N>
|
||||||
// int dim = GetDimension(R);
|
struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
|
||||||
// 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);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// @}
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace gtsam
|
} // namespace gtsam
|
||||||
|
|
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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
|
||||||
|
|
@ -10,16 +10,25 @@
|
||||||
* -------------------------------------------------------------------------- */
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @file testSOnBase.cpp
|
* @file testSOn.cpp
|
||||||
* @brief Unit tests for Base class of SO(n) classes.
|
* @brief Unit tests for dynamic SO(n) classes.
|
||||||
* @author Frank Dellaert
|
* @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/Lie.h>
|
||||||
#include <gtsam/base/Manifold.h>
|
#include <gtsam/base/Manifold.h>
|
||||||
#include <gtsam/base/Matrix.h>
|
#include <gtsam/base/Matrix.h>
|
||||||
#include <gtsam/base/Testable.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>
|
#include <boost/random.hpp>
|
||||||
|
|
||||||
|
|
@ -28,325 +37,6 @@
|
||||||
#include <type_traits>
|
#include <type_traits>
|
||||||
|
|
||||||
namespace gtsam {
|
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>;
|
using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
|
||||||
|
|
||||||
template <>
|
template <>
|
||||||
|
|
@ -365,336 +55,8 @@ SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
|
||||||
if (H2) *H2 = SOn::IdentityJacobian(g.rows());
|
if (H2) *H2 = SOn::IdentityJacobian(g.rows());
|
||||||
return result;
|
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
|
} // 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 std;
|
||||||
using namespace gtsam;
|
using namespace gtsam;
|
||||||
|
|
||||||
|
|
@ -1130,11 +492,11 @@ TEST(SO3, ApplyDexp) {
|
||||||
for (bool nearZeroApprox : {true, false}) {
|
for (bool nearZeroApprox : {true, false}) {
|
||||||
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||||
[=](const Vector3& omega, const Vector3& v) {
|
[=](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),
|
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)}) {
|
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),
|
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
|
||||||
Vector3(0.4, 0.3, 0.2)}) {
|
Vector3(0.4, 0.3, 0.2)}) {
|
||||||
EXPECT(assert_equal(Vector3(local.dexp() * v),
|
EXPECT(assert_equal(Vector3(local.dexp() * v),
|
||||||
|
|
@ -1153,11 +515,11 @@ TEST(SO3, ApplyInvDexp) {
|
||||||
for (bool nearZeroApprox : {true, false}) {
|
for (bool nearZeroApprox : {true, false}) {
|
||||||
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||||
[=](const Vector3& omega, const Vector3& v) {
|
[=](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),
|
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)}) {
|
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();
|
Matrix invDexp = local.dexp().inverse();
|
||||||
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
|
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1),
|
||||||
Vector3(0.4, 0.3, 0.2)}) {
|
Vector3(0.4, 0.3, 0.2)}) {
|
||||||
|
|
@ -1189,10 +551,10 @@ TEST(SO3, vec) {
|
||||||
// SO3 R = SO3::Expmap(Vector3(1, 2, 3));
|
// SO3 R = SO3::Expmap(Vector3(1, 2, 3));
|
||||||
// const Matrix3 expected = M * R.matrix();
|
// const Matrix3 expected = M * R.matrix();
|
||||||
// Matrix actualH;
|
// Matrix actualH;
|
||||||
// const Matrix3 actual = so3::compose(M, R, actualH);
|
// const Matrix3 actual = sot::compose(M, R, actualH);
|
||||||
// CHECK(assert_equal(expected, actual));
|
// CHECK(assert_equal(expected, actual));
|
||||||
// boost::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
|
// 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);
|
// Matrix numericalH = numericalDerivative11(f, M, 1e-2);
|
||||||
// CHECK(assert_equal(numericalH, actualH));
|
// CHECK(assert_equal(numericalH, actualH));
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue