New tests
dellaert
parent
27405fc185
commit
e66fcf8612
|
|
@ -133,6 +133,17 @@ TEST (Point3, distance) {
|
|||
EXPECT(assert_equal(numH2, H2, 1e-8));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Point3, cross) {
|
||||
Matrix aH1, aH2;
|
||||
boost::function<Point3(const Point3&, const Point3&)> f =
|
||||
boost::bind(&Point3::cross, _1, _2, boost::none, boost::none);
|
||||
const Point3 omega(0, 1, 0), theta(4, 6, 8);
|
||||
omega.cross(theta, aH1, aH2);
|
||||
EXPECT(assert_equal(numericalDerivative21(f, omega, theta), aH1));
|
||||
EXPECT(assert_equal(numericalDerivative22(f, omega, theta), aH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -715,7 +715,42 @@ TEST( Pose3, ExpmapDerivative1) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, LogmapDerivative1) {
|
||||
TEST(Pose3, ExpmapDerivative2) {
|
||||
// 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): T -> se(3) is a trajectory in the tangent space of SE(3)
|
||||
// and dexp[A] is a linear map from 4*4 to 4*4 derivatives of se(3)
|
||||
// Hence, the above matrix equation is typed: 4*4 = SE(3) * linear_map(4*4)
|
||||
|
||||
// In GTSAM, we don't work with the Lie-algebra elements A directly, but with 6-vectors.
|
||||
// xi is easy: d Expmap(xi(t)) / dt = ExmapDerivative[xi(t)] * xi'(t)
|
||||
|
||||
// Let's verify the above formula.
|
||||
|
||||
auto xi = [](double t) {
|
||||
Vector6 v;
|
||||
v << 2 * t, sin(t), 4 * t * t, 2 * t, sin(t), 4 * t * t;
|
||||
return v;
|
||||
};
|
||||
auto xi_dot = [](double t) {
|
||||
Vector6 v;
|
||||
v << 2, cos(t), 8 * t, 2, cos(t), 8 * t;
|
||||
return v;
|
||||
};
|
||||
|
||||
// We define a function T
|
||||
auto T = [xi](double t) { return Pose3::Expmap(xi(t)); };
|
||||
|
||||
for (double t = -2.0; t < 2.0; t += 0.3) {
|
||||
const Matrix expected = numericalDerivative11<Pose3, double>(T, t);
|
||||
const Matrix actual = Pose3::ExpmapDerivative(xi(t)) * xi_dot(t);
|
||||
CHECK(assert_equal(expected, actual, 1e-7));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, LogmapDerivative) {
|
||||
Matrix6 actualH;
|
||||
Vector6 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0;
|
||||
Pose3 p = Pose3::Expmap(w);
|
||||
|
|
|
|||
|
|
@ -244,44 +244,99 @@ TEST(Rot3, retract_localCoordinates2)
|
|||
EXPECT(assert_equal(t1, t2.retract(d21)));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Vector w = Vector3(0.1, 0.27, -0.2);
|
||||
|
||||
// Left trivialization Derivative of exp(w) wrpt w:
|
||||
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) {
|
||||
Matrix actualDexpL = Rot3::ExpmapDerivative(w);
|
||||
Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
|
||||
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));
|
||||
|
||||
Matrix actualDexpInvL = Rot3::LogmapDerivative(w);
|
||||
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)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 thetahat(0.1, 0, 0.1);
|
||||
TEST( Rot3, ExpmapDerivative2)
|
||||
TEST( Rot3, ExpmapDerivative3)
|
||||
{
|
||||
Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
|
||||
boost::bind(&Rot3::Expmap, _1, boost::none), thetahat);
|
||||
const Vector3 theta(10, 20, 30);
|
||||
const Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(
|
||||
boost::bind(&Rot3::Expmap, _1, boost::none), theta);
|
||||
|
||||
Matrix Jactual = Rot3::ExpmapDerivative(thetahat);
|
||||
CHECK(assert_equal(Jexpected, Jactual));
|
||||
CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta)));
|
||||
CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta)));
|
||||
}
|
||||
|
||||
Matrix Jactual2 = Rot3::ExpmapDerivative(thetahat);
|
||||
CHECK(assert_equal(Jexpected, Jactual2));
|
||||
/* ************************************************************************* */
|
||||
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 )
|
||||
{
|
||||
Matrix Jexpected = numericalDerivative11<Rot3, Vector3>(boost::bind(
|
||||
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);
|
||||
|
|
@ -291,18 +346,20 @@ TEST( Rot3, jacobianExpmap )
|
|||
/* ************************************************************************* */
|
||||
TEST( Rot3, LogmapDerivative )
|
||||
{
|
||||
Rot3 R = Rot3::Expmap(thetahat); // some rotation
|
||||
Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
|
||||
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::LogmapDerivative(thetahat);
|
||||
const Matrix3 Jactual = Rot3::LogmapDerivative(thetahat);
|
||||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, jacobianLogmap )
|
||||
TEST( Rot3, JacobianLogmap )
|
||||
{
|
||||
Rot3 R = Rot3::Expmap(thetahat); // some rotation
|
||||
Matrix Jexpected = numericalDerivative11<Vector,Rot3>(boost::bind(
|
||||
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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue