Merged in feature/FixedVectorsInPoses (pull request #142)

Switch to using Fixed Vectors in PoseX

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
 

timing/timeCalibratedCamera.cpp

@@ -15,10 +15,10 @@
  * @author  Frank Dellaert
  */
 
-#include <time.h>
-#include <iostream>
-
+#include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/CalibratedCamera.h>
+#include <ctime>
+#include <iostream>
 
 using namespace std;
 using namespace gtsam;

timing/timeLago.cpp

@@ -19,6 +19,7 @@
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/lago.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/linear/Sampler.h>
 #include <gtsam/base/timing.h>

timing/timeRot2.cpp

@@ -15,10 +15,10 @@
  * @author  Richard Roberts
  */
 
-#include <iostream>
 
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/timing.h>
-#include <gtsam/geometry/Rot2.h>
+#include <iostream>
 
 using namespace std;
 using namespace gtsam;