Hat and Vee for SO4
Frank Dellaert
Fan Jiang
parent
ab1be9c4de
commit
a765886e5a
|
|
@ -24,20 +24,23 @@
|
|||
#include <Eigen/Eigenvalues>
|
||||
#include <boost/random.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// /* ************************************************************************* */
|
||||
// static Vector3 randomOmega(boost::mt19937 &rng) {
|
||||
// /* *************************************************************************
|
||||
// */ static Vector3 randomOmega(boost::mt19937 &rng) {
|
||||
// static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
|
||||
// return Unit3::Random(rng).unitVector() * randomAngle(rng);
|
||||
// }
|
||||
|
||||
// /* ************************************************************************* */
|
||||
// /* *************************************************************************
|
||||
// */
|
||||
// // Create random SO(4) element using direct product of lie algebras.
|
||||
// SO4 SO4::Random(boost::mt19937 &rng) {
|
||||
// Vector6 delta;
|
||||
|
|
@ -45,185 +48,179 @@ namespace gtsam {
|
|||
// return SO4::Expmap(delta);
|
||||
// }
|
||||
|
||||
// /* ************************************************************************* */
|
||||
// void SO4::print(const string &s) const { cout << s << *this << endl; }
|
||||
//******************************************************************************
|
||||
template <>
|
||||
Matrix4 SO4::Hat(const Vector6& xi) {
|
||||
// skew symmetric matrix X = xi^
|
||||
// Unlike Luca, makes upper-left the SO(3) subgroup.
|
||||
Matrix4 Y = Z_4x4;
|
||||
Y(0, 1) = -xi(5);
|
||||
Y(0, 2) = +xi(4);
|
||||
Y(1, 2) = -xi(3);
|
||||
Y(0, 3) = -xi(2);
|
||||
Y(1, 3) = +xi(1);
|
||||
Y(2, 3) = -xi(0);
|
||||
return Y - Y.transpose();
|
||||
}
|
||||
|
||||
// /* ************************************************************************* */
|
||||
// bool SO4::equals(const SO4 &R, double tol) const {
|
||||
// return equal_with_abs_tol(*this, R, tol);
|
||||
// }
|
||||
//******************************************************************************
|
||||
template <>
|
||||
Vector6 SO4::Vee(const Matrix4& X) {
|
||||
Vector6 xi;
|
||||
xi(5) = -X(0, 1);
|
||||
xi(4) = +X(0, 2);
|
||||
xi(3) = -X(1, 2);
|
||||
xi(2) = -X(0, 3);
|
||||
xi(1) = +X(1, 3);
|
||||
xi(0) = -X(2, 3);
|
||||
return xi;
|
||||
}
|
||||
|
||||
// //******************************************************************************
|
||||
// Matrix4 SO4::Hat(const Vector6 &xi) {
|
||||
// // skew symmetric matrix X = xi^
|
||||
// // Unlike Luca, makes upper-left the SO(3) subgroup.
|
||||
// // See also
|
||||
// // http://scipp.ucsc.edu/~haber/archives/physics251_13/groups13_sol4.pdf
|
||||
// Matrix4 Y = Z_4x4;
|
||||
// Y(0, 1) = -xi(2);
|
||||
// Y(0, 2) = +xi(1);
|
||||
// Y(1, 2) = -xi(0);
|
||||
// Y(0, 3) = -xi(3);
|
||||
// Y(1, 3) = -xi(4);
|
||||
// Y(2, 3) = -xi(5);
|
||||
// return Y - Y.transpose();
|
||||
// }
|
||||
// /* ************************************************************************* */
|
||||
// Vector6 SO4::Vee(const Matrix4 &X) {
|
||||
// Vector6 xi;
|
||||
// xi(2) = -X(0, 1);
|
||||
// xi(1) = X(0, 2);
|
||||
// xi(0) = -X(1, 2);
|
||||
// xi(3) = -X(0, 3);
|
||||
// xi(4) = -X(1, 3);
|
||||
// xi(5) = -X(2, 3);
|
||||
// return xi;
|
||||
// }
|
||||
//******************************************************************************
|
||||
/* Exponential map, porting MATLAB implementation by Luca, which follows
|
||||
* "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
|
||||
* Ramona-Andreaa Rohan */
|
||||
template <>
|
||||
SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
|
||||
using namespace std;
|
||||
if (H) throw std::runtime_error("SO4::Expmap Jacobian");
|
||||
|
||||
// //******************************************************************************
|
||||
// /* Exponential map, porting MATLAB implementation by Luca, which follows
|
||||
// * "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
|
||||
// * Ramona-Andreaa Rohan */
|
||||
// template <>
|
||||
// SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
|
||||
// using namespace std;
|
||||
// if (H) throw std::runtime_error("SO4::Expmap Jacobian");
|
||||
// skew symmetric matrix X = xi^
|
||||
const Matrix4 X = Hat(xi);
|
||||
|
||||
// // skew symmetric matrix X = xi^
|
||||
// const Matrix4 X = Hat(xi);
|
||||
// do eigen-decomposition
|
||||
auto eig = Eigen::EigenSolver<Matrix4>(X);
|
||||
Eigen::Vector4cd e = eig.eigenvalues();
|
||||
using std::abs;
|
||||
sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
|
||||
return abs(a.imag()) > abs(b.imag());
|
||||
});
|
||||
|
||||
// // do eigen-decomposition
|
||||
// auto eig = Eigen::EigenSolver<Matrix4>(X);
|
||||
// Eigen::Vector4cd e = eig.eigenvalues();
|
||||
// using std::abs;
|
||||
// sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
|
||||
// return abs(a.imag()) > abs(b.imag());
|
||||
// });
|
||||
// Get a and b from eigenvalues +/i ai and +/- bi
|
||||
double a = e[0].imag(), b = e[2].imag();
|
||||
if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
|
||||
throw runtime_error("SO4::Expmap: wrong eigenvalues.");
|
||||
}
|
||||
|
||||
// // Get a and b from eigenvalues +/i ai and +/- bi
|
||||
// double a = e[0].imag(), b = e[2].imag();
|
||||
// if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
|
||||
// throw runtime_error("SO4::Expmap: wrong eigenvalues.");
|
||||
// }
|
||||
// Build expX = exp(xi^)
|
||||
Matrix4 expX;
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
const auto X2 = X * X;
|
||||
const auto X3 = X2 * X;
|
||||
double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
|
||||
if (a != 0 && b == 0) {
|
||||
double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
|
||||
return 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();
|
||||
}
|
||||
}
|
||||
|
||||
// // 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();
|
||||
// }
|
||||
// }
|
||||
//******************************************************************************
|
||||
// local vectorize
|
||||
static SO4::VectorN2 vec4(const Matrix4& Q) {
|
||||
return Eigen::Map<const SO4::VectorN2>(Q.data());
|
||||
}
|
||||
|
||||
// //******************************************************************************
|
||||
// static SO4::VectorN2 vec4(const Matrix4& Q) {
|
||||
// return Eigen::Map<const SO4::VectorN2>(Q.data());
|
||||
// }
|
||||
// so<4> generators
|
||||
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 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))});
|
||||
// vectorized generators
|
||||
static const Eigen::Matrix<double, 16, 6> P4 =
|
||||
(Eigen::Matrix<double, 16, 6>() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]),
|
||||
vec4(G4[3]), vec4(G4[4]), vec4(G4[5]))
|
||||
.finished();
|
||||
|
||||
// static 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 <>
|
||||
// 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);
|
||||
}
|
||||
|
||||
// //******************************************************************************
|
||||
// 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);
|
||||
// }
|
||||
///******************************************************************************
|
||||
template <>
|
||||
SO4 SO4::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
|
||||
if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
|
||||
gttic(SO4_Retract);
|
||||
const Matrix4 X = Hat(xi / 2);
|
||||
return SO4((I_4x4 + X) * (I_4x4 - X).inverse());
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
template <>
|
||||
Vector6 SO4::ChartAtOrigin::Local(const SO4& Q, ChartJacobian H) {
|
||||
if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
|
||||
const Matrix4& R = Q.matrix();
|
||||
const Matrix4 X = (I_4x4 - R) * (I_4x4 + R).inverse();
|
||||
return -2 * Vee(X);
|
||||
}
|
||||
|
||||
// //******************************************************************************
|
||||
// Vector6 SO4::Logmap(const SO4 &Q, ChartJacobian H) {
|
||||
// if (H) throw std::runtime_error("SO4::Logmap Jacobian");
|
||||
// throw std::runtime_error("SO4::Logmap");
|
||||
// }
|
||||
//******************************************************************************
|
||||
Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) {
|
||||
const Matrix4& R = Q.matrix();
|
||||
const Matrix3 M = R.topLeftCorner<3, 3>();
|
||||
if (H) {
|
||||
const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
|
||||
q = R.topRightCorner<3, 1>();
|
||||
*H << Z_3x1, Z_3x1, q, Z_3x1, -m3, m2, //
|
||||
Z_3x1, -q, Z_3x1, m3, Z_3x1, -m1, //
|
||||
q, Z_3x1, Z_3x1, -m2, m1, Z_3x1;
|
||||
}
|
||||
return M;
|
||||
}
|
||||
|
||||
// /* ************************************************************************* */
|
||||
// SO4 SO4::ChartAtOrigin::Retract(const Vector6 &xi, ChartJacobian H) {
|
||||
// if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
|
||||
// gttic(SO4_Retract);
|
||||
// const Matrix4 X = Hat(xi / 2);
|
||||
// return (I_4x4 + X) * (I_4x4 - X).inverse();
|
||||
// }
|
||||
//******************************************************************************
|
||||
Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) {
|
||||
const Matrix4& R = Q.matrix();
|
||||
const Matrix43 M = R.leftCols<3>();
|
||||
if (H) {
|
||||
const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3);
|
||||
*H << Z_4x1, Z_4x1, q, Z_4x1, -m3, m2, //
|
||||
Z_4x1, -q, Z_4x1, m3, Z_4x1, -m1, //
|
||||
q, Z_4x1, Z_4x1, -m2, m1, Z_4x1;
|
||||
}
|
||||
return M;
|
||||
}
|
||||
|
||||
// /* ************************************************************************* */
|
||||
// Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H) {
|
||||
// if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
|
||||
// const Matrix4 X = (I_4x4 - Q) * (I_4x4 + Q).inverse();
|
||||
// return -2 * Vee(X);
|
||||
// }
|
||||
|
||||
// /* ************************************************************************* */
|
||||
// Matrix3 SO4::topLeft(OptionalJacobian<9, 6> H) const {
|
||||
// const Matrix3 M = this->topLeftCorner<3, 3>();
|
||||
// const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
|
||||
// q = this->topRightCorner<3, 1>();
|
||||
// if (H) {
|
||||
// *H << Z_3x1, -m3, m2, q, Z_3x1, Z_3x1, //
|
||||
// m3, Z_3x1, -m1, Z_3x1, q, Z_3x1, //
|
||||
// -m2, m1, Z_3x1, Z_3x1, Z_3x1, q;
|
||||
// }
|
||||
// return M;
|
||||
// }
|
||||
|
||||
// /* ************************************************************************* */
|
||||
// Matrix43 SO4::stiefel(OptionalJacobian<12, 6> H) const {
|
||||
// const Matrix43 M = this->leftCols<3>();
|
||||
// const auto &m1 = col(0), m2 = col(1), m3 = col(2), q = col(3);
|
||||
// if (H) {
|
||||
// *H << Z_4x1, -m3, m2, q, Z_4x1, Z_4x1, //
|
||||
// m3, Z_4x1, -m1, Z_4x1, q, Z_4x1, //
|
||||
// -m2, m1, Z_4x1, Z_4x1, Z_4x1, q;
|
||||
// }
|
||||
// return M;
|
||||
// }
|
||||
|
||||
// /* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
|
||||
} // end namespace gtsam
|
||||
|
|
|
|||
|
|
@ -38,120 +38,40 @@ using SO4 = SO<4>;
|
|||
// /// Random SO(4) element (no big claims about uniformity)
|
||||
// static SO4 Random(boost::mt19937 &rng);
|
||||
|
||||
// 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;
|
||||
// Below are all declarations of SO<4> specializations.
|
||||
// They are *defined* in SO4.cpp.
|
||||
|
||||
//******************************************************************************
|
||||
/* 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");
|
||||
Matrix4 SO4::Hat(const TangentVector& xi);
|
||||
|
||||
// skew symmetric matrix X = xi^
|
||||
const Matrix4 X = Hat(xi);
|
||||
|
||||
// do eigen-decomposition
|
||||
auto eig = Eigen::EigenSolver<Matrix4>(X);
|
||||
Eigen::Vector4cd e = eig.eigenvalues();
|
||||
using std::abs;
|
||||
sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
|
||||
return abs(a.imag()) > abs(b.imag());
|
||||
});
|
||||
|
||||
// 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;
|
||||
}
|
||||
Vector6 SO4::Vee(const Matrix4& X);
|
||||
|
||||
//******************************************************************************
|
||||
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);
|
||||
}
|
||||
SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H);
|
||||
|
||||
// /// Vectorize
|
||||
// Vector16 vec(OptionalJacobian<16, 6> H = boost::none) const;
|
||||
template <>
|
||||
Matrix6 SO4::AdjointMap() 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;
|
||||
template <>
|
||||
SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) 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;
|
||||
template <>
|
||||
SO4 SO4::ChartAtOrigin::Retract(const Vector6& omega, ChartJacobian H);
|
||||
|
||||
template <>
|
||||
Vector6 SO4::ChartAtOrigin::Local(const SO4& R, ChartJacobian H);
|
||||
|
||||
/**
|
||||
* Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3).
|
||||
*/
|
||||
Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H = boost::none);
|
||||
|
||||
/**
|
||||
* Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q)
|
||||
* -> S \in St(3,4).
|
||||
*/
|
||||
Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H = boost::none);
|
||||
|
||||
// private:
|
||||
// /** Serialization function */
|
||||
|
|
|
|||
|
|
@ -19,8 +19,8 @@
|
|||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/lieProxies.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/SOt.h>
|
||||
#include <gtsam/geometry/SO4.h>
|
||||
#include <gtsam/geometry/SOt.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
|
|
@ -30,7 +30,6 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
//******************************************************************************
|
||||
|
||||
TEST(SO4, Identity) {
|
||||
const SO4 R;
|
||||
EXPECT_LONGS_EQUAL(4, R.rows());
|
||||
|
|
@ -83,13 +82,32 @@ TEST(SO4, Expmap) {
|
|||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Cayley) {
|
||||
CHECK(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100)));
|
||||
CHECK(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100)));
|
||||
TEST(SO4, Hat) {
|
||||
// Check that Hat specialization is equal to dynamic version
|
||||
EXPECT(assert_equal(SO4::Hat(v1), SOn::Hat(v1)));
|
||||
EXPECT(assert_equal(SO4::Hat(v2), SOn::Hat(v2)));
|
||||
EXPECT(assert_equal(SO4::Hat(v3), SOn::Hat(v3)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Vee) {
|
||||
// Check that Hat specialization is equal to dynamic version
|
||||
auto X1 = SOn::Hat(v1), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
|
||||
EXPECT(assert_equal(SO4::Vee(X1), SOn::Vee(X1)));
|
||||
EXPECT(assert_equal(SO4::Vee(X2), SOn::Vee(X2)));
|
||||
EXPECT(assert_equal(SO4::Vee(X3), SOn::Vee(X3)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Retract) {
|
||||
// Check that Cayley yields the same as Expmap for small values
|
||||
EXPECT(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100)));
|
||||
EXPECT(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100)));
|
||||
|
||||
// Check that Cayley is identical to dynamic version
|
||||
EXPECT(assert_equal(SOn(id.retract(v1 / 100)), SOn(4).retract(v1 / 100)));
|
||||
EXPECT(assert_equal(SOn(id.retract(v2 / 100)), SOn(4).retract(v2 / 100)));
|
||||
|
||||
auto v = Vector6::Zero();
|
||||
SO4 actual = traits<SO4>::Retract(id, v);
|
||||
EXPECT(assert_equal(id, actual));
|
||||
|
|
@ -97,6 +115,12 @@ TEST(SO4, Retract) {
|
|||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Local) {
|
||||
// Check that Cayley is identical to dynamic version
|
||||
EXPECT(
|
||||
assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1))));
|
||||
EXPECT(
|
||||
assert_equal(id.localCoordinates(Q2), SOn(4).localCoordinates(SOn(Q2))));
|
||||
|
||||
auto v0 = Vector6::Zero();
|
||||
Vector6 actual = traits<SO4>::Local(id, id);
|
||||
EXPECT(assert_equal((Vector)v0, actual));
|
||||
|
|
@ -122,15 +146,15 @@ TEST(SO4, compose) {
|
|||
SO4 expected = Q1 * Q2;
|
||||
Matrix actualH1, actualH2;
|
||||
SO4 actual = Q1.compose(Q2, actualH1, actualH2);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
|
||||
Matrix numericalH1 =
|
||||
numericalDerivative21(testing::compose<SO4>, Q1, Q2, 1e-2);
|
||||
CHECK(assert_equal(numericalH1, actualH1));
|
||||
EXPECT(assert_equal(numericalH1, actualH1));
|
||||
|
||||
Matrix numericalH2 =
|
||||
numericalDerivative22(testing::compose<SO4>, Q1, Q2, 1e-2);
|
||||
CHECK(assert_equal(numericalH2, actualH2));
|
||||
EXPECT(assert_equal(numericalH2, actualH2));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -139,39 +163,39 @@ TEST(SO4, vec) {
|
|||
const Vector16 expected = Eigen::Map<const Vector16>(Q2.matrix().data());
|
||||
Matrix actualH;
|
||||
const Vector16 actual = Q2.vec(actualH);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
boost::function<Vector16(const SO4&)> f = [](const SO4& Q) {
|
||||
return Q.vec();
|
||||
};
|
||||
const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
|
||||
CHECK(assert_equal(numericalH, actualH));
|
||||
EXPECT(assert_equal(numericalH, actualH));
|
||||
}
|
||||
|
||||
// /* *************************************************************************
|
||||
// */ TEST(SO4, topLeft) {
|
||||
// const Matrix3 expected = Q3.topLeftCorner<3, 3>();
|
||||
// Matrix actualH;
|
||||
// const Matrix3 actual = Q3.topLeft(actualH);
|
||||
// CHECK(assert_equal(expected, actual));
|
||||
// boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
|
||||
// return Q3.topLeft();
|
||||
// };
|
||||
// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
|
||||
// CHECK(assert_equal(numericalH, actualH));
|
||||
// }
|
||||
//******************************************************************************
|
||||
TEST(SO4, topLeft) {
|
||||
const Matrix3 expected = Q3.matrix().topLeftCorner<3, 3>();
|
||||
Matrix actualH;
|
||||
const Matrix3 actual = topLeft(Q3, actualH);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
|
||||
return topLeft(Q3);
|
||||
};
|
||||
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
|
||||
EXPECT(assert_equal(numericalH, actualH));
|
||||
}
|
||||
|
||||
// /* *************************************************************************
|
||||
// */ TEST(SO4, stiefel) {
|
||||
// const Matrix43 expected = Q3.leftCols<3>();
|
||||
// Matrix actualH;
|
||||
// const Matrix43 actual = Q3.stiefel(actualH);
|
||||
// CHECK(assert_equal(expected, actual));
|
||||
// boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
|
||||
// return Q3.stiefel();
|
||||
// };
|
||||
// const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
|
||||
// CHECK(assert_equal(numericalH, actualH));
|
||||
// }
|
||||
//******************************************************************************
|
||||
TEST(SO4, stiefel) {
|
||||
const Matrix43 expected = Q3.matrix().leftCols<3>();
|
||||
Matrix actualH;
|
||||
const Matrix43 actual = stiefel(Q3, actualH);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
|
||||
return stiefel(Q3);
|
||||
};
|
||||
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
|
||||
EXPECT(assert_equal(numericalH, actualH));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
|
|
|
|||
Loading…
Reference in New Issue