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-e3fd810bbb7frelease/4.3a0
							parent
							
								
									9636ace3bb
								
							
						
					
					
						commit
						046da246ba
					
				|  | @ -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<Rot3> { | ||||
| 
 | ||||
|   Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z
 | ||||
| 
 | ||||
|   typedef NoiseModelFactor1<Rot3> 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<Matrix&> 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<Rot3> { | ||||
| 
 | ||||
|   Sphere2 p_, z_; ///< Predicted and measured directions, p = iRc * z
 | ||||
| 
 | ||||
|   typedef NoiseModelFactor1<Rot3> Base; | ||||
|  | @ -24,7 +67,7 @@ class RotateFactor: public NoiseModelFactor1<Rot3> { | |||
| 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<Matrix&> 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
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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<Rot3>(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<Rot3>( | ||||
|         boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, | ||||
|             boost::none), iRc); | ||||
|     f.evaluateError(iRc, actual); | ||||
|     EXPECT(assert_equal(expected, actual, 1e-9)); | ||||
|   } | ||||
|   { | ||||
|     expected = numericalDerivative11<Rot3>( | ||||
|         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); | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue