Merge remote-tracking branch 'origin/feature/SO3_refactor' into feature/ImuFactorPush2
Frank Dellaert
commit
01ed8810a0
|
|
@ -11,8 +11,10 @@
|
|||
|
||||
/**
|
||||
* @file SO3.cpp
|
||||
* @brief 3*3 matrix representation o SO(3)
|
||||
* @brief 3*3 matrix representation of SO(3)
|
||||
* @author Frank Dellaert
|
||||
* @author Luca Carlone
|
||||
* @author Duy Nguyen Ta
|
||||
* @date December 2014
|
||||
*/
|
||||
|
||||
|
|
@ -24,65 +26,127 @@
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Near zero, we just have I + skew(omega)
|
||||
static SO3 FirstOrder(const Vector3& omega) {
|
||||
Matrix3 R;
|
||||
R(0, 0) = 1.;
|
||||
R(1, 0) = omega.z();
|
||||
R(2, 0) = -omega.y();
|
||||
R(0, 1) = -omega.z();
|
||||
R(1, 1) = 1.;
|
||||
R(2, 1) = omega.x();
|
||||
R(0, 2) = omega.y();
|
||||
R(1, 2) = -omega.x();
|
||||
R(2, 2) = 1.;
|
||||
return R;
|
||||
}
|
||||
// Functor implementing Exponential map
|
||||
struct ExpmapImpl {
|
||||
const double theta2;
|
||||
Matrix3 W, K, KK;
|
||||
bool nearZero;
|
||||
double theta, sin_theta, one_minus_cos; // only defined if !nearZero
|
||||
|
||||
void init() {
|
||||
nearZero = (theta2 <= std::numeric_limits<double>::epsilon());
|
||||
if (nearZero) return;
|
||||
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)]
|
||||
}
|
||||
|
||||
// Constructor with element of Lie algebra so(3)
|
||||
ExpmapImpl(const Vector3& omega) : 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();
|
||||
if (!nearZero) {
|
||||
theta = std::sqrt(theta2);
|
||||
K = W / theta;
|
||||
KK = K * K;
|
||||
}
|
||||
}
|
||||
|
||||
// Constructor with axis-angle
|
||||
ExpmapImpl(const Vector3& axis, double angle) : 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();
|
||||
if (!nearZero) {
|
||||
theta = angle;
|
||||
KK = K * K;
|
||||
}
|
||||
}
|
||||
|
||||
// Rodrgues formula
|
||||
SO3 expmap() const {
|
||||
if (nearZero)
|
||||
return I_3x3 + W;
|
||||
else
|
||||
return I_3x3 + sin_theta * K + one_minus_cos * K * K;
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
|
||||
if (theta*theta > std::numeric_limits<double>::epsilon()) {
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
// get components of axis \omega, where is a unit vector
|
||||
const double& wx = axis.x(), wy = axis.y(), wz = axis.z();
|
||||
|
||||
const double costheta = cos(theta), sintheta = sin(theta), s2 = sin(theta/2.0), c_1 = 2.0*s2*s2;
|
||||
const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta,
|
||||
wz_sintheta = wz * sintheta;
|
||||
|
||||
const double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz;
|
||||
const double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz;
|
||||
const double C22 = c_1 * wz * wz;
|
||||
|
||||
Matrix3 R;
|
||||
R(0, 0) = costheta + C00;
|
||||
R(1, 0) = wz_sintheta + C01;
|
||||
R(2, 0) = -wy_sintheta + C02;
|
||||
R(0, 1) = -wz_sintheta + C01;
|
||||
R(1, 1) = costheta + C11;
|
||||
R(2, 1) = wx_sintheta + C12;
|
||||
R(0, 2) = wy_sintheta + C02;
|
||||
R(1, 2) = -wx_sintheta + C12;
|
||||
R(2, 2) = costheta + C22;
|
||||
return R;
|
||||
} else {
|
||||
return FirstOrder(axis*theta);
|
||||
}
|
||||
|
||||
return ExpmapImpl(axis, theta).expmap();
|
||||
}
|
||||
|
||||
/// simply convert omega to axis/angle representation
|
||||
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
|
||||
if (H) *H = ExpmapDerivative(omega);
|
||||
/* ************************************************************************* */
|
||||
// Functor that implements Exponential map *and* its derivatives
|
||||
struct DexpImpl : ExpmapImpl {
|
||||
const Vector3 omega;
|
||||
double a, b; // constants used in dexp and applyDexp
|
||||
|
||||
double theta2 = omega.dot(omega);
|
||||
if (theta2 > std::numeric_limits<double>::epsilon()) {
|
||||
double theta = std::sqrt(theta2);
|
||||
return AxisAngle(omega / theta, theta);
|
||||
} else {
|
||||
return FirstOrder(omega);
|
||||
// Constructor with element of Lie algebra so(3)
|
||||
DexpImpl(const Vector3& omega) : ExpmapImpl(omega), omega(omega) {
|
||||
if (nearZero) return;
|
||||
a = one_minus_cos / theta;
|
||||
b = 1.0 - sin_theta / theta;
|
||||
}
|
||||
|
||||
// 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) */
|
||||
SO3 dexp() const {
|
||||
if (nearZero)
|
||||
return I_3x3 - 0.5 * W;
|
||||
else
|
||||
return I_3x3 - a * K + b * KK;
|
||||
}
|
||||
|
||||
// Just multiplies with dexp()
|
||||
Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
|
||||
OptionalJacobian<3, 3> H2) const {
|
||||
if (nearZero) {
|
||||
if (H1) *H1 = 0.5 * skewSymmetric(v);
|
||||
if (H2) *H2 = I_3x3;
|
||||
return v - 0.5 * omega.cross(v);
|
||||
}
|
||||
const Vector3 Kv = omega.cross(v / theta);
|
||||
const Vector3 KKv = omega.cross(Kv / theta);
|
||||
if (H1) {
|
||||
// TODO(frank): Iserles hints that there should be a form I + c*K + d*KK
|
||||
const Matrix3 T = skewSymmetric(v / theta);
|
||||
const double Da = (sin_theta - 2.0 * a) / theta2;
|
||||
const double Db = (one_minus_cos - 3.0 * b) / theta2;
|
||||
*H1 = (-Da * Kv + Db * KKv) * omega.transpose() + a * T -
|
||||
skewSymmetric(Kv * b / theta) - b * K * T;
|
||||
}
|
||||
if (H2) *H2 = dexp();
|
||||
return v - a * Kv + b * KKv;
|
||||
}
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
|
||||
if (H) {
|
||||
DexpImpl impl(omega);
|
||||
*H = impl.dexp();
|
||||
return impl.expmap();
|
||||
} else
|
||||
return ExpmapImpl(omega).expmap();
|
||||
}
|
||||
|
||||
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
|
||||
return DexpImpl(omega).dexp();
|
||||
}
|
||||
|
||||
Vector3 SO3::ApplyExpmapDerivative(const Vector3& omega, const Vector3& v,
|
||||
OptionalJacobian<3, 3> H1,
|
||||
OptionalJacobian<3, 3> H2) {
|
||||
return DexpImpl(omega).applyDexp(v, H1, H2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -96,7 +160,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
|
|||
const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
|
||||
|
||||
// Get trace(R)
|
||||
double tr = R.trace();
|
||||
const double tr = R.trace();
|
||||
|
||||
Vector3 omega;
|
||||
|
||||
|
|
@ -112,7 +176,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
|
|||
omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
|
||||
} else {
|
||||
double magnitude;
|
||||
double tr_3 = tr - 3.0; // always negative
|
||||
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));
|
||||
|
|
@ -129,58 +193,13 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
|
||||
using std::sin;
|
||||
|
||||
const double theta2 = omega.dot(omega);
|
||||
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
|
||||
const double theta = std::sqrt(theta2); // rotation angle
|
||||
#ifdef DUY_VERSION
|
||||
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
|
||||
Matrix3 X = skewSymmetric(omega);
|
||||
Matrix3 X2 = X*X;
|
||||
double vi = theta/2.0;
|
||||
double s1 = sin(vi)/vi;
|
||||
double s2 = (theta - sin(theta))/(theta*theta*theta);
|
||||
return I_3x3 - 0.5*s1*s1*X + s2*X2;
|
||||
#else // Luca's version
|
||||
/**
|
||||
* 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(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega)
|
||||
* where Jr = ExpmapDerivative(thetahat);
|
||||
* This maps a perturbation in the tangent space (omega) to
|
||||
* a perturbation on the manifold (expmap(Jr * omega))
|
||||
*/
|
||||
// element of Lie algebra so(3): X = omega^, normalized by normx
|
||||
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
|
||||
Matrix3 Y;
|
||||
Y << 0.0, -wz, +wy,
|
||||
+wz, 0.0, -wx,
|
||||
-wy, +wx, 0.0;
|
||||
const double s2 = sin(theta / 2.0);
|
||||
const double a = (2.0 * s2 * s2 / theta2);
|
||||
const double b = (1.0 - sin(theta) / theta) / theta2;
|
||||
return I_3x3 - a * Y + b * Y * Y;
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 SO3::LogmapDerivative(const Vector3& 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
|
||||
#ifdef DUY_VERSION
|
||||
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
|
||||
Matrix3 X = skewSymmetric(omega);
|
||||
Matrix3 X2 = X*X;
|
||||
double vi = theta/2.0;
|
||||
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
|
||||
return I_3x3 + 0.5*X - s2*X2;
|
||||
#else // Luca's version
|
||||
/** 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
|
||||
|
|
@ -188,11 +207,10 @@ Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
|
|||
* This maps a perturbation on the manifold (expmap(omega))
|
||||
* to a perturbation in the tangent space (Jrinv * omega)
|
||||
*/
|
||||
const Matrix3 X = skewSymmetric(omega); // element of Lie algebra so(3): X = omega^
|
||||
return I_3x3 + 0.5 * X
|
||||
+ (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X
|
||||
* X;
|
||||
#endif
|
||||
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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -13,6 +13,8 @@
|
|||
* @file SO3.h
|
||||
* @brief 3*3 matrix representation of SO(3)
|
||||
* @author Frank Dellaert
|
||||
* @author Luca Carlone
|
||||
* @author Duy Nguyen Ta
|
||||
* @date December 2014
|
||||
*/
|
||||
|
||||
|
|
@ -97,15 +99,20 @@ public:
|
|||
*/
|
||||
static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
|
||||
|
||||
/// Derivative of Expmap
|
||||
static Matrix3 ExpmapDerivative(const Vector3& omega);
|
||||
|
||||
/// Implement ExpmapDerivative(omega) * v, with derivatives
|
||||
static Vector3 ApplyExpmapDerivative(const Vector3& omega, const Vector3& v,
|
||||
OptionalJacobian<3, 3> H1 = boost::none,
|
||||
OptionalJacobian<3, 3> H2 = boost::none);
|
||||
|
||||
/**
|
||||
* 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 Expmap
|
||||
static Matrix3 ExpmapDerivative(const Vector3& omega);
|
||||
|
||||
/// Derivative of Logmap
|
||||
static Matrix3 LogmapDerivative(const Vector3& omega);
|
||||
|
||||
|
|
|
|||
|
|
@ -243,129 +243,6 @@ TEST(Rot3, retract_localCoordinates2)
|
|||
Vector d21 = t2.localCoordinates(t1);
|
||||
EXPECT(assert_equal(t1, t2.retract(d21)));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
namespace exmap_derivative {
|
||||
static const Vector3 w(0.1, 0.27, -0.2);
|
||||
}
|
||||
// Left trivialized Derivative of exp(w) wrpt w:
|
||||
// How does exp(w) change when w changes?
|
||||
// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
|
||||
// => y = log (exp(-w) * exp(w+dw))
|
||||
Vector3 testDexpL(const Vector3& dw) {
|
||||
using exmap_derivative::w;
|
||||
return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
|
||||
}
|
||||
|
||||
TEST( Rot3, ExpmapDerivative) {
|
||||
using exmap_derivative::w;
|
||||
const Matrix actualDexpL = Rot3::ExpmapDerivative(w);
|
||||
const Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
|
||||
Vector3::Zero(), 1e-2);
|
||||
EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7));
|
||||
|
||||
const Matrix actualDexpInvL = Rot3::LogmapDerivative(w);
|
||||
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, ExpmapDerivative2)
|
||||
{
|
||||
const Vector3 theta(0.1, 0, 0.1);
|
||||
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
|
||||
boost::bind(&Rot3::Expmap, _1, boost::none), theta);
|
||||
|
||||
CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
|
||||
CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, ExpmapDerivative3)
|
||||
{
|
||||
const Vector3 theta(10, 20, 30);
|
||||
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
|
||||
boost::bind(&Rot3::Expmap, _1, boost::none), theta);
|
||||
|
||||
CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
|
||||
CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, ExpmapDerivative4) {
|
||||
// Iserles05an (Lie-group Methods) says:
|
||||
// scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
|
||||
// matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
|
||||
// where A(t): R -> so(3) is a trajectory in the tangent space of SO(3)
|
||||
// and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3)
|
||||
// Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3)
|
||||
|
||||
// In GTSAM, we don't work with the skew-symmetric matrices A directly, but with 3-vectors.
|
||||
// omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t)
|
||||
|
||||
// Let's verify the above formula.
|
||||
|
||||
auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
|
||||
auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
|
||||
|
||||
// We define a function R
|
||||
auto R = [w](double t) { return Rot3::Expmap(w(t)); };
|
||||
|
||||
for (double t = -2.0; t < 2.0; t += 0.3) {
|
||||
const Matrix expected = numericalDerivative11<Rot3, double>(R, t);
|
||||
const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t);
|
||||
CHECK(assert_equal(expected, actual, 1e-7));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, ExpmapDerivative5) {
|
||||
auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
|
||||
auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
|
||||
|
||||
// Same as above, but define R as mapping local coordinates to neighborhood aroud R0.
|
||||
const Rot3 R0 = Rot3::Rodrigues(0.1, 0.4, 0.2);
|
||||
auto R = [R0, w](double t) { return R0.expmap(w(t)); };
|
||||
|
||||
for (double t = -2.0; t < 2.0; t += 0.3) {
|
||||
const Matrix expected = numericalDerivative11<Rot3, double>(R, t);
|
||||
const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t);
|
||||
CHECK(assert_equal(expected, actual, 1e-7));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, jacobianExpmap )
|
||||
{
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(boost::bind(
|
||||
&Rot3::Expmap, _1, boost::none), thetahat);
|
||||
Matrix3 Jactual;
|
||||
const Rot3 R = Rot3::Expmap(thetahat, Jactual);
|
||||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, LogmapDerivative )
|
||||
{
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const Rot3 R = Rot3::Expmap(thetahat); // some rotation
|
||||
const Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
|
||||
&Rot3::Logmap, _1, boost::none), R);
|
||||
const Matrix3 Jactual = Rot3::LogmapDerivative(thetahat);
|
||||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, JacobianLogmap )
|
||||
{
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const Rot3 R = Rot3::Expmap(thetahat); // some rotation
|
||||
const Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
|
||||
&Rot3::Logmap, _1, boost::none), R);
|
||||
Matrix3 Jactual;
|
||||
Rot3::Logmap(R, Jactual);
|
||||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, manifold_expmap)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* 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)
|
||||
|
|
@ -24,16 +24,14 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<SO3 >));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<SO3 >));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<SO3 >));
|
||||
TEST(SO3, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<SO3>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<SO3>));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<SO3>));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , Constructor) {
|
||||
SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
|
||||
}
|
||||
TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); }
|
||||
|
||||
//******************************************************************************
|
||||
SO3 id;
|
||||
|
|
@ -42,46 +40,220 @@ SO3 R1(Eigen::AngleAxisd(0.1, z_axis));
|
|||
SO3 R2(Eigen::AngleAxisd(0.2, z_axis));
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , Local) {
|
||||
TEST(SO3, Local) {
|
||||
Vector3 expected(0, 0, 0.1);
|
||||
Vector3 actual = traits<SO3>::Local(R1, R2);
|
||||
EXPECT(assert_equal((Vector)expected,actual));
|
||||
EXPECT(assert_equal((Vector)expected, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , Retract) {
|
||||
TEST(SO3, Retract) {
|
||||
Vector3 v(0, 0, 0.1);
|
||||
SO3 actual = traits<SO3>::Retract(R1, v);
|
||||
EXPECT(actual.isApprox(R2));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , Invariants) {
|
||||
EXPECT(check_group_invariants(id,id));
|
||||
EXPECT(check_group_invariants(id,R1));
|
||||
EXPECT(check_group_invariants(R2,id));
|
||||
EXPECT(check_group_invariants(R2,R1));
|
||||
TEST(SO3, Invariants) {
|
||||
EXPECT(check_group_invariants(id, id));
|
||||
EXPECT(check_group_invariants(id, R1));
|
||||
EXPECT(check_group_invariants(R2, id));
|
||||
EXPECT(check_group_invariants(R2, R1));
|
||||
|
||||
EXPECT(check_manifold_invariants(id,id));
|
||||
EXPECT(check_manifold_invariants(id,R1));
|
||||
EXPECT(check_manifold_invariants(R2,id));
|
||||
EXPECT(check_manifold_invariants(R2,R1));
|
||||
EXPECT(check_manifold_invariants(id, id));
|
||||
EXPECT(check_manifold_invariants(id, R1));
|
||||
EXPECT(check_manifold_invariants(R2, id));
|
||||
EXPECT(check_manifold_invariants(R2, R1));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , LieGroupDerivatives) {
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id,id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id,R2);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2,id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2,R1);
|
||||
TEST(SO3, LieGroupDerivatives) {
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id, id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(id, R2);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2, id);
|
||||
CHECK_LIE_GROUP_DERIVATIVES(R2, R1);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3 , ChartDerivatives) {
|
||||
CHECK_CHART_DERIVATIVES(id,id);
|
||||
CHECK_CHART_DERIVATIVES(id,R2);
|
||||
CHECK_CHART_DERIVATIVES(R2,id);
|
||||
CHECK_CHART_DERIVATIVES(R2,R1);
|
||||
TEST(SO3, ChartDerivatives) {
|
||||
CHECK_CHART_DERIVATIVES(id, id);
|
||||
CHECK_CHART_DERIVATIVES(id, R2);
|
||||
CHECK_CHART_DERIVATIVES(R2, id);
|
||||
CHECK_CHART_DERIVATIVES(R2, R1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace exmap_derivative {
|
||||
static const Vector3 w(0.1, 0.27, -0.2);
|
||||
}
|
||||
// Left trivialized Derivative of exp(w) wrpt w:
|
||||
// How does exp(w) change when w changes?
|
||||
// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
|
||||
// => y = log (exp(-w) * exp(w+dw))
|
||||
Vector3 testDexpL(const Vector3& dw) {
|
||||
using exmap_derivative::w;
|
||||
return SO3::Logmap(SO3::Expmap(-w) * SO3::Expmap(w + dw));
|
||||
}
|
||||
|
||||
TEST(SO3, ExpmapDerivative) {
|
||||
using exmap_derivative::w;
|
||||
const Matrix actualDexpL = SO3::ExpmapDerivative(w);
|
||||
const Matrix expectedDexpL =
|
||||
numericalDerivative11<Vector3, Vector3>(testDexpL, Vector3::Zero(), 1e-2);
|
||||
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-7));
|
||||
|
||||
const Matrix actualDexpInvL = SO3::LogmapDerivative(w);
|
||||
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, ExpmapDerivative2) {
|
||||
const Vector3 theta(0.1, 0, 0.1);
|
||||
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
|
||||
boost::bind(&SO3::Expmap, _1, boost::none), theta);
|
||||
|
||||
CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
|
||||
CHECK(assert_equal(Matrix3(Jexpected.transpose()),
|
||||
SO3::ExpmapDerivative(-theta)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, ExpmapDerivative3) {
|
||||
const Vector3 theta(10, 20, 30);
|
||||
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
|
||||
boost::bind(&SO3::Expmap, _1, boost::none), theta);
|
||||
|
||||
CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta)));
|
||||
CHECK(assert_equal(Matrix3(Jexpected.transpose()),
|
||||
SO3::ExpmapDerivative(-theta)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, ExpmapDerivative4) {
|
||||
// Iserles05an (Lie-group Methods) says:
|
||||
// scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
|
||||
// matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t)
|
||||
// where A(t): R -> so(3) is a trajectory in the tangent space of SO(3)
|
||||
// and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3)
|
||||
// Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3)
|
||||
|
||||
// In GTSAM, we don't work with the skew-symmetric matrices A directly, but
|
||||
// with 3-vectors.
|
||||
// omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t)
|
||||
|
||||
// Let's verify the above formula.
|
||||
|
||||
auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
|
||||
auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
|
||||
|
||||
// We define a function R
|
||||
auto R = [w](double t) { return SO3::Expmap(w(t)); };
|
||||
|
||||
for (double t = -2.0; t < 2.0; t += 0.3) {
|
||||
const Matrix expected = numericalDerivative11<SO3, double>(R, t);
|
||||
const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t);
|
||||
CHECK(assert_equal(expected, actual, 1e-7));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, ExpmapDerivative5) {
|
||||
auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
|
||||
auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
|
||||
|
||||
// Now define R as mapping local coordinates to neighborhood around R0.
|
||||
const SO3 R0 = SO3::Expmap(Vector3(0.1, 0.4, 0.2));
|
||||
auto R = [R0, w](double t) { return R0.expmap(w(t)); };
|
||||
|
||||
for (double t = -2.0; t < 2.0; t += 0.3) {
|
||||
const Matrix expected = numericalDerivative11<SO3, double>(R, t);
|
||||
const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t);
|
||||
CHECK(assert_equal(expected, actual, 1e-7));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, ExpmapDerivative6) {
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
|
||||
boost::bind(&SO3::Expmap, _1, boost::none), thetahat);
|
||||
Matrix3 Jactual;
|
||||
SO3::Expmap(thetahat, Jactual);
|
||||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, LogmapDerivative) {
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const SO3 R = SO3::Expmap(thetahat); // some rotation
|
||||
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
|
||||
boost::bind(&SO3::Logmap, _1, boost::none), R);
|
||||
const Matrix3 Jactual = SO3::LogmapDerivative(thetahat);
|
||||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, JacobianLogmap) {
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const SO3 R = SO3::Expmap(thetahat); // some rotation
|
||||
const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
|
||||
boost::bind(&SO3::Logmap, _1, boost::none), R);
|
||||
Matrix3 Jactual;
|
||||
SO3::Logmap(R, Jactual);
|
||||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, ApplyExpmapDerivative1) {
|
||||
Matrix aH1, aH2;
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none);
|
||||
for (Vector3 omega : {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)}) {
|
||||
Matrix3 H = SO3::ExpmapDerivative(omega);
|
||||
Vector3 expected = H * v;
|
||||
EXPECT(assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v)));
|
||||
EXPECT(assert_equal(expected,
|
||||
SO3::ApplyExpmapDerivative(omega, v, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
|
||||
EXPECT(assert_equal(H, aH2));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, ApplyExpmapDerivative2) {
|
||||
Matrix aH1, aH2;
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none);
|
||||
const Vector3 omega(0, 0, 0);
|
||||
for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) {
|
||||
Matrix3 H = SO3::ExpmapDerivative(omega);
|
||||
Vector3 expected = H * v;
|
||||
EXPECT(assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v)));
|
||||
EXPECT(
|
||||
assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
|
||||
EXPECT(assert_equal(H, aH2));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, ApplyExpmapDerivative3) {
|
||||
Matrix aH1, aH2;
|
||||
boost::function<Vector3(const Vector3&, const Vector3&)> f =
|
||||
boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none);
|
||||
const Vector3 omega(0.1, 0.2, 0.3), v(0.4, 0.3, 0.2);
|
||||
Matrix3 H = SO3::ExpmapDerivative(omega);
|
||||
Vector3 expected = H * v;
|
||||
EXPECT(assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v)));
|
||||
EXPECT(
|
||||
assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v, aH1, aH2)));
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2));
|
||||
EXPECT(assert_equal(H, aH2));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -90,4 +262,3 @@ int main() {
|
|||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
//******************************************************************************
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue