diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index da585ce5a..c53ff16bf 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -15,6 +15,7 @@ * @author Can Erdogan * @author Frank Dellaert * @author Alex Trevor + * @author Zhaoyang Lv * @brief The Unit3 class - basically a point on a unit sphere */ @@ -36,6 +37,7 @@ #include #include +#include using namespace std; @@ -43,12 +45,14 @@ namespace gtsam { /* ************************************************************************* */ Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) { - // 3*3 Derivative of representation with respect to point is 3*3: - Matrix3 D_p_point; - Unit3 direction; - direction.p_ = point.normalize(H ? &D_p_point : 0); - if (H) + Unit3 direction(point); + if (H) { + // 3*3 Derivative of representation with respect to point is 3*3: + Matrix3 D_p_point; + point.normalize(D_p_point); // TODO, this calculates norm a second time :-( + // Calculate the 2*3 Jacobian *H << direction.basis().transpose() * D_p_point; + } return direction; } @@ -58,12 +62,10 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { boost::uniform_on_sphere randomDirection(3); // This variate_generator object is required for versions of boost somewhere // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng). - boost::variate_generator > - generator(rng, randomDirection); + boost::variate_generator > generator( + rng, randomDirection); vector d = generator(); - Unit3 result; - result.p_ = Point3(d[0], d[1], d[2]); - return result; + return Unit3(d[0], d[1], d[2]); } /* ************************************************************************* */ @@ -88,20 +90,19 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { // Get the unit vector and derivative wrt this. // NOTE(hayk): We can't call point3(), because it would recursively call basis(). - const Point3& n = p_; + Point3 n(p_); // Get the axis of rotation with the minimum projected length of the point Point3 axis; - double mx = fabs(n.x()), my = fabs(n.y()), mz = fabs(n.z()); - if ((mx <= my) && (mx <= mz)) { + double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z()); + if ((mx <= my) && (mx <= mz)) axis = Point3(1.0, 0.0, 0.0); - } else if ((my <= mx) && (my <= mz)) { + else if ((my <= mx) && (my <= mz)) axis = Point3(0.0, 1.0, 0.0); - } else if ((mz <= mx) && (mz <= my)) { + else if ((mz <= mx) && (mz <= my)) axis = Point3(0.0, 0.0, 1.0); - } else { + else assert(false); - } // Choose the direction of the first basis vector b1 in the tangent plane by crossing n with // the chosen axis. @@ -137,17 +138,17 @@ const Matrix32& Unit3::basis(OptionalJacobian<6, 2> H) const { } /* ************************************************************************* */ -const Point3& Unit3::point3(OptionalJacobian<3, 2> H) const { +Point3 Unit3::point3(OptionalJacobian<3, 2> H) const { if (H) *H = basis(); - return p_; + return Point3(p_); } /* ************************************************************************* */ -Vector3 Unit3::unitVector(boost::optional H) const { +Vector3 Unit3::unitVector(OptionalJacobian<3, 2> H) const { if (H) *H = basis(); - return (p_.vector()); + return (p_); } /* ************************************************************************* */ @@ -194,7 +195,7 @@ double Unit3::dot(const Unit3& q, OptionalJacobian<1,2> H_p, OptionalJacobian<1, Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H_q) const { // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1 Matrix23 Bt = basis().transpose(); - Vector2 xi = Bt * q.p_.vector(); + Vector2 xi = Bt * q.p_; if (H_q) { *H_q = Bt * q.basis(); } @@ -248,46 +249,39 @@ double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const { /* ************************************************************************* */ Unit3 Unit3::retract(const Vector2& v) const { - - // Get the vector form of the point and the basis matrix - Vector3 p = p_.vector(); - Matrix32 B = basis(); - // Compute the 3D xi_hat vector - Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1); + Vector3 xi_hat = basis() * v; + double theta = xi_hat.norm(); - double xi_hat_norm = xi_hat.norm(); - - // Avoid nan - if (xi_hat_norm == 0.0) { - if (v.norm() == 0.0) - return Unit3(point3()); - else - return Unit3(-point3()); + // Treat case of very small v differently + if (theta < std::numeric_limits::epsilon()) { + return Unit3(cos(theta) * p_ + xi_hat); } - Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p - + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); + Vector3 exp_p_xi_hat = + cos(theta) * p_ + xi_hat * (sin(theta) / theta); return Unit3(exp_p_xi_hat); } /* ************************************************************************* */ -Vector2 Unit3::localCoordinates(const Unit3& y) const { - - Vector3 p = p_.vector(), q = y.p_.vector(); - double dot = p.dot(q); - - // Check for special cases - if (dot > 1.0 - 1e-16) - return Vector2(0, 0); - else if (dot < -1.0 + 1e-16) - return Vector2(M_PI, 0); - else { +Vector2 Unit3::localCoordinates(const Unit3& other) const { + const double x = p_.dot(other.p_); + // Crucial quantity here is y = theta/sin(theta) with theta=acos(x) + // Now, y = acos(x) / sin(acos(x)) = acos(x)/sqrt(1-x^2) + // We treat the special case 1 and -1 below + const double x2 = x * x; + const double z = 1 - x2; + double y; + if (z < std::numeric_limits::epsilon()) { + if (x > 0) // first order expansion at x=1 + y = 1.0 - (x - 1.0) / 3.0; + else // cop out + return Vector2(M_PI, 0.0); + } else { // no special case - double theta = acos(dot); - Vector3 result_hat = (theta / sin(theta)) * (q - p * dot); - return basis().transpose() * result_hat; + y = acos(x) / sqrt(z); } + return basis().transpose() * y * (other.p_ - x * p_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 784e5c5e1..e8fe24c9e 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -38,7 +38,7 @@ class GTSAM_EXPORT Unit3 { private: - Point3 p_; ///< The location of the point on the unit sphere + Vector3 p_; ///< The location of the point on the unit sphere mutable boost::optional B_; ///< Cached basis mutable boost::optional H_B_; ///< Cached basis derivative @@ -62,23 +62,18 @@ public: /// Construct from point explicit Unit3(const Point3& p) : - p_(p.normalize()) { + p_(p.vector().normalized()) { } /// Construct from a vector3 - explicit Unit3(const Vector3& v) : - p_(v / v.norm()) { + explicit Unit3(const Vector3& p) : + p_(p.normalized()) { } /// Construct from x,y,z Unit3(double x, double y, double z) : - p_(Point3(x, y, z).normalize()) { - } - - /// Construct from 2D point in plane at focal length f - /// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point - explicit Unit3(const Point2& p, double f=1.0) : - p_(Point3(p.x(), p.y(), f).normalize()) { + p_(x, y, z) { + p_.normalize(); } /// Named constructor from Point3 with optional Jacobian @@ -100,7 +95,7 @@ public: /// The equals function with tolerance bool equals(const Unit3& s, double tol = 1e-9) const { - return p_.equals(s.p_, tol); + return equal_with_abs_tol(p_, s.p_, tol); } /// @} @@ -119,14 +114,14 @@ public: Matrix3 skew() const; /// Return unit-norm Point3 - const Point3& point3(OptionalJacobian<3, 2> H = boost::none) const; + Point3 point3(OptionalJacobian<3, 2> H = boost::none) const; /// Return unit-norm Vector - Vector3 unitVector(boost::optional H = boost::none) const; + Vector3 unitVector(OptionalJacobian<3, 2> H = boost::none) const; /// Return scaled direction as Point3 friend Point3 operator*(double s, const Unit3& d) { - return s * d.p_; + return Point3(s * d.p_); } /// Return dot product with q @@ -152,7 +147,7 @@ public: /// Cross-product w Point3 Point3 cross(const Point3& q) const { - return Point3(p_.vector().cross(q.vector())); + return point3().cross(q); } /// @} @@ -172,7 +167,7 @@ public: enum CoordinatesMode { EXPMAP, ///< Use the exponential map to retract - RENORM ///< Retract with vector addtion and renormalize. + RENORM ///< Retract with vector addition and renormalize. }; /// The retract function @@ -192,13 +187,6 @@ private: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_NVP(p_); - // homebrew serialize Eigen Matrix - ar & boost::serialization::make_nvp("B11", (*B_)(0, 0)); - ar & boost::serialization::make_nvp("B12", (*B_)(0, 1)); - ar & boost::serialization::make_nvp("B21", (*B_)(1, 0)); - ar & boost::serialization::make_nvp("B22", (*B_)(1, 1)); - ar & boost::serialization::make_nvp("B31", (*B_)(2, 0)); - ar & boost::serialization::make_nvp("B32", (*B_)(2, 1)); } /// @}