Made retract etc take Fixed vectors in poses

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];

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):

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<Pose3> align(const vector<Point3Pair>& 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);
 

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
 

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));