From 22ff9b6aefca430323461be6b3407e559bb6e85a Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 26 May 2015 00:10:03 -0700 Subject: [PATCH] Made retract etc take Fixed vectors in poses --- gtsam/geometry/Pose2.cpp | 4 ++-- gtsam/geometry/Pose2.h | 7 +++---- gtsam/geometry/Pose3.cpp | 26 +++++++++++++------------- gtsam/geometry/Pose3.h | 4 ++-- gtsam/geometry/tests/testPose2.cpp | 2 +- 5 files changed, 21 insertions(+), 22 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 0b0172857..9e87407f4 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -59,7 +59,7 @@ bool Pose2::equals(const Pose2& q, double tol) const { } /* ************************************************************************* */ -Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) { +Pose2 Pose2::Expmap(const Vector3& xi, OptionalJacobian<3, 3> H) { if (H) *H = Pose2::ExpmapDerivative(xi); assert(xi.size() == 3); Point2 v(xi(0),xi(1)); @@ -130,7 +130,7 @@ Matrix3 Pose2::AdjointMap() const { } /* ************************************************************************* */ -Matrix3 Pose2::adjointMap(const Vector& v) { +Matrix3 Pose2::adjointMap(const Vector3& v) { // See Chirikjian12book2, vol.2, pg. 36 Matrix3 ad = zeros(3,3); ad(0,1) = -v[2]; diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 59d53305a..e7a8ab10c 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -118,7 +118,7 @@ public: /// @{ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ - static Pose2 Expmap(const Vector& xi, ChartJacobian H = boost::none); + static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none); ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none); @@ -128,15 +128,14 @@ public: * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) */ Matrix3 AdjointMap() const; - inline Vector Adjoint(const Vector& xi) const { - assert(xi.size() == 3); + inline Vector3 Adjoint(const Vector3& xi) const { return AdjointMap()*xi; } /** * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19 */ - static Matrix3 adjointMap(const Vector& v); + static Matrix3 adjointMap(const Vector3& v); /** * wedge for SE(2): diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 7b7c861fd..32fd75eb8 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -111,7 +111,7 @@ bool Pose3::equals(const Pose3& pose, double tol) const { /* ************************************************************************* */ /** Modified from Murray94book version (which assumes w and v normalized?) */ -Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) { +Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) { if (H) { *H = ExpmapDerivative(xi); } @@ -354,25 +354,25 @@ boost::optional align(const vector& pairs) { return boost::none; // we need at least three pairs // calculate centroids - Vector cp = zero(3), cq = zero(3); - BOOST_FOREACH(const Point3Pair& pair, pairs){ - cp += pair.first.vector(); - cq += pair.second.vector(); -} + Vector3 cp = Vector3::Zero(), cq = Vector3::Zero(); + BOOST_FOREACH(const Point3Pair& pair, pairs) { + cp += pair.first.vector(); + cq += pair.second.vector(); + } double f = 1.0 / n; cp *= f; cq *= f; // Add to form H matrix - Matrix3 H = Eigen::Matrix3d::Zero(); - BOOST_FOREACH(const Point3Pair& pair, pairs){ - Vector dp = pair.first.vector() - cp; - Vector dq = pair.second.vector() - cq; - H += dp * dq.transpose(); -} + Matrix3 H = Z_3x3; + BOOST_FOREACH(const Point3Pair& pair, pairs) { + Vector3 dp = pair.first.vector() - cp; + Vector3 dq = pair.second.vector() - cq; + H += dp * dq.transpose(); + } // Compute SVD - Matrix U,V; + Matrix U, V; Vector S; svd(H, U, S, V); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index b11ae2587..02ad87e80 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -110,7 +110,7 @@ public: /// @{ /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ - static Pose3 Expmap(const Vector& xi, OptionalJacobian<6, 6> H = boost::none); + static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> H = boost::none); /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none); @@ -125,7 +125,7 @@ public: * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ */ - Vector Adjoint(const Vector& xi_b) const { + Vector6 Adjoint(const Vector6& xi_b) const { return AdjointMap() * xi_b; } /// FIXME Not tested - marked as incorrect diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 803bb7c3f..1ce8cd2ea 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -104,7 +104,7 @@ TEST(Pose2, expmap3) { Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0; Matrix expected = eye(3) + A + A2 + A3 + A4; - Vector v = Vector3(0.01, -0.015, 0.99); + Vector3 v(0.01, -0.015, 0.99); Pose2 pose = Pose2::Expmap(v); Pose2 pose2(v); EXPECT(assert_equal(pose, pose2));