No more LieVector/LieScalar

gtsam_unstable/geometry/InvDepthCamera3.h

@@ -69,10 +69,9 @@ public:
    * @param inv inverse depth
    * @return Point3
    */
-  static gtsam::Point3 invDepthTo3D(const gtsam::LieVector& pw, const gtsam::LieScalar& inv) {
+  static gtsam::Point3 invDepthTo3D(const Vector5& pw, double rho) {
-    gtsam::Point3 ray_base(pw.vector().segment(0,3));
+    gtsam::Point3 ray_base(pw.segment(0,3));
     double theta = pw(3), phi = pw(4);
-    double rho = inv.value();  // inverse depth
     gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
     return ray_base + m/rho;
   }
@@ -82,15 +81,14 @@ public:
    *  @param H1 is the jacobian w.r.t. [pose3 calibration]
    *  @param H2 is the jacobian w.r.t. inv_depth_landmark
    */
-  inline gtsam::Point2 project(const gtsam::LieVector& pw,
+  inline gtsam::Point2 project(const Vector5& pw,
-      const gtsam::LieScalar& inv_depth,
+      double rho,
       boost::optional<gtsam::Matrix&> H1 = boost::none,
       boost::optional<gtsam::Matrix&> H2 = boost::none,
       boost::optional<gtsam::Matrix&> H3 = boost::none) const {
 
-    gtsam::Point3 ray_base(pw.vector().segment(0,3));
+    gtsam::Point3 ray_base(pw.segment(0,3));
     double theta = pw(3), phi = pw(4);
-    double rho = inv_depth.value();  // inverse depth
     gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
     const gtsam::Point3 landmark = ray_base + m/rho;
 
@@ -155,7 +153,7 @@ public:
    * useful for point initialization
    */
 
-  inline std::pair<gtsam::LieVector, gtsam::LieScalar> backproject(const gtsam::Point2& pi, const double depth) const {
+  inline std::pair<Vector5, double> backproject(const gtsam::Point2& pi, const double depth) const {
     const gtsam::Point2 pn = k_->calibrate(pi);
     gtsam::Point3 pc(pn.x(), pn.y(), 1.0);
     pc = pc/pc.norm();
@@ -164,8 +162,8 @@ public:
     gtsam::Point3 ray = pw - pt;
     double theta = atan2(ray.y(), ray.x()); // longitude
     double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
-    return std::make_pair(gtsam::LieVector((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)),
+    return std::make_pair(Vector5((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)),
-        gtsam::LieScalar(1./depth));
+        double(1./depth));
   }
 
 private:

gtsam_unstable/slam/InvDepthFactor3.h

@@ -80,7 +80,7 @@ public:
   }
 
   /// Evaluate error h(x)-z and optionally derivatives
-  gtsam::Vector evaluateError(const POSE& pose, const gtsam::LieVector& point, const INVDEPTH& invDepth,
+  gtsam::Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
       boost::optional<gtsam::Matrix&> H1=boost::none,
       boost::optional<gtsam::Matrix&> H2=boost::none,
       boost::optional<gtsam::Matrix&> H3=boost::none) const {

gtsam_unstable/slam/InvDepthFactorVariant1.h

@@ -22,7 +22,7 @@ namespace gtsam {
 /**
  * Binary factor representing a visual measurement using an inverse-depth parameterization
  */
-class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, LieVector> {
+class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, Vector6> {
 protected:
 
   // Keep a copy of measurement and calibration for I/O
@@ -32,7 +32,7 @@ protected:
 public:
 
   /// shorthand for base class type
-  typedef NoiseModelFactor2<Pose3, LieVector> Base;
+  typedef NoiseModelFactor2<Pose3, Vector6> Base;
 
   /// shorthand for this class
   typedef InvDepthFactorVariant1 This;
@@ -78,7 +78,7 @@ public:
         && this->K_->equals(*e->K_, tol);
   }
 
-  Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const {
+  Vector inverseDepthError(const Pose3& pose, const Vector6& landmark) const {
     try {
       // Calculate the 3D coordinates of the landmark in the world frame
       double x = landmark(0), y = landmark(1), z = landmark(2);
@@ -99,7 +99,7 @@ public:
   }
 
   /// Evaluate error h(x)-z and optionally derivatives
-  Vector evaluateError(const Pose3& pose, const LieVector& landmark,
+  Vector evaluateError(const Pose3& pose, const Vector6& landmark,
       boost::optional<gtsam::Matrix&> H1=boost::none,
       boost::optional<gtsam::Matrix&> H2=boost::none) const {
 

gtsam_unstable/slam/InvDepthFactorVariant2.h

@@ -23,7 +23,7 @@ namespace gtsam {
 /**
  * Binary factor representing a visual measurement using an inverse-depth parameterization
  */
-class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, LieVector> {
+class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, Vector3> {
 protected:
 
   // Keep a copy of measurement and calibration for I/O
@@ -34,7 +34,7 @@ protected:
 public:
 
   /// shorthand for base class type
-  typedef NoiseModelFactor2<Pose3, LieVector> Base;
+  typedef NoiseModelFactor2<Pose3, Vector3> Base;
 
   /// shorthand for this class
   typedef InvDepthFactorVariant2 This;
@@ -82,7 +82,7 @@ public:
         && this->referencePoint_.equals(e->referencePoint_, tol);
   }
 
-  Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const {
+  Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {
     try {
       // Calculate the 3D coordinates of the landmark in the world frame
       double theta = landmark(0), phi = landmark(1), rho = landmark(2);
@@ -102,7 +102,7 @@ public:
   }
 
   /// Evaluate error h(x)-z and optionally derivatives
-  Vector evaluateError(const Pose3& pose, const LieVector& landmark,
+  Vector evaluateError(const Pose3& pose, const Vector3& landmark,
       boost::optional<gtsam::Matrix&> H1=boost::none,
       boost::optional<gtsam::Matrix&> H2=boost::none) const {
 

gtsam_unstable/slam/InvDepthFactorVariant3.h

@@ -22,7 +22,7 @@ namespace gtsam {
 /**
  * Binary factor representing the first visual measurement using an inverse-depth parameterization
  */
-class InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, LieVector> {
+class InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, Vector3> {
 protected:
 
   // Keep a copy of measurement and calibration for I/O
@@ -32,7 +32,7 @@ protected:
 public:
 
   /// shorthand for base class type
-  typedef NoiseModelFactor2<Pose3, LieVector> Base;
+  typedef NoiseModelFactor2<Pose3, Vector3> Base;
 
   /// shorthand for this class
   typedef InvDepthFactorVariant3a This;
@@ -80,7 +80,7 @@ public:
         && this->K_->equals(*e->K_, tol);
   }
 
-  Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const {
+  Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {
     try {
       // Calculate the 3D coordinates of the landmark in the Pose frame
       double theta = landmark(0), phi = landmark(1), rho = landmark(2);
@@ -102,7 +102,7 @@ public:
   }
 
   /// Evaluate error h(x)-z and optionally derivatives
-  Vector evaluateError(const Pose3& pose, const LieVector& landmark,
+  Vector evaluateError(const Pose3& pose, const Vector3& landmark,
       boost::optional<gtsam::Matrix&> H1=boost::none,
       boost::optional<gtsam::Matrix&> H2=boost::none) const {
 
@@ -141,7 +141,7 @@ private:
 /**
  * Ternary factor representing a visual measurement using an inverse-depth parameterization
  */
-class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, LieVector> {
+class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, Vector3> {
 protected:
 
   // Keep a copy of measurement and calibration for I/O
@@ -151,7 +151,7 @@ protected:
 public:
 
   /// shorthand for base class type
-  typedef NoiseModelFactor3<Pose3, Pose3, LieVector> Base;
+  typedef NoiseModelFactor3<Pose3, Pose3, Vector3> Base;
 
   /// shorthand for this class
   typedef InvDepthFactorVariant3b This;
@@ -199,7 +199,7 @@ public:
         && this->K_->equals(*e->K_, tol);
   }
 
-  Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const LieVector& landmark) const {
+  Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark) const {
     try {
       // Calculate the 3D coordinates of the landmark in the Pose1 frame
       double theta = landmark(0), phi = landmark(1), rho = landmark(2);
@@ -221,20 +221,19 @@ public:
   }
 
   /// Evaluate error h(x)-z and optionally derivatives
-  Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const LieVector& landmark,
+  Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark,
       boost::optional<gtsam::Matrix&> H1=boost::none,
       boost::optional<gtsam::Matrix&> H2=boost::none,
       boost::optional<gtsam::Matrix&> H3=boost::none) const {
 
-    if(H1) {
+    if(H1)
       (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1);
-    }
+
-    if(H2) {
+    if(H2)
       (*H2) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2);
-    }
+
-    if(H3) {
+    if(H3)
       (*H3) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark);
-    }
 
     return inverseDepthError(pose1, pose2, landmark);
   }

gtsam_unstable/slam/tests/testInvDepthFactor3.cpp

@@ -25,7 +25,7 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
 Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
 SimpleCamera level_camera(level_pose, *K);
 
-typedef InvDepthFactor3<Pose3, LieVector, LieScalar> InverseDepthFactor;
+typedef InvDepthFactor3<Pose3, Vector5, double> InverseDepthFactor;
 typedef NonlinearEquality<Pose3> PoseConstraint;
 
 /* ************************************************************************* */
@@ -38,10 +38,10 @@ TEST( InvDepthFactor, optimize) {
   Point2 expected_uv = level_camera.project(landmark);
 
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
-  LieVector inv_landmark((Vector(5) << 0., 0., 1., 0., 0.));
+  Vector5 inv_landmark((Vector(5) << 0., 0., 1., 0., 0.));
   // initialize inverse depth with "incorrect" depth of 1/4
   // in reality this is 1/5, but initial depth is guessed
-  LieScalar inv_depth(1./4);
+  double inv_depth(1./4);
 
   gtsam::NonlinearFactorGraph graph;
   Values initial;
@@ -82,8 +82,8 @@ TEST( InvDepthFactor, optimize) {
   Values result2 = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
   
   Point3 result2_lmk = InvDepthCamera3<Cal3_S2>::invDepthTo3D(
-      result2.at<LieVector>(Symbol('l',1)),
+      result2.at<Vector5>(Symbol('l',1)),
-      result2.at<LieScalar>(Symbol('d',1)));
+      result2.at<double>(Symbol('d',1)));
   EXPECT(assert_equal(landmark, result2_lmk, 1e-9));
   
   // TODO: need to add priors to make this work with

gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp

@@ -45,7 +45,7 @@ TEST( InvDepthFactorVariant1, optimize) {
   double theta = atan2(ray.y(), ray.x());
   double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
   double rho = 1./ray.norm();
-  LieVector expected((Vector(6) << x, y, z, theta, phi, rho));
+  Vector6 expected((Vector(6) << x, y, z, theta, phi, rho));
 
 
   
@@ -68,12 +68,12 @@ TEST( InvDepthFactorVariant1, optimize) {
   Values values;
   values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
   values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
-  values.insert(landmarkKey, expected.retract((Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05)));
+  values.insert(landmarkKey, Vector6(expected + (Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05)));
 
   // Optimize the graph to recover the actual landmark position
   LevenbergMarquardtParams params;
   Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
-  LieVector actual = result.at<LieVector>(landmarkKey);
+  Vector6 actual = result.at<Vector6>(landmarkKey);
   
 
 //  values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
@@ -84,22 +84,22 @@ TEST( InvDepthFactorVariant1, optimize) {
 //  result.at<Pose3>(poseKey2).print("Pose2 After:\n");
 //  pose2.print("Pose2 Truth:\n");
 //  cout << endl << endl;
-//  values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
+//  values.at<Vector6>(landmarkKey).print("Landmark Before:\n");
-//  result.at<LieVector>(landmarkKey).print("Landmark After:\n");
+//  result.at<Vector6>(landmarkKey).print("Landmark After:\n");
 //  expected.print("Landmark Truth:\n");
 //  cout << endl << endl;
 
   // Calculate world coordinates of landmark versions
   Point3 world_landmarkBefore;
   {
-    LieVector landmarkBefore = values.at<LieVector>(landmarkKey);
+    Vector6 landmarkBefore = values.at<Vector6>(landmarkKey);
     double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2);
     double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5);
     world_landmarkBefore = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
   }
   Point3 world_landmarkAfter;
   {
-    LieVector landmarkAfter = result.at<LieVector>(landmarkKey);
+    Vector6 landmarkAfter = result.at<Vector6>(landmarkKey);
     double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2);
     double theta = landmarkAfter(3), phi = landmarkAfter(4), rho = landmarkAfter(5);
     world_landmarkAfter = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);

gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp

@@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant2, optimize) {
   double theta = atan2(ray.y(), ray.x());
   double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
   double rho = 1./ray.norm();
-  LieVector expected((Vector(3) << theta, phi, rho));
+  Vector3 expected((Vector(3) << theta, phi, rho));
 
 
   
@@ -66,12 +66,12 @@ TEST( InvDepthFactorVariant2, optimize) {
   Values values;
   values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
   values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
-  values.insert(landmarkKey, expected.retract((Vector(3) << +0.02, -0.04, +0.05)));
+  values.insert(landmarkKey, Vector3(expected + (Vector(3) << +0.02, -0.04, +0.05)));
 
   // Optimize the graph to recover the actual landmark position
   LevenbergMarquardtParams params;
   Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
-  LieVector actual = result.at<LieVector>(landmarkKey);
+  Vector3 actual = result.at<Vector3>(landmarkKey);
   
 //  values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
 //  result.at<Pose3>(poseKey1).print("Pose1 After:\n");
@@ -81,21 +81,21 @@ TEST( InvDepthFactorVariant2, optimize) {
 //  result.at<Pose3>(poseKey2).print("Pose2 After:\n");
 //  pose2.print("Pose2 Truth:\n");
 //  std::cout << std::endl << std::endl;
-//  values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
+//  values.at<Vector3>(landmarkKey).print("Landmark Before:\n");
-//  result.at<LieVector>(landmarkKey).print("Landmark After:\n");
+//  result.at<Vector3>(landmarkKey).print("Landmark After:\n");
 //  expected.print("Landmark Truth:\n");
 //  std::cout << std::endl << std::endl;
 
   // Calculate world coordinates of landmark versions
   Point3 world_landmarkBefore;
   {
-    LieVector landmarkBefore = values.at<LieVector>(landmarkKey);
+    Vector3 landmarkBefore = values.at<Vector3>(landmarkKey);
     double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2);
     world_landmarkBefore = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
   }
   Point3 world_landmarkAfter;
   {
-    LieVector landmarkAfter = result.at<LieVector>(landmarkKey);
+    Vector3 landmarkAfter = result.at<Vector3>(landmarkKey);
     double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2);
     world_landmarkAfter = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
   }
@@ -106,7 +106,7 @@ TEST( InvDepthFactorVariant2, optimize) {
 //  std::cout << std::endl << std::endl;
 
   // Test that the correct landmark parameters have been recovered
-  EXPECT(assert_equal(expected, actual, 1e-9));
+  EXPECT(assert_equal((Vector)expected, actual, 1e-9));
 }
 
 

gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp

@@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant3, optimize) {
   double theta = atan2(landmark_p1.x(), landmark_p1.z());
   double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z()));
   double rho = 1./landmark_p1.norm();
-  LieVector expected((Vector(3) << theta, phi, rho));
+  Vector3 expected((Vector(3) << theta, phi, rho));
 
 
   
@@ -66,17 +66,17 @@ TEST( InvDepthFactorVariant3, optimize) {
   Values values;
   values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
   values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
-  values.insert(landmarkKey, expected.retract((Vector(3) <<  +0.02, -0.04, +0.05)));
+  values.insert(landmarkKey, Vector3(expected + (Vector(3) <<  +0.02, -0.04, +0.05)));
 
   // Optimize the graph to recover the actual landmark position
   LevenbergMarquardtParams params;
   Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
-  LieVector actual = result.at<LieVector>(landmarkKey);
+  Vector3 actual = result.at<Vector3>(landmarkKey);
   
 
 
   // Test that the correct landmark parameters have been recovered
-  EXPECT(assert_equal(expected, actual, 1e-9));
+  EXPECT(assert_equal((Vector)expected, actual, 1e-9));
 }