Added expmap coordinate mode option for Sphere2 retract and local coordinates, as well as unrotate for Sphere2.
							parent
							
								
									bfd334dd0e
								
							
						
					
					
						commit
						401fede2e9
					
				|  | @ -81,6 +81,17 @@ Sphere2 Rot3::rotate(const Sphere2& p, | |||
|   return q; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Sphere2 Rot3::unrotate(const Sphere2& p, | ||||
|     boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const { | ||||
|   Sphere2 q = unrotate(p.point3(Hp)); | ||||
|   if (Hp) | ||||
|     (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp); | ||||
|   if (HR) | ||||
|     (*HR) = q.basis().transpose() * q.skew(); | ||||
|   return q; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Sphere2 Rot3::operator*(const Sphere2& p) const { | ||||
|   return rotate(p); | ||||
|  |  | |||
|  | @ -331,6 +331,10 @@ namespace gtsam { | |||
|     Sphere2 rotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none, | ||||
|         boost::optional<Matrix&> Hp = boost::none) const; | ||||
| 
 | ||||
|     /// unrotate 3D direction from world frame to rotated coordinate frame
 | ||||
|     Sphere2 unrotate(const Sphere2& p, boost::optional<Matrix&> HR = boost::none, | ||||
|         boost::optional<Matrix&> Hp = boost::none) const; | ||||
| 
 | ||||
|     /// rotate 3D direction from rotated coordinate frame to world frame
 | ||||
|     Sphere2 operator*(const Sphere2& p) const; | ||||
| 
 | ||||
|  |  | |||
|  | @ -98,7 +98,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const { | |||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Sphere2 Sphere2::retract(const Vector& v) const { | ||||
| Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const { | ||||
| 
 | ||||
|   // Get the vector form of the point and the basis matrix
 | ||||
|   Vector p = Point3::Logmap(p_); | ||||
|  | @ -106,35 +106,64 @@ Sphere2 Sphere2::retract(const Vector& v) const { | |||
| 
 | ||||
|   // Compute the 3D xi_hat vector
 | ||||
|   Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1); | ||||
|   Vector newPoint = p + xi_hat; | ||||
|    | ||||
|   if (mode == Sphere2::EXPMAP) { | ||||
|     double xi_hat_norm = xi_hat.norm(); | ||||
|     Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); | ||||
|     return Sphere2(exp_p_xi_hat); | ||||
|   } else if (mode == Sphere2::RENORM) { | ||||
|     // Project onto the manifold, i.e. the closest point on the circle to the new location;
 | ||||
|     // same as putting it onto the unit circle
 | ||||
|     Vector newPoint = p + xi_hat; | ||||
|     Vector projected = newPoint / newPoint.norm(); | ||||
| 
 | ||||
|   // Project onto the manifold, i.e. the closest point on the circle to the new location;
 | ||||
|   // same as putting it onto the unit circle
 | ||||
|   Vector projected = newPoint / newPoint.norm(); | ||||
| 
 | ||||
|   return Sphere2(Point3::Expmap(projected)); | ||||
|     return Sphere2(Point3::Expmap(projected)); | ||||
|   } else { | ||||
|     assert (false); | ||||
|     exit (1); | ||||
|   }   | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| Vector Sphere2::localCoordinates(const Sphere2& y) const { | ||||
|   Vector Sphere2::localCoordinates(const Sphere2& y, Sphere2::CoordinatesMode mode) const { | ||||
| 
 | ||||
|   // Make sure that the angle different between x and y is less than 90. Otherwise,
 | ||||
|   // we can project x + xi_hat from the tangent space at x to y.
 | ||||
|   assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y."); | ||||
|   if (mode == Sphere2::EXPMAP) { | ||||
|     Matrix B = basis(); | ||||
|      | ||||
|     Vector p = Point3::Logmap(p_); | ||||
|     Vector q = Point3::Logmap(y.p_); | ||||
|     double theta = acos(p.transpose() * q); | ||||
| 
 | ||||
|   // Get the basis matrix
 | ||||
|   Matrix B = basis(); | ||||
| 
 | ||||
|   // Create the vector forms of p and q (the Point3 of y).
 | ||||
|   Vector p = Point3::Logmap(p_); | ||||
|   Vector q = Point3::Logmap(y.p_); | ||||
| 
 | ||||
|   // Compute the basis coefficients [v0,v1] = (B'q)/(p'q).
 | ||||
|   double alpha = p.transpose() * q; | ||||
|   assert(alpha != 0.0); | ||||
|   Matrix coeffs = (B.transpose() * q) / alpha; | ||||
|   Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0)); | ||||
|   return result; | ||||
|     // the below will be nan if theta == 0.0
 | ||||
|     if (theta == 0.0) | ||||
|       return (Vector (2) << 0, 0); | ||||
|      | ||||
|     Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta)); | ||||
|     Vector result = B.transpose() * result_hat; | ||||
|      | ||||
|     return result; | ||||
|   } else if (mode == Sphere2::RENORM) { | ||||
|     // Make sure that the angle different between x and y is less than 90. Otherwise,
 | ||||
|     // we can project x + xi_hat from the tangent space at x to y.
 | ||||
|     assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y."); | ||||
|      | ||||
|     // Get the basis matrix
 | ||||
|     Matrix B = basis(); | ||||
|      | ||||
|     // Create the vector forms of p and q (the Point3 of y).
 | ||||
|     Vector p = Point3::Logmap(p_); | ||||
|     Vector q = Point3::Logmap(y.p_); | ||||
|      | ||||
|     // Compute the basis coefficients [v0,v1] = (B'q)/(p'q).
 | ||||
|     double alpha = p.transpose() * q; | ||||
|     assert(alpha != 0.0); | ||||
|     Matrix coeffs = (B.transpose() * q) / alpha; | ||||
|     Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0)); | ||||
|     return result; | ||||
|   } else { | ||||
|     assert (false); | ||||
|     exit (1); | ||||
|   } | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
|  |  | |||
|  | @ -22,6 +22,10 @@ | |||
| #include <gtsam/geometry/Point3.h> | ||||
| #include <gtsam/base/DerivedValue.h> | ||||
| 
 | ||||
| #ifndef SPHERE2_DEFAULT_COORDINATES_MODE | ||||
|   #define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::EXPMAP | ||||
| #endif | ||||
| 
 | ||||
| // (Cumbersome) forward declaration for random generator
 | ||||
| namespace boost { | ||||
| namespace random { | ||||
|  | @ -121,11 +125,16 @@ public: | |||
|     return 2; | ||||
|   } | ||||
| 
 | ||||
|   enum CoordinatesMode { | ||||
|     EXPMAP, ///< Use the exponential map to retract
 | ||||
|     RENORM ///< Retract with vector addtion and renormalize.
 | ||||
|   }; | ||||
| 
 | ||||
|   /// The retract function
 | ||||
|   Sphere2 retract(const Vector& v) const; | ||||
|   Sphere2 retract(const Vector& v, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const; | ||||
| 
 | ||||
|   /// The local coordinates function
 | ||||
|   Vector localCoordinates(const Sphere2& s) const; | ||||
|   Vector localCoordinates(const Sphere2& s, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const; | ||||
| 
 | ||||
|   /// @}
 | ||||
| }; | ||||
|  |  | |||
|  | @ -75,6 +75,31 @@ TEST(Sphere2, rotate) { | |||
|   } | ||||
| } | ||||
| 
 | ||||
| //*******************************************************************************
 | ||||
| static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) { | ||||
|   return R.unrotate (p); | ||||
| } | ||||
| 
 | ||||
| TEST(Sphere2, unrotate) { | ||||
|   Rot3 R = Rot3::yaw(-M_PI/2.0); | ||||
|   Sphere2 p(1, 0, 0); | ||||
|   Sphere2 expected = Sphere2(0, 1, 0); | ||||
|   Sphere2 actual = R.unrotate (p); | ||||
|   EXPECT(assert_equal(expected, actual, 1e-8)); | ||||
|   Matrix actualH, expectedH; | ||||
|   // Use numerical derivatives to calculate the expected Jacobian
 | ||||
|   { | ||||
|     expectedH = numericalDerivative21(unrotate_, R, p); | ||||
|     R.unrotate(p, actualH, boost::none); | ||||
|     EXPECT(assert_equal(expectedH, actualH, 1e-9)); | ||||
|   } | ||||
|   { | ||||
|     expectedH = numericalDerivative22(unrotate_, R, p); | ||||
|     R.unrotate(p, boost::none, actualH); | ||||
|     EXPECT(assert_equal(expectedH, actualH, 1e-9)); | ||||
|   } | ||||
| } | ||||
| 
 | ||||
| //*******************************************************************************
 | ||||
| TEST(Sphere2, error) { | ||||
|   Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue