diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index af56d0197..2a8c7852a 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -76,100 +76,17 @@ namespace gtsam { /// @{ /// identity for group operation - inline static Point3 identity() { - return Point3(); - } + inline static Point3 identity() { return Point3();} - /// "Inverse" - negates the coordinates such that compose(p, inverse(p)) = Point3() - inline Point3 inverse(OptionalJacobian<3,3> H=boost::none) const { - if (H) *H = -I_3x3; - return Point3(-x_, -y_, -z_); - } - - /// syntactic sugar for inverse, i.e., -p == inverse(p) + /// inverse Point3 operator - () const { return Point3(-x_,-y_,-z_);} - /// "Compose" - just adds coordinates of two points - inline Point3 compose(const Point3& p2, - OptionalJacobian<3,3> H1=boost::none, - OptionalJacobian<3,3> H2=boost::none) const { - if (H1) *H1 << I_3x3; - if (H2) *H2 << I_3x3; - return *this + p2; - } - - ///syntactic sugar for adding two points, i.e., p+q == compose(p,q) + /// add Point3 operator + (const Point3& q) const; - /** Between using the default implementation */ - inline Point3 between(const Point3& p2, - OptionalJacobian<3,3> H1=boost::none, - OptionalJacobian<3,3> H2=boost::none) const { - if(H1) *H1 = -I_3x3; - if(H2) *H2 = I_3x3; - return p2 - *this; - } - - /// syntactic sugar for subtracting points, i.e., q-p == between(p,q) + /// subtract Point3 operator - (const Point3& q) const; - /// @} - /// @name Manifold - /// @{ - - /// dimension of the variable - used to autodetect sizes - inline static size_t Dim() { return 3; } - - /// return dimensionality of tangent space, DOF = 3 - inline size_t dim() const { return 3; } - - /// Updates a with tangent space delta - inline Point3 retract(const Vector& v) const { return *this + Point3(v); } - - /// Returns inverse retraction - inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); } - - /// @} - /// @name Lie Group - /// @{ - - /** Exponential map at identity - just create a Point3 from x,y,z */ - static inline Point3 Expmap(const Vector& v, OptionalJacobian<3,3> H=boost::none) { - if (H) *H = I_3x3; - return Point3(v); - } - - /** Log map at identity - return the x,y,z of this point */ - static inline Vector3 Logmap(const Point3& dp, OptionalJacobian<3,3> H=boost::none) { - if (H) *H = I_3x3; - return Vector3(dp.x(), dp.y(), dp.z()); - } - - inline Point3 retract(const Vector& v, OptionalJacobian<3, 3> H1, - OptionalJacobian<3, 3> H2 = boost::none) const { - if (H1) *H1 = I_3x3; - if (H2) *H2 = I_3x3; - return *this + Point3(v); - } - - inline Vector3 localCoordinates(const Point3& q, OptionalJacobian<3, 3> H1, - OptionalJacobian<3, 3> H2 = boost::none) const { - if (H1) *H1 = - I_3x3; - if (H2) *H2 = I_3x3; - Point3 dp = q - *this; - return Vector3(dp.x(), dp.y(), dp.z()); - } - - /// Left-trivialized derivative of the exponential map - static Matrix3 dexpL(const Vector& v) { - return I_3x3; - } - - /// Left-trivialized derivative inverse of the exponential map - static Matrix3 dexpInvL(const Vector& v) { - return I_3x3; - } - /// @} /// @name Vector Space /// @{ diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 5b4a8b33e..611cf7cde 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -131,7 +131,7 @@ 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 = Point3::Logmap(p_); + Vector3 p = p_.vector(); Matrix32 B = basis(); // Compute the 3D xi_hat vector @@ -156,8 +156,7 @@ Unit3 Unit3::retract(const Vector2& v) const { /* ************************************************************************* */ Vector2 Unit3::localCoordinates(const Unit3& y) const { - Vector3 p = Point3::Logmap(p_); - Vector3 q = Point3::Logmap(y.p_); + Vector3 p = p_.vector(), q = y.p_.vector(); double dot = p.dot(q); // Check for special cases diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index 18273b182..f81e2f7b2 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -26,22 +26,36 @@ GTSAM_CONCEPT_LIE_INST(Point3) static Point3 P(0.2, 0.7, -2); +//****************************************************************************** +TEST(Point3 , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsVectorSpace)); +} + +//****************************************************************************** +TEST(Point3 , Invariants) { + Point3 p1(1, 2, 3), p2(4, 5, 6); + check_group_invariants(p1, p2); + check_manifold_invariants(p1, p2); +} + /* ************************************************************************* */ TEST(Point3, Lie) { Point3 p1(1, 2, 3); Point3 p2(4, 5, 6); Matrix H1, H2; - EXPECT(assert_equal(Point3(5, 7, 9), p1.compose(p2, H1, H2))); + EXPECT(assert_equal(Point3(5, 7, 9), traits_x::Compose(p1, p2, H1, H2))); EXPECT(assert_equal(eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(3, 3, 3), p1.between(p2, H1, H2))); + EXPECT(assert_equal(Point3(3, 3, 3), traits_x::Between(p1, p2, H1, H2))); EXPECT(assert_equal(-eye(3), H1)); EXPECT(assert_equal(eye(3), H2)); - EXPECT(assert_equal(Point3(5, 7, 9), p1.retract(Vector3(4., 5., 6.)))); - EXPECT(assert_equal((Vector)Vector3(3.,3.,3.), p1.localCoordinates(p2))); + EXPECT(assert_equal(Point3(5, 7, 9), traits_x::Retract(p1, Vector3(4,5,6)))); + EXPECT(assert_equal(Vector3(3, 3, 3), traits_x::Local(p1,p2))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 798836005..561e912db 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -151,11 +151,11 @@ Pose3 Agrawal06iros(const Vector& xi) { Vector v = xi.tail(3); double t = norm_2(w); if (t < 1e-5) - return Pose3(Rot3(), Point3::Expmap(v)); + return Pose3(Rot3(), Point3(v)); else { Matrix W = skewSymmetric(w/t); Matrix A = eye(3) + ((1 - cos(t)) / t) * W + ((t - sin(t)) / t) * (W * W); - return Pose3(Rot3::Expmap (w), Point3::Expmap(A * v)); + return Pose3(Rot3::Expmap (w), Point3(A * v)); } } diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index fd769b188..bfd3ebb52 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -24,15 +24,15 @@ namespace gtsam { //*************************************************************************** void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "GPSFactor on " << keyFormatter(this->key()) << "\n"; + cout << s << "GPSFactor on " << keyFormatter(key()) << "\n"; nT_.print(" prior mean: "); - this->noiseModel_->print(" noise model: "); + noiseModel_->print(" noise model: "); } //*************************************************************************** bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->nT_.equals(e->nT_, tol); + return e != NULL && Base::equals(*e, tol) && nT_.equals(e->nT_, tol); } //*************************************************************************** @@ -43,8 +43,7 @@ Vector GPSFactor::evaluateError(const Pose3& p, H->block < 3, 3 > (0, 0) << zeros(3, 3); H->block < 3, 3 > (0, 3) << p.rotation().matrix(); } - // manifold equivalent of h(x)-z -> log(z,h(x)) - return nT_.localCoordinates(p.translation()); + return (p.translation() -nT_).vector(); } //*************************************************************************** diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 6e930fcb5..a87d1e071 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -93,8 +93,9 @@ public: */ NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) : - Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_( - feasible), allow_error_(false), error_gain_(0.0), compare_(_compare) { + Base(noiseModel::Constrained::All(traits_x::GetDimension(feasible)), + j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // + compare_(_compare) { } /** diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 8131414d8..74767ff87 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -59,14 +59,14 @@ void PoseRTV::print(const string& s) const { /* ************************************************************************* */ PoseRTV PoseRTV::Expmap(const Vector9& v) { Pose3 newPose = Pose3::Expmap(v.head<6>()); - Velocity3 newVel = Velocity3::Expmap(v.tail<3>()); + Velocity3 newVel = Velocity3(v.tail<3>()); return PoseRTV(newPose, newVel); } /* ************************************************************************* */ Vector9 PoseRTV::Logmap(const PoseRTV& p) { Vector6 Lx = Pose3::Logmap(p.Rt_); - Vector3 Lv = Velocity3::Logmap(p.v_); + Vector3 Lv = p.v_.vector(); return (Vector9() << Lx, Lv).finished(); } @@ -108,7 +108,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1, PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); } PoseRTV PoseRTV::inverse(OptionalJacobian H1) const { if (H1) *H1 = numericalDerivative11(inverse_, *this, 1e-5); - return PoseRTV(Rt_.inverse(), v_.inverse()); + return PoseRTV(Rt_.inverse(), - v_); } /* ************************************************************************* */ @@ -118,7 +118,7 @@ PoseRTV PoseRTV::compose(const PoseRTV& p, OptionalJacobian H2) const { if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5); if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5); - return PoseRTV(Rt_.compose(p.Rt_), v_.compose(p.v_)); + return PoseRTV(Rt_.compose(p.Rt_), v_+p.v_); } /* ************************************************************************* */ @@ -196,7 +196,7 @@ PoseRTV PoseRTV::generalDynamics( Rot3 r2 = rotation().retract(gyro * dt); // Integrate Velocity Equations - Velocity3 v2 = v_.compose(Velocity3(dt * (r2.matrix() * accel + g))); + Velocity3 v2 = v_ + (Velocity3(dt * (r2.matrix() * accel + g))); // Integrate Position Equations Point3 t2 = translationIntegration(r2, v2, dt); @@ -213,15 +213,15 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { Vector6 imu; // acceleration - Vector accel = v1.localCoordinates(v2) / dt; + Vector3 accel = (v2-v1).vector() / dt; imu.head<3>() = r2.transpose() * (accel - g); // rotation rates // just using euler angles based on matlab code // FIXME: this is silly - we shouldn't use differences in Euler angles Matrix Enb = RRTMnb(r1); - Vector euler1 = r1.xyz(), euler2 = r2.xyz(); - Vector dR = euler2 - euler1; + Vector3 euler1 = r1.xyz(), euler2 = r2.xyz(); + Vector3 dR = euler2 - euler1; // normalize yaw in difference (as per Mitch's code) dR(2) = Rot2::fromAngle(dR(2)).theta(); diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 6cfcd93e6..efe1df640 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -218,7 +218,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector3 Vel1(Vector3(0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); - Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt)); + Point3 t2 = t1 + Point3(Vel1 * measurement_dt); Pose3 Pose2(R2, t2); Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); Vector3 Vel2 = Vel1 + dv; @@ -568,7 +568,7 @@ Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, Vector3 Vel1(Vector3(0.5, -0.5, 0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); - Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt)); + Point3 t2 = t1+ Point3(Vel1 * measurement_dt); Pose3 Pose2(R2, t2); Vector dv = measurement_dt