From 046da246baa451fdbdb4aba8f22dba8f3c291f46 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 21 Dec 2013 15:06:39 +0000 Subject: [PATCH] RotateFactor now takes full rotations, old one that takes directions is RotateDirectionsFactor (in same header) git-svn-id: https://svn.cc.gatech.edu/borg/gtsam/trunk@20421 898a188c-9671-0410-8e00-e3fd810bbb7f --- gtsam/slam/RotateFactor.h | 61 ++++++++++---- gtsam/slam/tests/testRotateFactor.cpp | 109 +++++++++++++++++++++++--- 2 files changed, 142 insertions(+), 28 deletions(-) diff --git a/gtsam/slam/RotateFactor.h b/gtsam/slam/RotateFactor.h index 83e89b713..f30392d62 100644 --- a/gtsam/slam/RotateFactor.h +++ b/gtsam/slam/RotateFactor.h @@ -13,10 +13,53 @@ namespace gtsam { /** - * Factor that evaluates distance between two rotated directions + * Factor on unknown rotation iRC that relates two incremental rotations + * c1Rc2 = iRc' * i1Ri2 * iRc + * Which we can write (see doc/math.lyx) + * e^[z] = iRc' * e^[p] * iRc = e^([iRc'*p]) + * with z and p measured and predicted angular velocities, and hence + * p = iRc * z */ class RotateFactor: public NoiseModelFactor1 { + Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z + + typedef NoiseModelFactor1 Base; + +public: + + /// Constructor + RotateFactor(Key key, const Rot3& P, const Rot3& Z, + const SharedNoiseModel& model) : + Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) { + } + + /// print + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s); + std::cout << "RotateFactor:" << std::endl; + p_.print("p"); + z_.print("z"); + } + + /// vector of errors returns 2D vector + Vector evaluateError(const Rot3& R, + boost::optional H = boost::none) const { + // predict p_ as q = R*z_, derivative H will be filled if not none + Point3 q = R.rotate(z_,H); + // error is just difference, and note derivative of that wrpt q is I3 + return Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z(); + } + +}; + +/** + * Factor on unknown rotation R that relates two directions p_i = iRc * z_c + * Directions provide less constraints than a full rotation + */ +class RotateDirectionsFactor: public NoiseModelFactor1 { + Sphere2 p_, z_; ///< Predicted and measured directions, p = iRc * z typedef NoiseModelFactor1 Base; @@ -24,7 +67,7 @@ class RotateFactor: public NoiseModelFactor1 { public: /// Constructor - RotateFactor(Key key, const Sphere2& p, const Sphere2& z, + RotateDirectionsFactor(Key key, const Sphere2& p, const Sphere2& z, const SharedNoiseModel& model) : Base(model, key), p_(p), z_(z) { } @@ -33,6 +76,7 @@ public: virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { Base::print(s); + std::cout << "RotateDirectionsFactor:" << std::endl; p_.print("p"); z_.print("z"); } @@ -50,19 +94,6 @@ public: return e; } - /// Obsolete: vector of errors returns 1D vector - Vector evaluateError1(const Rot3& R, - boost::optional H = boost::none) const { - Sphere2 q = R * z_; - double e = p_.distance(q, H); - if (H) { - Matrix DR; - R.rotate(z_, DR); - *H = (*H) * DR; - } - return (Vector(1) << e); - } - }; } // gtsam diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index 56ea09b5d..715b72b74 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -26,23 +26,42 @@ using namespace gtsam; // And camera is looking forward. Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0); Rot3 iRc(cameraX, cameraY, cameraZ); + // Now, let's create some rotations around IMU frame Sphere2 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1); -Rot3 gRi1 = Rot3::Expmap(Vector3(1, 0, 0)); +Rot3 i1Ri2 = Rot3::rodriguez(p1, 1), // +i2Ri3 = Rot3::rodriguez(p2, 1), // +i3Ri4 = Rot3::rodriguez(p3, 1); + // The corresponding rotations in the camera frame -Sphere2 z1 = iRc.inverse() * p1, z2 = iRc.inverse() * p2, // +Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, // +c2Zc3 = iRc.inverse() * i2Ri3 * iRc, // +c3Zc4 = iRc.inverse() * i3Ri4 * iRc; + +// The corresponding rotated directions in the camera frame +Sphere2 z1 = iRc.inverse() * p1, // +z2 = iRc.inverse() * p2, // z3 = iRc.inverse() * p3; -// noise model -noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, 0.01); +typedef noiseModel::Isotropic::shared_ptr Model; + +//************************************************************************* +TEST (RotateFactor, checkMath) { + EXPECT(assert_equal(c1Zc2, Rot3::rodriguez(z1, 1))); + EXPECT(assert_equal(c2Zc3, Rot3::rodriguez(z2, 1))); + EXPECT(assert_equal(c3Zc4, Rot3::rodriguez(z3, 1))); +} //************************************************************************* TEST (RotateFactor, test) { - RotateFactor f(1, p1, z1, model); + Model model = noiseModel::Isotropic::Sigma(3, 0.01); + RotateFactor f(1, i1Ri2, c1Zc2, model); + EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8)); + Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); - EXPECT(assert_equal((Vector(2)<<0,0), f.evaluateError(iRc), 1e-8)); - EXPECT( - assert_equal((Vector(2)<<-0.08867, -0.20197), f.evaluateError(R), 1e-5)); + Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867); + EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); + Matrix actual, expected; // Use numerical derivatives to calculate the expected Jacobian { @@ -63,9 +82,10 @@ TEST (RotateFactor, test) { TEST (RotateFactor, minimization) { // Let's try to recover the correct iRc by minimizing NonlinearFactorGraph graph; - graph.add(RotateFactor(1, p1, z1, model)); - graph.add(RotateFactor(1, p2, z2, model)); - graph.add(RotateFactor(1, p3, z3, model)); + Model model = noiseModel::Isotropic::Sigma(3, 0.01); + graph.add(RotateFactor(1, i1Ri2, c1Zc2, model)); + graph.add(RotateFactor(1, i2Ri3, c2Zc3, model)); + graph.add(RotateFactor(1, i3Ri4, c3Zc4, model)); // Check error at ground truth Values truth; @@ -74,8 +94,71 @@ TEST (RotateFactor, minimization) { // Check error at initial estimate Values initial; - double degree = M_PI/180; - Rot3 initialE = iRc.retract(degree*(Vector(3) << 20, -20, 20)); + double degree = M_PI / 180; + Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); + initial.insert(1, initialE); + EXPECT_DOUBLES_EQUAL(3349, graph.error(initial), 1); + + // Optimize + LevenbergMarquardtParams parameters; + //parameters.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check result + Rot3 actual = result.at(1); + EXPECT(assert_equal(iRc, actual,1e-1)); + + // Check error at result + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); +} + +//************************************************************************* +TEST (RotateDirectionsFactor, test) { + Model model = noiseModel::Isotropic::Sigma(2, 0.01); + RotateDirectionsFactor f(1, p1, z1, model); + EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8)); + + Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1)); + Vector expectedE = (Vector(2) << -0.08867, -0.20197); + EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5)); + + Matrix actual, expected; + // Use numerical derivatives to calculate the expected Jacobian + { + expected = numericalDerivative11( + boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, + boost::none), iRc); + f.evaluateError(iRc, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } + { + expected = numericalDerivative11( + boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, + boost::none), R); + f.evaluateError(R, actual); + EXPECT(assert_equal(expected, actual, 1e-9)); + } +} + +//************************************************************************* +TEST (RotateDirectionsFactor, minimization) { + // Let's try to recover the correct iRc by minimizing + NonlinearFactorGraph graph; + Model model = noiseModel::Isotropic::Sigma(2, 0.01); + graph.add(RotateDirectionsFactor(1, p1, z1, model)); + graph.add(RotateDirectionsFactor(1, p2, z2, model)); + graph.add(RotateDirectionsFactor(1, p3, z3, model)); + + // Check error at ground truth + Values truth; + truth.insert(1, iRc); + EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); + + // Check error at initial estimate + Values initial; + double degree = M_PI / 180; + Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20)); initial.insert(1, initialE); EXPECT_DOUBLES_EQUAL(3162, graph.error(initial), 1);