Most of LieXXX is gone, greatly simplifies code. All tests pass.

Merge remote-tracking branch 'origin/feature/BAD_noLieXX' into feature/BAD

.cproject

@@ -736,14 +736,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testImuFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testImuFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="testInvDepthFactor3.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -2265,6 +2257,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testVelocityConstraint3.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testVelocityConstraint3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testDiscreteBayesTree.run" path="build/gtsam/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j1</buildArguments>
@@ -2766,10 +2766,10 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBetweenFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testPriorFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
-				<buildTarget>testBetweenFactor.run</buildTarget>
+				<buildTarget>testPriorFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>

gtsam/geometry/tests/testPoint3.cpp

@@ -16,7 +16,6 @@
 
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/base/Testable.h>
-#include <gtsam/base/LieScalar.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
 
@@ -90,8 +89,8 @@ TEST (Point3, normalize) {
 }
 
 //*************************************************************************
-LieScalar norm_proxy(const Point3& point) {
-  return LieScalar(point.norm());
+double norm_proxy(const Point3& point) {
+  return double(point.norm());
 }
 
 TEST (Point3, norm) {
@@ -99,7 +98,7 @@ TEST (Point3, norm) {
   Point3 point(3,4,5); // arbitrary point
   double expected = sqrt(50);
   EXPECT_DOUBLES_EQUAL(expected, point.norm(actualH), 1e-8);
-  Matrix expectedH = numericalDerivative11<LieScalar, Point3>(norm_proxy, point);
+  Matrix expectedH = numericalDerivative11<double, Point3>(norm_proxy, point);
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 

gtsam/linear/tests/testHessianFactor.cpp

@@ -276,8 +276,8 @@ TEST(HessianFactor, CombineAndEliminate)
       1.0, 0.0, 0.0,
       0.0, 1.0, 0.0,
       0.0, 0.0, 1.0);
-  Vector b0 = (Vector(3) << 1.5, 1.5, 1.5);
-  Vector s0 = (Vector(3) << 1.6, 1.6, 1.6);
+  Vector3 b0(1.5, 1.5, 1.5);
+  Vector3 s0(1.6, 1.6, 1.6);
 
   Matrix A10 = (Matrix(3,3) <<
       2.0, 0.0, 0.0,
@@ -287,15 +287,15 @@ TEST(HessianFactor, CombineAndEliminate)
       -2.0, 0.0, 0.0,
       0.0, -2.0, 0.0,
       0.0, 0.0, -2.0);
-  Vector b1 = (Vector(3) << 2.5, 2.5, 2.5);
-  Vector s1 = (Vector(3) << 2.6, 2.6, 2.6);
+  Vector3 b1(2.5, 2.5, 2.5);
+  Vector3 s1(2.6, 2.6, 2.6);
 
   Matrix A21 = (Matrix(3,3) <<
       3.0, 0.0, 0.0,
       0.0, 3.0, 0.0,
       0.0, 0.0, 3.0);
-  Vector b2 = (Vector(3) << 3.5, 3.5, 3.5);
-  Vector s2 = (Vector(3) << 3.6, 3.6, 3.6);
+  Vector3 b2(3.5, 3.5, 3.5);
+  Vector3 s2(3.6, 3.6, 3.6);
 
   GaussianFactorGraph gfg;
   gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@@ -305,8 +305,8 @@ TEST(HessianFactor, CombineAndEliminate)
   Matrix zero3x3 = zeros(3,3);
   Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
   Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
-  Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
-  Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
+  Vector9 b; b << b1, b0, b2;
+  Vector9 sigmas; sigmas << s1, s0, s2;
 
   // create a full, uneliminated version of the factor
   JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));

gtsam/linear/tests/testJacobianFactor.cpp

@@ -369,8 +369,8 @@ TEST(JacobianFactor, eliminate)
     1.0, 0.0, 0.0,
     0.0, 1.0, 0.0,
     0.0, 0.0, 1.0);
-  Vector b0 = (Vector(3) << 1.5, 1.5, 1.5);
-  Vector s0 = (Vector(3) << 1.6, 1.6, 1.6);
+  Vector3 b0(1.5, 1.5, 1.5);
+  Vector3 s0(1.6, 1.6, 1.6);
 
   Matrix A10 = (Matrix(3, 3) <<
     2.0, 0.0, 0.0,
@@ -380,15 +380,15 @@ TEST(JacobianFactor, eliminate)
     -2.0, 0.0, 0.0,
     0.0, -2.0, 0.0,
     0.0, 0.0, -2.0);
-  Vector b1 = (Vector(3) << 2.5, 2.5, 2.5);
-  Vector s1 = (Vector(3) << 2.6, 2.6, 2.6);
+  Vector3 b1(2.5, 2.5, 2.5);
+  Vector3 s1(2.6, 2.6, 2.6);
 
   Matrix A21 = (Matrix(3, 3) <<
     3.0, 0.0, 0.0,
     0.0, 3.0, 0.0,
     0.0, 0.0, 3.0);
-  Vector b2 = (Vector(3) << 3.5, 3.5, 3.5);
-  Vector s2 = (Vector(3) << 3.6, 3.6, 3.6);
+  Vector3 b2(3.5, 3.5, 3.5);
+  Vector3 s2(3.6, 3.6, 3.6);
 
   GaussianFactorGraph gfg;
   gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@@ -398,8 +398,8 @@ TEST(JacobianFactor, eliminate)
   Matrix zero3x3 = zeros(3,3);
   Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
   Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
-  Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
-  Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
+  Vector9 b; b << b1, b0, b2;
+  Vector9 sigmas; sigmas << s1, s0, s2;
 
   JacobianFactor combinedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
   GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(list_of(0));

gtsam/navigation/CombinedImuFactor.h

@@ -17,11 +17,10 @@
 #pragma once
 
 /* GTSAM includes */
-#include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/base/LieVector.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/base/debug.h>
 
 /* External or standard includes */
@@ -46,7 +45,7 @@ namespace gtsam {
    * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
    */
 
-  class CombinedImuFactor: public NoiseModelFactor6<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> {
+  class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> {
 
   public:
 
@@ -214,7 +213,7 @@ namespace gtsam {
         Matrix3 H_vel_vel    = I_3x3;
         Matrix3 H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
         // analytic expression corresponding to the following numerical derivative
-        // Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
+        // Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
         Matrix3 H_vel_biasacc = - deltaRij.matrix() * deltaT;
 
         Matrix3 H_angles_pos   = Z_3x3;
@@ -222,7 +221,7 @@ namespace gtsam {
         Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
         Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT;
         // analytic expression corresponding to the following numerical derivative
-        // Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
+        // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
 
         // overall Jacobian wrt preintegrated measurements (df/dx)
         Matrix F(15,15);
@@ -322,7 +321,7 @@ namespace gtsam {
   private:
 
     typedef CombinedImuFactor This;
-    typedef NoiseModelFactor6<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> Base;
+    typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base;
 
     CombinedPreintegratedMeasurements preintegratedMeasurements_;
     Vector3 gravity_;
@@ -412,7 +411,7 @@ namespace gtsam {
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
-    Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
+    Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
         const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none,
@@ -626,7 +625,7 @@ namespace gtsam {
 
 
     /** predicted states from IMU */
-    static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j,
+    static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j,
         const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j,
         const CombinedPreintegratedMeasurements& preintegratedMeasurements,
         const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
@@ -649,7 +648,7 @@ namespace gtsam {
 		  - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
 		  + 0.5 * gravity * deltaTij*deltaTij;
 
-	  vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+	  vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
 		  + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
 		  + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
 		  - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term

gtsam/navigation/ImuBias.h

@@ -144,7 +144,7 @@ namespace imuBias {
     /// return dimensionality of tangent space
     inline size_t dim() const { return dimension; }
 
-    /** Update the LieVector with a tangent space update */
+    /** Update the bias with a tangent space update */
     inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); }
 
     /** @return the local coordinates of another object */

gtsam/navigation/ImuFactor.h

@@ -21,7 +21,6 @@
 #include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/base/debug.h>
 
 /* External or standard includes */
@@ -46,7 +45,7 @@ namespace gtsam {
    * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
    */
 
-  class ImuFactor: public NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> {
+  class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
 
   public:
 
@@ -203,13 +202,13 @@ namespace gtsam {
         Matrix H_vel_vel    = I_3x3;
         Matrix H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
         // analytic expression corresponding to the following numerical derivative
-        // Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
+        // Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
 
         Matrix H_angles_pos   = Z_3x3;
         Matrix H_angles_vel    = Z_3x3;
         Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
         // analytic expression corresponding to the following numerical derivative
-        // Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
+        // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
 
         // overall Jacobian wrt preintegrated measurements (df/dx)
         Matrix F(9,9);
@@ -285,7 +284,7 @@ namespace gtsam {
   private:
 
     typedef ImuFactor This;
-    typedef NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> Base;
+    typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base;
 
     PreintegratedMeasurements preintegratedMeasurements_;
     Vector3 gravity_;
@@ -373,7 +372,7 @@ namespace gtsam {
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
-    Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
+    Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
         const imuBias::ConstantBias& bias,
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none,
@@ -525,7 +524,7 @@ namespace gtsam {
 
 
     /** predicted states from IMU */
-    static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j,
+    static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j,
         const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
         const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
         const bool use2ndOrderCoriolis = false)
@@ -547,7 +546,7 @@ namespace gtsam {
 		  - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
 		  + 0.5 * gravity * deltaTij*deltaTij;
 
-	  vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+	  vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
 		  + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
 		  + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
 		  - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term

gtsam/navigation/MagFactor.h

@@ -19,7 +19,6 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/geometry/Rot3.h>
-#include <gtsam/base/LieScalar.h>
 
 namespace gtsam {
 
@@ -165,7 +164,7 @@ public:
  * This version uses model measured bM = scale * bRn * direction + bias
  * and optimizes for both scale, direction, and the bias.
  */
-class MagFactor3: public NoiseModelFactor3<LieScalar, Unit3, Point3> {
+class MagFactor3: public NoiseModelFactor3<double, Unit3, Point3> {
 
   const Point3 measured_; ///< The measured magnetometer values
   const Rot3 bRn_; ///< The assumed known rotation from nav to body
@@ -175,7 +174,7 @@ public:
   /** Constructor */
   MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
       const Rot3& nRb, const SharedNoiseModel& model) :
-      NoiseModelFactor3<LieScalar, Unit3, Point3>(model, key1, key2, key3), //
+      NoiseModelFactor3<double, Unit3, Point3>(model, key1, key2, key3), //
       measured_(measured), bRn_(nRb.inverse()) {
   }
 
@@ -190,7 +189,7 @@ public:
    * @param nM (unknown) local earth magnetic field vector, in nav frame
    * @param bias (unknown) 3D bias
    */
-  Vector evaluateError(const LieScalar& scale, const Unit3& direction,
+  Vector evaluateError(double scale, const Unit3& direction,
       const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
           boost::none) const {

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -21,7 +21,6 @@
 #include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
@@ -143,9 +142,9 @@ TEST( CombinedImuFactor, ErrorWithBiases )
   imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
   imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
   Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
-  LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
+  Vector3 v1(0.5, 0.0, 0.0);
   Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
-  LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
+  Vector3 v2(0.5, 0.0, 0.0);
 
   // Measurements
   Vector3 gravity; gravity << 0, 0, 9.81;

gtsam/nonlinear/NonlinearFactor.cpp

@@ -18,6 +18,7 @@
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <boost/make_shared.hpp>
+#include <boost/format.hpp>
 
 namespace gtsam {
 
@@ -62,6 +63,7 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
 void NoiseModelFactor::print(const std::string& s,
     const KeyFormatter& keyFormatter) const {
   Base::print(s, keyFormatter);
+  if (noiseModel_)
     noiseModel_->print("  noise model: ");
 }
 
@@ -76,18 +78,19 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
 
 /* ************************************************************************* */
 static void check(const SharedNoiseModel& noiseModel, size_t m) {
-  if (!noiseModel)
-    throw std::invalid_argument("NoiseModelFactor: no NoiseModel.");
-  if (m != noiseModel->dim())
+  if (noiseModel && m != noiseModel->dim())
     throw std::invalid_argument(
-        "NoiseModelFactor was created with a NoiseModel of incorrect dimension.");
+        boost::str(
+            boost::format(
+                "NoiseModelFactor: NoiseModel has dimension %1% instead of %2%.")
+                % noiseModel->dim() % m));
 }
 
 /* ************************************************************************* */
 Vector NoiseModelFactor::whitenedError(const Values& c) const {
   const Vector b = unwhitenedError(c);
   check(noiseModel_, b.size());
-  return noiseModel_->whiten(b);
+  return noiseModel_ ? noiseModel_->whiten(b) : b;
 }
 
 /* ************************************************************************* */
@@ -95,7 +98,10 @@ double NoiseModelFactor::error(const Values& c) const {
   if (active(c)) {
     const Vector b = unwhitenedError(c);
     check(noiseModel_, b.size());
+    if (noiseModel_)
       return 0.5 * noiseModel_->distance(b);
+    else
+      return 0.5 * b.squaredNorm();
   } else {
     return 0.0;
   }
@@ -115,6 +121,7 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
   check(noiseModel_, b.size());
 
   // Whiten the corresponding system now
+  if (noiseModel_)
     noiseModel_->WhitenSystem(A, b);
 
   // Fill in terms, needed to create JacobianFactor below
@@ -125,7 +132,7 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
   }
 
   // TODO pass unwhitened + noise model to Gaussian factor
-  if (noiseModel_->is_constrained())
+  if (noiseModel_ && noiseModel_->is_constrained())
     return GaussianFactor::shared_ptr(
         new JacobianFactor(terms, b,
             boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()));

gtsam/nonlinear/WhiteNoiseFactor.h

@@ -19,7 +19,6 @@
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/linear/HessianFactor.h>
-#include <gtsam/base/LieScalar.h>
 #include <cmath>
 
 namespace gtsam {
@@ -126,7 +125,7 @@ namespace gtsam {
 
     /// Calculate the error of the factor, typically equal to log-likelihood
     inline double error(const Values& x) const {
-      return f(z_, x.at<LieScalar>(meanKey_), x.at<LieScalar>(precisionKey_));
+      return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
     }
 
     /**
@@ -155,8 +154,8 @@ namespace gtsam {
 
     /// linearize returns a Hessianfactor that is an approximation of error(p)
     virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
-      double u = x.at<LieScalar>(meanKey_);
-      double p = x.at<LieScalar>(precisionKey_);
+      double u = x.at<double>(meanKey_);
+      double p = x.at<double>(precisionKey_);
       Key j1 = meanKey_;
       Key j2 = precisionKey_;
       return linearize(z_, u, p, j1, j2);

gtsam/nonlinear/tests/testValues.cpp

@@ -21,7 +21,6 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/TestableAssertions.h>
-#include <gtsam/base/LieVector.h>
 
 #include <CppUnitLite/TestHarness.h>
 #include <boost/assign/std/list.hpp> // for operator +=
@@ -74,7 +73,7 @@ struct dimension<TestValue> : public boost::integral_constant<int, 0> {};
 TEST( Values, equals1 )
 {
   Values expected;
-  LieVector v((Vector(3) << 5.0, 6.0, 7.0));
+  Vector3 v(5.0, 6.0, 7.0);
 
   expected.insert(key1, v);
   Values actual;
@@ -86,8 +85,8 @@ TEST( Values, equals1 )
 TEST( Values, equals2 )
 {
   Values cfg1, cfg2;
-  LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
-  LieVector v2((Vector(3) << 5.0, 6.0, 8.0));
+  Vector3 v1(5.0, 6.0, 7.0);
+  Vector3 v2(5.0, 6.0, 8.0);
 
   cfg1.insert(key1, v1);
   cfg2.insert(key1, v2);
@@ -99,8 +98,8 @@ TEST( Values, equals2 )
 TEST( Values, equals_nan )
 {
   Values cfg1, cfg2;
-  LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
-  LieVector v2((Vector(3) << inf, inf, inf));
+  Vector3 v1(5.0, 6.0, 7.0);
+  Vector3 v2(inf, inf, inf);
 
   cfg1.insert(key1, v1);
   cfg2.insert(key1, v2);
@@ -112,10 +111,10 @@ TEST( Values, equals_nan )
 TEST( Values, insert_good )
 {
   Values cfg1, cfg2, expected;
-  LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
-  LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
-  LieVector v3((Vector(3) << 2.0, 4.0, 3.0));
-  LieVector v4((Vector(3) << 8.0, 3.0, 7.0));
+  Vector3 v1(5.0, 6.0, 7.0);
+  Vector3 v2(8.0, 9.0, 1.0);
+  Vector3 v3(2.0, 4.0, 3.0);
+  Vector3 v4(8.0, 3.0, 7.0);
 
   cfg1.insert(key1, v1);
   cfg1.insert(key2, v2);
@@ -134,10 +133,10 @@ TEST( Values, insert_good )
 TEST( Values, insert_bad )
 {
   Values cfg1, cfg2;
-  LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
-  LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
-  LieVector v3((Vector(3) << 2.0, 4.0, 3.0));
-  LieVector v4((Vector(3) << 8.0, 3.0, 7.0));
+  Vector3 v1(5.0, 6.0, 7.0);
+  Vector3 v2(8.0, 9.0, 1.0);
+  Vector3 v3(2.0, 4.0, 3.0);
+  Vector3 v4(8.0, 3.0, 7.0);
 
   cfg1.insert(key1, v1);
   cfg1.insert(key2, v2);
@@ -151,16 +150,16 @@ TEST( Values, insert_bad )
 TEST( Values, update_element )
 {
   Values cfg;
-  LieVector v1((Vector(3) << 5.0, 6.0, 7.0));
-  LieVector v2((Vector(3) << 8.0, 9.0, 1.0));
+  Vector3 v1(5.0, 6.0, 7.0);
+  Vector3 v2(8.0, 9.0, 1.0);
 
   cfg.insert(key1, v1);
   CHECK(cfg.size() == 1);
-  CHECK(assert_equal(v1, cfg.at<LieVector>(key1)));
+  CHECK(assert_equal((Vector)v1, cfg.at<Vector3>(key1)));
 
   cfg.update(key1, v2);
   CHECK(cfg.size() == 1);
-  CHECK(assert_equal(v2, cfg.at<LieVector>(key1)));
+  CHECK(assert_equal((Vector)v2, cfg.at<Vector3>(key1)));
 }
 
 /* ************************************************************************* */
@@ -168,10 +167,10 @@ TEST(Values, basic_functions)
 {
   Values values;
   const Values& values_c = values;
-  values.insert(2, LieVector());
-  values.insert(4, LieVector());
-  values.insert(6, LieVector());
-  values.insert(8, LieVector());
+  values.insert(2, Vector3());
+  values.insert(4, Vector3());
+  values.insert(6, Vector3());
+  values.insert(8, Vector3());
 
   // find
   EXPECT_LONGS_EQUAL(4, values.find(4)->key);
@@ -195,8 +194,8 @@ TEST(Values, basic_functions)
 //TEST(Values, dim_zero)
 //{
 //  Values config0;
-//  config0.insert(key1, LieVector((Vector(2) << 2.0, 3.0));
-//  config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0));
+//  config0.insert(key1, Vector2((Vector(2) << 2.0, 3.0));
+//  config0.insert(key2, Vector3(5.0, 6.0, 7.0));
 //  LONGS_EQUAL(5, config0.dim());
 //
 //  VectorValues expected;
@@ -209,16 +208,16 @@ TEST(Values, basic_functions)
 TEST(Values, expmap_a)
 {
   Values config0;
-  config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
-  config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
+  config0.insert(key1, Vector3(1.0, 2.0, 3.0));
+  config0.insert(key2, Vector3(5.0, 6.0, 7.0));
 
   VectorValues increment = pair_list_of<Key, Vector>
     (key1, (Vector(3) << 1.0, 1.1, 1.2))
     (key2, (Vector(3) << 1.3, 1.4, 1.5));
 
   Values expected;
-  expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2)));
-  expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
+  expected.insert(key1, Vector3(2.0, 3.1, 4.2));
+  expected.insert(key2, Vector3(6.3, 7.4, 8.5));
 
   CHECK(assert_equal(expected, config0.retract(increment)));
 }
@@ -227,15 +226,15 @@ TEST(Values, expmap_a)
 TEST(Values, expmap_b)
 {
   Values config0;
-  config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
-  config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
+  config0.insert(key1, Vector3(1.0, 2.0, 3.0));
+  config0.insert(key2, Vector3(5.0, 6.0, 7.0));
 
   VectorValues increment = pair_list_of<Key, Vector>
     (key2, (Vector(3) << 1.3, 1.4, 1.5));
 
   Values expected;
-  expected.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
-  expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
+  expected.insert(key1, Vector3(1.0, 2.0, 3.0));
+  expected.insert(key2, Vector3(6.3, 7.4, 8.5));
 
   CHECK(assert_equal(expected, config0.retract(increment)));
 }
@@ -244,16 +243,16 @@ TEST(Values, expmap_b)
 //TEST(Values, expmap_c)
 //{
 //  Values config0;
-//  config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
-//  config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
+//  config0.insert(key1, Vector3(1.0, 2.0, 3.0));
+//  config0.insert(key2, Vector3(5.0, 6.0, 7.0));
 //
-//  Vector increment = LieVector(6,
+//  Vector increment = Vector6(
 //      1.0, 1.1, 1.2,
 //      1.3, 1.4, 1.5);
 //
 //  Values expected;
-//  expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2)));
-//  expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5)));
+//  expected.insert(key1, Vector3(2.0, 3.1, 4.2));
+//  expected.insert(key2, Vector3(6.3, 7.4, 8.5));
 //
 //  CHECK(assert_equal(expected, config0.retract(increment)));
 //}
@@ -262,8 +261,8 @@ TEST(Values, expmap_b)
 TEST(Values, expmap_d)
 {
   Values config0;
-  config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
-  config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
+  config0.insert(key1, Vector3(1.0, 2.0, 3.0));
+  config0.insert(key2, Vector3(5.0, 6.0, 7.0));
   //config0.print("config0");
   CHECK(equal(config0, config0));
   CHECK(config0.equals(config0));
@@ -280,8 +279,8 @@ TEST(Values, expmap_d)
 TEST(Values, localCoordinates)
 {
   Values valuesA;
-  valuesA.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0)));
-  valuesA.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)));
+  valuesA.insert(key1, Vector3(1.0, 2.0, 3.0));
+  valuesA.insert(key2, Vector3(5.0, 6.0, 7.0));
 
   VectorValues expDelta = pair_list_of<Key, Vector>
     (key1, (Vector(3) << 0.1, 0.2, 0.3))
@@ -317,28 +316,28 @@ TEST(Values, extract_keys)
 TEST(Values, exists_)
 {
   Values config0;
-  config0.insert(key1, LieVector((Vector(1) << 1.)));
-  config0.insert(key2, LieVector((Vector(1) << 2.)));
+  config0.insert(key1, 1.0);
+  config0.insert(key2, 2.0);
 
-  boost::optional<const LieVector&> v = config0.exists<LieVector>(key1);
-  CHECK(assert_equal((Vector(1) << 1.),*v));
+  boost::optional<const double&> v = config0.exists<double>(key1);
+  DOUBLES_EQUAL(1.0,*v,1e-9);
 }
 
 /* ************************************************************************* */
 TEST(Values, update)
 {
   Values config0;
-  config0.insert(key1, LieVector((Vector(1) << 1.)));
-  config0.insert(key2, LieVector((Vector(1) << 2.)));
+  config0.insert(key1, 1.0);
+  config0.insert(key2, 2.0);
 
   Values superset;
-  superset.insert(key1, LieVector((Vector(1) << -1.)));
-  superset.insert(key2, LieVector((Vector(1) << -2.)));
+  superset.insert(key1, -1.0);
+  superset.insert(key2, -2.0);
   config0.update(superset);
 
   Values expected;
-  expected.insert(key1, LieVector((Vector(1) << -1.)));
-  expected.insert(key2, LieVector((Vector(1) << -2.)));
+  expected.insert(key1, -1.0);
+  expected.insert(key2, -2.0);
   CHECK(assert_equal(expected, config0));
 }
 

gtsam/slam/EssentialMatrixFactor.h

@@ -10,7 +10,6 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/EssentialMatrix.h>
 #include <gtsam/geometry/SimpleCamera.h>
-#include <gtsam/base/LieScalar.h>
 #include <iostream>
 
 namespace gtsam {
@@ -85,14 +84,13 @@ public:
  * Binary factor that optimizes for E and inverse depth d: assumes measurement
  * in image 2 is perfect, and returns re-projection error in image 1
  */
-class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix,
-    LieScalar> {
+class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix, double> {
 
   Point3 dP1_; ///< 3D point corresponding to measurement in image 1
   Point2 pn_; ///< Measurement in image 2, in ideal coordinates
   double f_; ///< approximate conversion factor for error scaling
 
-  typedef NoiseModelFactor2<EssentialMatrix, LieScalar> Base;
+  typedef NoiseModelFactor2<EssentialMatrix, double> Base;
   typedef EssentialMatrixFactor2 This;
 
 public:
@@ -149,7 +147,7 @@ public:
    * @param E essential matrix
    * @param d inverse depth d
    */
-  Vector evaluateError(const EssentialMatrix& E, const LieScalar& d,
+  Vector evaluateError(const EssentialMatrix& E, const double& d,
       boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
           boost::none) const {
 
@@ -237,7 +235,8 @@ public:
    */
   template<class CALIBRATION>
   EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
-      const Rot3& cRb, const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) :
+      const Rot3& cRb, const SharedNoiseModel& model,
+      boost::shared_ptr<CALIBRATION> K) :
       EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {
   }
 
@@ -259,7 +258,7 @@ public:
    * @param E essential matrix
    * @param d inverse depth d
    */
-  Vector evaluateError(const EssentialMatrix& E, const LieScalar& d,
+  Vector evaluateError(const EssentialMatrix& E, const double& d,
       boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
           boost::none) const {
     if (!DE) {

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -164,17 +164,17 @@ TEST (EssentialMatrixFactor2, factor) {
     Vector expected = reprojectionError.vector();
 
     Matrix Hactual1, Hactual2;
-    LieScalar d(baseline / P1.z());
+    double d(baseline / P1.z());
     Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
     // Use numerical derivatives to calculate the expected Jacobian
     Matrix Hexpected1, Hexpected2;
-    boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
-        boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
-            boost::none, boost::none);
-    Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, trueE, d);
-    Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, trueE, d);
+    boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
+        &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
+        boost::none);
+    Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, trueE, d);
+    Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, trueE, d);
 
     // Verify the Jacobian is correct
     EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@@ -197,7 +197,7 @@ TEST (EssentialMatrixFactor2, minimization) {
   truth.insert(100, trueE);
   for (size_t i = 0; i < 5; i++) {
     Point3 P1 = data.tracks[i].p;
-    truth.insert(i, LieScalar(baseline / P1.z()));
+    truth.insert(i, double(baseline / P1.z()));
   }
   EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
 
@@ -211,7 +211,7 @@ TEST (EssentialMatrixFactor2, minimization) {
   EssentialMatrix actual = result.at<EssentialMatrix>(100);
   EXPECT(assert_equal(trueE, actual, 1e-1));
   for (size_t i = 0; i < 5; i++)
-    EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1));
+    EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
 
   // Check error at result
   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
@@ -238,17 +238,17 @@ TEST (EssentialMatrixFactor3, factor) {
     Vector expected = reprojectionError.vector();
 
     Matrix Hactual1, Hactual2;
-    LieScalar d(baseline / P1.z());
+    double d(baseline / P1.z());
     Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
     // Use numerical derivatives to calculate the expected Jacobian
     Matrix Hexpected1, Hexpected2;
-    boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
-        boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
-            boost::none, boost::none);
-    Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, bodyE, d);
-    Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, bodyE, d);
+    boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
+        &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
+        boost::none);
+    Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, bodyE, d);
+    Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, bodyE, d);
 
     // Verify the Jacobian is correct
     EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@@ -270,7 +270,7 @@ TEST (EssentialMatrixFactor3, minimization) {
   truth.insert(100, bodyE);
   for (size_t i = 0; i < 5; i++) {
     Point3 P1 = data.tracks[i].p;
-    truth.insert(i, LieScalar(baseline / P1.z()));
+    truth.insert(i, double(baseline / P1.z()));
   }
   EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
 
@@ -284,7 +284,7 @@ TEST (EssentialMatrixFactor3, minimization) {
   EssentialMatrix actual = result.at<EssentialMatrix>(100);
   EXPECT(assert_equal(bodyE, actual, 1e-1));
   for (size_t i = 0; i < 5; i++)
-    EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1));
+    EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
 
   // Check error at result
   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
@@ -380,17 +380,17 @@ TEST (EssentialMatrixFactor2, extraTest) {
     Vector expected = reprojectionError.vector();
 
     Matrix Hactual1, Hactual2;
-    LieScalar d(baseline / P1.z());
+    double d(baseline / P1.z());
     Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
     // Use numerical derivatives to calculate the expected Jacobian
     Matrix Hexpected1, Hexpected2;
-    boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
-        boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2,
-            boost::none, boost::none);
-    Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, trueE, d);
-    Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, trueE, d);
+    boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
+        &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
+        boost::none);
+    Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, trueE, d);
+    Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, trueE, d);
 
     // Verify the Jacobian is correct
     EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
@@ -413,7 +413,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) {
   truth.insert(100, trueE);
   for (size_t i = 0; i < data.number_tracks(); i++) {
     Point3 P1 = data.tracks[i].p;
-    truth.insert(i, LieScalar(baseline / P1.z()));
+    truth.insert(i, double(baseline / P1.z()));
   }
   EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
 
@@ -427,7 +427,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) {
   EssentialMatrix actual = result.at<EssentialMatrix>(100);
   EXPECT(assert_equal(trueE, actual, 1e-1));
   for (size_t i = 0; i < data.number_tracks(); i++)
-    EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1));
+    EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
 
   // Check error at result
   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
@@ -449,17 +449,17 @@ TEST (EssentialMatrixFactor3, extraTest) {
     Vector expected = reprojectionError.vector();
 
     Matrix Hactual1, Hactual2;
-    LieScalar d(baseline / P1.z());
+    double d(baseline / P1.z());
     Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
     EXPECT(assert_equal(expected, actual, 1e-7));
 
     // Use numerical derivatives to calculate the expected Jacobian
     Matrix Hexpected1, Hexpected2;
-    boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f =
-        boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2,
-            boost::none, boost::none);
-    Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, bodyE, d);
-    Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, bodyE, d);
+    boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
+        &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
+        boost::none);
+    Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, bodyE, d);
+    Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, bodyE, d);
 
     // Verify the Jacobian is correct
     EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));

gtsam/slam/tests/testPriorFactor.cpp

@@ -0,0 +1,28 @@
+/**
+ * @file   testPriorFactor.cpp
+ * @brief  Test PriorFactor
+ * @author Frank Dellaert
+ * @date   Nov 4, 2014
+ */
+
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/base/LieScalar.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+
+// Constructor
+TEST(PriorFactor, Constructor) {
+  SharedNoiseModel model;
+  PriorFactor<LieScalar> factor(1, LieScalar(1.0), model);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam_unstable/dynamics/FullIMUFactor.h

@@ -7,7 +7,6 @@
 #pragma once
 
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
@@ -29,20 +28,20 @@ public:
 protected:
 
   /** measurements from the IMU */
-  Vector accel_, gyro_;
+  Vector3 accel_, gyro_;
   double dt_; /// time between measurements
 
 public:
 
   /** Standard constructor */
-  FullIMUFactor(const Vector& accel, const Vector& gyro,
+  FullIMUFactor(const Vector3& accel, const Vector3& gyro,
       double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
   : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {
     assert(model->dim() == 9);
   }
 
   /** Single IMU vector - imu = [accel, gyro] */
-  FullIMUFactor(const Vector& imu,
+  FullIMUFactor(const Vector6& imu,
       double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
   : Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) {
     assert(imu.size() == 6);
@@ -68,15 +67,15 @@ public:
   void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
     std::string a = "FullIMUFactor: " + s;
     Base::print(a, formatter);
-    gtsam::print(accel_, "accel");
-    gtsam::print(gyro_, "gyro");
+    gtsam::print((Vector)accel_, "accel");
+    gtsam::print((Vector)gyro_, "gyro");
     std::cout << "dt: " << dt_ << std::endl;
   }
 
   // access
-  const Vector& gyro() const { return gyro_; }
-  const Vector& accel() const { return accel_; }
-  Vector z() const { return concatVectors(2, &accel_, &gyro_); }
+  const Vector3& gyro() const { return gyro_; }
+  const Vector3& accel() const { return accel_; }
+  Vector6 z() const { return (Vector6() << accel_, gyro_); }
 
   /**
    * Error evaluation with optional derivatives - calculates
@@ -85,13 +84,13 @@ public:
   virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none) const {
-    Vector z(9);
+    Vector9 z;
     z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang
     z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
     z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang
-    if (H1) *H1 = numericalDerivative21<LieVector, PoseRTV, PoseRTV>(
+    if (H1) *H1 = numericalDerivative21<Vector9, PoseRTV, PoseRTV>(
         boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
-    if (H2) *H2 = numericalDerivative22<LieVector, PoseRTV, PoseRTV>(
+    if (H2) *H2 = numericalDerivative22<Vector9, PoseRTV, PoseRTV>(
         boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
     return z - predict_proxy(x1, x2, dt_);
   }
@@ -107,11 +106,11 @@ public:
 private:
 
   /** copy of the measurement function formulated for numerical derivatives */
-  static LieVector predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
-    Vector hx(9);
+  static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
+    Vector9 hx;
     hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
     hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang
-    return LieVector(hx);
+    return hx;
   }
 };
 

gtsam_unstable/dynamics/IMUFactor.h

@@ -7,7 +7,6 @@
 #pragma once
 
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam_unstable/dynamics/PoseRTV.h>
 
@@ -27,18 +26,18 @@ public:
 protected:
 
   /** measurements from the IMU */
-  Vector accel_, gyro_;
+  Vector3 accel_, gyro_;
   double dt_; /// time between measurements
 
 public:
 
   /** Standard constructor */
-  IMUFactor(const Vector& accel, const Vector& gyro,
+  IMUFactor(const Vector3& accel, const Vector3& gyro,
       double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
   : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {}
 
   /** Full IMU vector specification */
-  IMUFactor(const Vector& imu_vector,
+  IMUFactor(const Vector6& imu_vector,
       double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
   : Base(model, key1, key2), accel_(imu_vector.head(3)), gyro_(imu_vector.tail(3)), dt_(dt) {}
 
@@ -61,15 +60,15 @@ public:
   void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
     std::string a = "IMUFactor: " + s;
     Base::print(a, formatter);
-    gtsam::print(accel_, "accel");
-    gtsam::print(gyro_, "gyro");
+    gtsam::print((Vector)accel_, "accel");
+    gtsam::print((Vector)gyro_, "gyro");
     std::cout << "dt: " << dt_ << std::endl;
   }
 
   // access
-  const Vector& gyro() const { return gyro_; }
-  const Vector& accel() const { return accel_; }
-  Vector z() const { return concatVectors(2, &accel_, &gyro_); }
+  const Vector3& gyro() const { return gyro_; }
+  const Vector3& accel() const { return accel_; }
+  Vector6 z() const { return (Vector6() << accel_, gyro_);}
 
   /**
    * Error evaluation with optional derivatives - calculates
@@ -78,10 +77,10 @@ public:
   virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none) const {
-    const Vector meas = z();
-    if (H1) *H1 = numericalDerivative21<LieVector, PoseRTV, PoseRTV>(
+    const Vector6 meas = z();
+    if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
         boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
-    if (H2) *H2 = numericalDerivative22<LieVector, PoseRTV, PoseRTV>(
+    if (H2) *H2 = numericalDerivative22<Vector6, PoseRTV, PoseRTV>(
         boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
     return predict_proxy(x1, x2, dt_, meas);
   }
@@ -96,10 +95,10 @@ public:
 
 private:
   /** copy of the measurement function formulated for numerical derivatives */
-  static LieVector predict_proxy(const PoseRTV& x1, const PoseRTV& x2,
-      double dt, const Vector& meas) {
-    Vector hx = x1.imuPrediction(x2, dt);
-    return LieVector(meas - hx);
+  static Vector6 predict_proxy(const PoseRTV& x1, const PoseRTV& x2,
+      double dt, const Vector6& meas) {
+    Vector6 hx = x1.imuPrediction(x2, dt);
+    return meas - hx;
   }
 };
 

gtsam_unstable/dynamics/Pendulum.h

@@ -9,7 +9,6 @@
 
 #pragma once
 
-#include <gtsam/base/LieScalar.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace gtsam {
@@ -21,11 +20,11 @@ namespace gtsam {
  *    - For implicit Euler method:  q_{k+1} = q_k + h*v_{k+1}
  *    - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1}
  */
-class PendulumFactor1: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
+class PendulumFactor1: public NoiseModelFactor3<double, double, double> {
 public:
 
 protected:
-  typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
+  typedef NoiseModelFactor3<double, double, double> Base;
 
   /** default constructor to allow for serialization */
   PendulumFactor1() {}
@@ -38,7 +37,7 @@ public:
 
   ///Constructor.  k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step
   PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0)
-  : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), k1, k, velKey), h_(h) {}
+  : Base(noiseModel::Constrained::All(1, fabs(mu)), k1, k, velKey), h_(h) {}
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
@@ -46,15 +45,15 @@ public:
         gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
 
   /** q_k + h*v - q_k1 = 0, with optional derivatives */
-  Vector evaluateError(const LieScalar& qk1, const LieScalar& qk, const LieScalar& v,
+  Vector evaluateError(const double& qk1, const double& qk, const double& v,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none) const {
-    const size_t p = LieScalar::Dim();
+    const size_t p = 1;
     if (H1) *H1 = -eye(p);
     if (H2) *H2 = eye(p);
     if (H3) *H3 = eye(p)*h_;
-    return qk1.localCoordinates(qk.compose(LieScalar(v*h_)));
+    return (Vector(1) << qk+v*h_-qk1);
   }
 
 }; // \PendulumFactor1
@@ -67,11 +66,11 @@ public:
  *    - For implicit Euler method:  v_{k+1} = v_k - h*g/L*sin(q_{k+1})
  *    - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k)
  */
-class PendulumFactor2: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
+class PendulumFactor2: public NoiseModelFactor3<double, double, double> {
 public:
 
 protected:
-  typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
+  typedef NoiseModelFactor3<double, double, double> Base;
 
   /** default constructor to allow for serialization */
   PendulumFactor2() {}
@@ -86,7 +85,7 @@ public:
 
   ///Constructor.  vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step
   PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0)
-  : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
+  : Base(noiseModel::Constrained::All(1, fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
@@ -94,15 +93,15 @@ public:
         gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
 
   /**  v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */
-  Vector evaluateError(const LieScalar& vk1, const LieScalar& vk, const LieScalar& q,
+  Vector evaluateError(const double & vk1, const double & vk, const double & q,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none) const {
-    const size_t p = LieScalar::Dim();
+    const size_t p = 1;
     if (H1) *H1 = -eye(p);
     if (H2) *H2 = eye(p);
-    if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q.value());
-    return vk1.localCoordinates(LieScalar(vk - h_*g_/r_*sin(q)));
+    if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q);
+    return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1);
   }
 
 }; // \PendulumFactor2
@@ -114,11 +113,11 @@ public:
  *    p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right)
  *    = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1})
  */
-class PendulumFactorPk: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
+class PendulumFactorPk: public NoiseModelFactor3<double, double, double> {
 public:
 
 protected:
-  typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
+  typedef NoiseModelFactor3<double, double, double> Base;
 
   /** default constructor to allow for serialization */
   PendulumFactorPk() {}
@@ -136,7 +135,7 @@ public:
   ///Constructor
   PendulumFactorPk(Key pKey, Key qKey, Key qKey1,
       double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
-  : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey, qKey, qKey1),
+  : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey, qKey, qKey1),
     h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
 
   /// @return a deep copy of this factor
@@ -145,11 +144,11 @@ public:
         gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); }
 
   /**  1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */
-  Vector evaluateError(const LieScalar& pk, const LieScalar& qk, const LieScalar& qk1,
+  Vector evaluateError(const double & pk, const double & qk, const double & qk1,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none) const {
-    const size_t p = LieScalar::Dim();
+    const size_t p = 1;
 
     double qmid = (1-alpha_)*qk + alpha_*qk1;
     double mr2_h = 1/h_*m_*r_*r_;
@@ -159,7 +158,7 @@ public:
     if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
     if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
 
-    return pk.localCoordinates(LieScalar(mr2_h*(qk1-qk) + mgrh*(1-alpha_)*sin(qmid)));
+    return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk);
   }
 
 }; // \PendulumFactorPk
@@ -170,11 +169,11 @@ public:
  *    p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right)
  *    = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1})
  */
-class PendulumFactorPk1: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
+class PendulumFactorPk1: public NoiseModelFactor3<double, double, double> {
 public:
 
 protected:
-  typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
+  typedef NoiseModelFactor3<double, double, double> Base;
 
   /** default constructor to allow for serialization */
   PendulumFactorPk1() {}
@@ -192,7 +191,7 @@ public:
   ///Constructor
   PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1,
       double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
-  : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey1, qKey, qKey1),
+  : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey1, qKey, qKey1),
     h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
 
   /// @return a deep copy of this factor
@@ -201,11 +200,11 @@ public:
         gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); }
 
   /**  1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */
-  Vector evaluateError(const LieScalar& pk1, const LieScalar& qk, const LieScalar& qk1,
+  Vector evaluateError(const double & pk1, const double & qk, const double & qk1,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none) const {
-    const size_t p = LieScalar::Dim();
+    const size_t p = 1;
 
     double qmid = (1-alpha_)*qk + alpha_*qk1;
     double mr2_h = 1/h_*m_*r_*r_;
@@ -215,7 +214,7 @@ public:
     if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
     if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
 
-    return pk1.localCoordinates(LieScalar(mr2_h*(qk1-qk) - mgrh*alpha_*sin(qmid)));
+    return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1);
   }
 
 }; // \PendulumFactorPk1

gtsam_unstable/dynamics/PoseRTV.cpp

@@ -57,18 +57,17 @@ void PoseRTV::print(const string& s) const {
 }
 
 /* ************************************************************************* */
-PoseRTV PoseRTV::Expmap(const Vector& v) {
-  assert(v.size() == 9);
-  Pose3 newPose = Pose3::Expmap(sub(v, 0, 6));
-  Velocity3 newVel = Velocity3::Expmap(sub(v, 6, 9));
+PoseRTV PoseRTV::Expmap(const Vector9& v) {
+  Pose3 newPose = Pose3::Expmap(v.head<6>());
+  Velocity3 newVel = Velocity3::Expmap(v.tail<3>());
   return PoseRTV(newPose, newVel);
 }
 
 /* ************************************************************************* */
-Vector PoseRTV::Logmap(const PoseRTV& p) {
-  Vector Lx = Pose3::Logmap(p.Rt_);
-  Vector Lv = Velocity3::Logmap(p.v_);
-  return concatVectors(2, &Lx, &Lv);
+Vector9 PoseRTV::Logmap(const PoseRTV& p) {
+  Vector6 Lx = Pose3::Logmap(p.Rt_);
+  Vector3 Lv = Velocity3::Logmap(p.v_);
+  return (Vector9() << Lx, Lv);
 }
 
 /* ************************************************************************* */
@@ -84,9 +83,9 @@ PoseRTV PoseRTV::retract(const Vector& v) const {
 Vector PoseRTV::localCoordinates(const PoseRTV& p1) const {
   const Pose3& x0 = pose(), &x1 = p1.pose();
   // First order approximation
-  Vector poseLogmap = x0.localCoordinates(x1);
-  Vector lv = rotation().unrotate(p1.velocity() - v_).vector();
-  return concatVectors(2, &poseLogmap, &lv);
+  Vector6 poseLogmap = x0.localCoordinates(x1);
+  Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector();
+  return (Vector(9) << poseLogmap, lv);
 }
 
 /* ************************************************************************* */
@@ -190,16 +189,16 @@ PoseRTV PoseRTV::generalDynamics(
 }
 
 /* ************************************************************************* */
-Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
+Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
   // split out states
   const Rot3      &r1 = R(), &r2 = x2.R();
   const Velocity3 &v1 = v(), &v2 = x2.v();
 
-  Vector imu(6);
+  Vector6 imu;
 
   // acceleration
   Vector accel = v1.localCoordinates(v2) / dt;
-  imu.head(3) = r2.transpose() * (accel - g);
+  imu.head<3>() = r2.transpose() * (accel - g);
 
   // rotation rates
   // just using euler angles based on matlab code
@@ -211,7 +210,7 @@ Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
   // normalize yaw in difference (as per Mitch's code)
   dR(2) = Rot2::fromAngle(dR(2)).theta();
   dR /= dt;
-  imu.tail(3) = Enb * dR;
+  imu.tail<3>() = Enb * dR;
 //  imu.tail(3) = r1.transpose() * dR;
 
   return imu;

gtsam_unstable/dynamics/PoseRTV.h

@@ -86,8 +86,8 @@ public:
    * expmap/logmap are poor approximations that assume independence of components
    * Currently implemented using the poor retract/unretract approximations
    */
-  static PoseRTV Expmap(const Vector& v);
-  static Vector Logmap(const PoseRTV& p);
+  static PoseRTV Expmap(const Vector9& v);
+  static Vector9 Logmap(const PoseRTV& p);
 
   static PoseRTV identity() { return PoseRTV(); }
 
@@ -129,7 +129,7 @@ public:
   /// Dynamics predictor for both ground and flying robots, given states at 1 and 2
   /// Always move from time 1 to time 2
   /// @return imu measurement, as [accel, gyro]
-  Vector imuPrediction(const PoseRTV& x2, double dt) const;
+  Vector6 imuPrediction(const PoseRTV& x2, double dt) const;
 
   /// predict measurement and where Point3 for x2 should be, as a way
   /// of enforcing a velocity constraint

gtsam_unstable/dynamics/SimpleHelicopter.h

@@ -9,7 +9,6 @@
 
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace gtsam {
@@ -24,10 +23,10 @@ namespace gtsam {
  * Note: this factor is necessary if one needs to smooth the entire graph. It's not needed
  *  in sequential update method.
  */
-class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, LieVector>  {
+class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, Vector6>  {
 
   double h_;  // time step
-  typedef NoiseModelFactor3<Pose3, Pose3, LieVector> Base;
+  typedef NoiseModelFactor3<Pose3, Pose3, Vector6> Base;
 public:
   Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) :
     Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), gKey1, gKey,
@@ -41,7 +40,7 @@ public:
         gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); }
 
   /** \f$ log((g_k\exp(h\xi_k))^{-1}g_{k+1}) = 0, with optional derivatives */
-  Vector evaluateError(const Pose3& gk1, const Pose3& gk, const LieVector& xik,
+  Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none) const {
@@ -74,7 +73,7 @@ public:
 /**
  * Implement the Discrete Euler-Poincare' equation:
  */
-class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<LieVector, LieVector, Pose3>  {
+class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<Vector6, Vector6, Pose3>  {
 
   double h_;  /// time step
   Matrix Inertia_;  /// Inertia tensors Inertia = [ J 0; 0 M ]
@@ -87,7 +86,7 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<LieVector, LieV
   // This might be needed in control or system identification problems.
   // We treat them as constant here, since the control inputs are to specify.
 
-  typedef NoiseModelFactor3<LieVector, LieVector, Pose3> Base;
+  typedef NoiseModelFactor3<Vector6, Vector6, Pose3> Base;
 
 public:
   DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,
@@ -108,7 +107,7 @@ public:
    * where pk = CT_TLN(h*xi_k)*Inertia*xi_k
    *       pk_1 = CT_TLN(-h*xi_k_1)*Inertia*xi_k_1
    * */
-  Vector evaluateError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk,
+  Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none) const {
@@ -149,7 +148,7 @@ public:
   }
 
 #if 0
-  Vector computeError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk) const {
+  Vector computeError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk) const {
     Vector pk = Pose3::dExpInv_exp(h_*xik).transpose()*Inertia_*xik;
     Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1;
 
@@ -161,13 +160,13 @@ public:
     return hx;
   }
 
-  Vector evaluateError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk,
+  Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none) const {
     if (H1) {
       (*H1) = numericalDerivative31(
-          boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
+          boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
               boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
           ),
           xik, xik_1, gk, 1e-5
@@ -175,7 +174,7 @@ public:
     }
     if (H2) {
       (*H2) = numericalDerivative32(
-          boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
+          boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
               boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
           ),
           xik, xik_1, gk, 1e-5
@@ -183,7 +182,7 @@ public:
     }
     if (H3) {
       (*H3) = numericalDerivative33(
-          boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>(
+          boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
               boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
           ),
           xik, xik_1, gk, 1e-5

gtsam_unstable/dynamics/VelocityConstraint3.h

@@ -1,21 +1,20 @@
 /**
  * @file VelocityConstraint3.h
- * @brief A simple 3-way factor constraining LieScalar poses and velocity
+ * @brief A simple 3-way factor constraining double poses and velocity
  * @author Duy-Nguyen Ta
  */
 
 #pragma once
 
-#include <gtsam/base/LieScalar.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 namespace gtsam {
 
-class VelocityConstraint3 : public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> {
+class VelocityConstraint3 : public NoiseModelFactor3<double, double, double> {
 public:
 
 protected:
-  typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base;
+  typedef NoiseModelFactor3<double, double, double> Base;
 
   /** default constructor to allow for serialization */
   VelocityConstraint3() {}
@@ -28,7 +27,7 @@ public:
 
   ///TODO: comment
   VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0)
-  : Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), key1, key2, velKey), dt_(dt) {}
+  : Base(noiseModel::Constrained::All(1, fabs(mu)), key1, key2, velKey), dt_(dt) {}
   virtual ~VelocityConstraint3() {}
 
   /// @return a deep copy of this factor
@@ -37,15 +36,15 @@ public:
         gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); }
 
   /** x1 + v*dt - x2 = 0, with optional derivatives */
-  Vector evaluateError(const LieScalar& x1, const LieScalar& x2, const LieScalar& v,
+  Vector evaluateError(const double& x1, const double& x2, const double& v,
       boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none) const {
-    const size_t p = LieScalar::Dim();
+    const size_t p = 1;
     if (H1) *H1 = eye(p);
     if (H2) *H2 = -eye(p);
     if (H3) *H3 = eye(p)*dt_;
-    return x2.localCoordinates(x1.compose(LieScalar(v*dt_)));
+    return (Vector(1) << x1+v*dt_-x2);
   }
 
 private:

gtsam_unstable/dynamics/tests/testIMUSystem.cpp

@@ -62,10 +62,9 @@ TEST( testIMUSystem, optimize_chain ) {
 
   // create measurements
   SharedDiagonal model = noiseModel::Unit::Create(6);
-  Vector imu12(6), imu23(6), imu34(6);
-  imu12 = pose1.imuPrediction(pose2, dt);
-  imu23 = pose2.imuPrediction(pose3, dt);
-  imu34 = pose3.imuPrediction(pose4, dt);
+  Vector6 imu12 = pose1.imuPrediction(pose2, dt);
+  Vector6 imu23 = pose2.imuPrediction(pose3, dt);
+  Vector6 imu34 = pose3.imuPrediction(pose4, dt);
 
   // assemble simple graph with IMU measurements and velocity constraints
   NonlinearFactorGraph graph;
@@ -109,10 +108,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
 
   // create measurements
   SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0);
-  Vector imu12(6), imu23(6), imu34(6);
-  imu12 = pose1.imuPrediction(pose2, dt);
-  imu23 = pose2.imuPrediction(pose3, dt);
-  imu34 = pose3.imuPrediction(pose4, dt);
+  Vector6 imu12 = pose1.imuPrediction(pose2, dt);
+  Vector6 imu23 = pose2.imuPrediction(pose3, dt);
+  Vector6 imu34 = pose3.imuPrediction(pose4, dt);
 
   // assemble simple graph with IMU measurements and velocity constraints
   NonlinearFactorGraph graph;

gtsam_unstable/dynamics/tests/testPendulumFactors.cpp

@@ -18,8 +18,8 @@ namespace {
   const double g = 9.81, l = 1.0;
 
   const double deg2rad = M_PI/180.0;
-  LieScalar origin, q1(deg2rad*30.0), q2(deg2rad*31.0);
-  LieScalar v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1)));
+  double q1(deg2rad*30.0), q2(deg2rad*31.0);
+  double v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1)));
 
 }
 
@@ -46,7 +46,7 @@ TEST( testPendulumFactorPk, evaluateError) {
   // hard constraints don't need a noise model
   PendulumFactorPk constraint(P(1), Q(1), Q(2), h);
 
-  LieScalar pk( 1/h * (q2-q1) + h*g*sin(q1) );
+  double pk( 1/h * (q2-q1) + h*g*sin(q1) );
 
   // verify error function
   EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol));
@@ -57,7 +57,7 @@ TEST( testPendulumFactorPk1, evaluateError) {
   // hard constraints don't need a noise model
   PendulumFactorPk1 constraint(P(2), Q(1), Q(2), h);
 
-  LieScalar pk1( 1/h * (q2-q1) );
+  double pk1( 1/h * (q2-q1) );
 
   // verify error function
   EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol));

gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp

@@ -8,23 +8,16 @@
 #include <gtsam/inference/Symbol.h>
 #include <gtsam_unstable/dynamics/VelocityConstraint3.h>
 
-/* ************************************************************************* */
 using namespace gtsam;
+using namespace gtsam::symbol_shorthand;
 
-namespace {
+/* ************************************************************************* */
+// evaluateError
+TEST( testVelocityConstraint3, evaluateError) {
 
   const double tol = 1e-5;
   const double dt = 1.0;
-
-  LieScalar origin,
-    x1(1.0), x2(2.0), v(1.0);
-
-}
-
-/* ************************************************************************* */
-TEST( testVelocityConstraint3, evaluateError) {
-
-  using namespace gtsam::symbol_shorthand;
+  double x1(1.0), x2(2.0), v(1.0);
 
   // hard constraints don't need a noise model
   VelocityConstraint3 constraint(X(1), X(2), V(1), dt);
@@ -33,7 +26,9 @@ TEST( testVelocityConstraint3, evaluateError) {
   EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol));
 }
 
-
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam_unstable/geometry/InvDepthCamera3.h

@@ -15,8 +15,6 @@
 #include <boost/optional.hpp>
 #include <boost/serialization/nvp.hpp>
 #include <gtsam/base/Vector.h>
-#include <gtsam/base/LieVector.h>
-#include <gtsam/base/LieScalar.h>
 #include <gtsam/base/Matrix.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Pose3.h>
@@ -71,10 +69,9 @@ public:
    * @param inv inverse depth
    * @return Point3
    */
-  static gtsam::Point3 invDepthTo3D(const gtsam::LieVector& pw, const gtsam::LieScalar& inv) {
-    gtsam::Point3 ray_base(pw.vector().segment(0,3));
+  static gtsam::Point3 invDepthTo3D(const Vector5& pw, double rho) {
+    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;
   }
@@ -84,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,
-      const gtsam::LieScalar& inv_depth,
+  inline gtsam::Point2 project(const Vector5& pw,
+      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;
 
@@ -157,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();
@@ -166,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)),
-        gtsam::LieScalar(1./depth));
+    return std::make_pair(Vector5((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)),
+        double(1./depth));
   }
 
 private:

gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp

@@ -30,9 +30,9 @@ TEST( InvDepthFactor, Project1) {
 
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
   Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.));
-  LieScalar inv_depth(1./4);
+  double inv_depth(1./4);
   Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
-  EXPECT(assert_equal(expected_uv, actual_uv));
+  EXPECT(assert_equal(expected_uv, actual_uv,1e-8));
   EXPECT(assert_equal(Point2(640,480), actual_uv));
 }
 
@@ -46,7 +46,7 @@ TEST( InvDepthFactor, Project2) {
 
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
   Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0))));
-  LieScalar inv_depth(1/sqrt(3.0));
+  double inv_depth(1/sqrt(3.0));
   Point2 actual = inv_camera.project(diag_landmark, inv_depth);
   EXPECT(assert_equal(expected, actual));
 }
@@ -61,7 +61,7 @@ TEST( InvDepthFactor, Project3) {
 
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
   Vector5 diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0))));
-  LieScalar inv_depth( 1./sqrt(1.0+1+4));
+  double inv_depth( 1./sqrt(1.0+1+4));
   Point2 actual = inv_camera.project(diag_landmark, inv_depth);
   EXPECT(assert_equal(expected, actual));
 }
@@ -76,24 +76,24 @@ TEST( InvDepthFactor, Project4) {
 
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
   Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.))));
-  LieScalar inv_depth(1./sqrt(1.+16.+4.));
+  double inv_depth(1./sqrt(1.+16.+4.));
   Point2 actual = inv_camera.project(diag_landmark, inv_depth);
   EXPECT(assert_equal(expected, actual));
 }
 
 
 /* ************************************************************************* */
-Point2 project_(const Pose3& pose, const Vector5& landmark, const LieScalar& inv_depth) {
+Point2 project_(const Pose3& pose, const Vector5& landmark, const double& inv_depth) {
   return InvDepthCamera3<Cal3_S2>(pose,K).project(landmark, inv_depth); }
 
 TEST( InvDepthFactor, Dproject_pose)
 {
   Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
-  LieScalar inv_depth(1./4);
+  double inv_depth(1./4);
   Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth);
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
   Matrix actual;
-  Point2 uv = inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none);
+  inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none);
   EXPECT(assert_equal(expected,actual,1e-6));
 }
 
@@ -101,11 +101,11 @@ TEST( InvDepthFactor, Dproject_pose)
 TEST( InvDepthFactor, Dproject_landmark)
 {
   Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
-  LieScalar inv_depth(1./4);
+  double inv_depth(1./4);
   Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth);
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
   Matrix actual;
-  Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none);
+  inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none);
   EXPECT(assert_equal(expected,actual,1e-7));
 }
 
@@ -113,11 +113,11 @@ TEST( InvDepthFactor, Dproject_landmark)
 TEST( InvDepthFactor, Dproject_inv_depth)
 {
   Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
-  LieScalar inv_depth(1./4);
+  double inv_depth(1./4);
   Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth);
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
   Matrix actual;
-  Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual);
+  inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual);
   EXPECT(assert_equal(expected,actual,1e-7));
 }
 
@@ -125,15 +125,15 @@ TEST( InvDepthFactor, Dproject_inv_depth)
 TEST(InvDepthFactor, backproject)
 {
   Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2));
-  LieScalar inv_depth(1./4);
+  double inv_depth(1./4);
   InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
   Point2 z = inv_camera.project(expected, inv_depth);
 
   Vector5 actual_vec;
-  LieScalar actual_inv;
+  double actual_inv;
   boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4);
   EXPECT(assert_equal(expected,actual_vec,1e-7));
-  EXPECT(assert_equal(inv_depth,actual_inv,1e-7));
+  EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
 }
 
 /* ************************************************************************* */
@@ -141,15 +141,15 @@ TEST(InvDepthFactor, backproject2)
 {
   // backwards facing camera
   Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1));
-  LieScalar inv_depth(1./10);
+  double inv_depth(1./10);
   InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
   Point2 z = inv_camera.project(expected, inv_depth);
 
   Vector5 actual_vec;
-  LieScalar actual_inv;
+  double actual_inv;
   boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10);
   EXPECT(assert_equal(expected,actual_vec,1e-7));
-  EXPECT(assert_equal(inv_depth,actual_inv,1e-7));
+  EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
 }
 
 /* ************************************************************************* */

gtsam_unstable/gtsam_unstable.h

@@ -515,7 +515,6 @@ virtual class PendulumFactorPk1 : gtsam::NonlinearFactor {
   Vector evaluateError(const gtsam::LieScalar& pk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const;
 };
 
-#include <gtsam/base/LieVector.h>
 #include <gtsam_unstable/dynamics/SimpleHelicopter.h>
 
 virtual class Reconstruction : gtsam::NonlinearFactor {

gtsam_unstable/slam/AHRS.cpp

@@ -48,12 +48,12 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
 
   F_g_ = -I3 / tau_g;
   F_a_ = -I3 / tau_a;
-  Vector var_omega_w = 0 * ones(3); // TODO
-  Vector var_omega_g = (0.0034 * 0.0034) * ones(3);
-  Vector var_omega_a = (0.034 * 0.034) * ones(3);
-  Vector sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g);
-  Vector vars = concatVectors(4, &var_omega_w, &var_omega_g, &sigmas_v_g_sq,
-      &var_omega_a);
+  Vector3 var_omega_w = 0 * ones(3); // TODO
+  Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3);
+  Vector3 var_omega_a = (0.034 * 0.034) * ones(3);
+  Vector3 sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g);
+  Vector vars(12);
+  vars << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a;
   var_w_ = diag(vars);
 
   // Quantities needed for aiding

gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h

@@ -22,7 +22,6 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/geometry/Rot3.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/base/Matrix.h>
 
 // Using numerical derivative to calculate d(Pose3::Expmap)/dw
@@ -269,7 +268,7 @@ public:
     VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12;
 
     // Predict
-    return Vel1.compose( VelDelta );
+    return Vel1 + VelDelta;
 
   }
 
@@ -295,7 +294,7 @@ public:
     VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1);
 
     // Calculate error
-    return Vel2.between(Vel2Pred);
+    return Vel2Pred-Vel2;
   }
 
   Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
@@ -344,7 +343,7 @@ public:
     }
 
     Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2)));
-    Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2)));
+    Vector ErrVelVector(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2));
 
     return concatVectors(2, &ErrPoseVector, &ErrVelVector);
   }
@@ -438,18 +437,18 @@ public:
     Matrix Z_3x3 = zeros(3,3);
     Matrix I_3x3 = eye(3,3);
 
-    Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
-    Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
+    Matrix H_pos_pos = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
+    Matrix H_pos_vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
     Matrix H_pos_angles = Z_3x3;
     Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3);
 
-    Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0);
-    Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
-    Matrix H_vel_bias = numericalDerivative11<LieVector, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
+    Matrix H_vel_vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0);
+    Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
+    Matrix H_vel_bias = numericalDerivative11<Vector3, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
     Matrix H_vel_pos = Z_3x3;
 
-    Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
-    Matrix H_angles_bias = numericalDerivative11<LieVector, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
+    Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
+    Matrix H_angles_bias = numericalDerivative11<Vector3, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
     Matrix H_angles_pos = Z_3x3;
     Matrix H_angles_vel = Z_3x3;
 

gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h

@@ -22,7 +22,6 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/geometry/Rot3.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/base/Matrix.h>
 
 // Using numerical derivative to calculate d(Pose3::Expmap)/dw

gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h

@@ -21,7 +21,6 @@
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/geometry/Rot3.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/base/Matrix.h>
 
 // Using numerical derivative to calculate d(Pose3::Expmap)/dw
@@ -200,7 +199,7 @@ public:
     VELOCITY VelDelta(world_a_body*dt_);
 
     // Predict
-    return Vel1.compose(VelDelta);
+    return Vel1 + VelDelta;
   }
 
   void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const {
@@ -221,7 +220,7 @@ public:
     VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1);
 
     // Calculate error
-    return Vel2.between(Vel2Pred);
+    return Vel2Pred - Vel2;
   }
 
   /** implement functions needed to derive from Factor */
@@ -271,7 +270,7 @@ public:
     }
 
     Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2)));
-    Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2)));
+    Vector ErrVelVector(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2));
 
     return concatVectors(2, &ErrPoseVector, &ErrVelVector);
   }

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

@@ -15,7 +15,6 @@
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Point2.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/base/numericalDerivative.h>
 
 namespace gtsam {
@@ -23,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
@@ -33,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;
@@ -79,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);
@@ -100,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

@@ -16,7 +16,6 @@
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Point2.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/base/numericalDerivative.h>
 
 namespace gtsam {
@@ -24,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
@@ -35,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;
@@ -83,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);
@@ -103,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

@@ -15,7 +15,6 @@
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Point2.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/base/numericalDerivative.h>
 
 namespace gtsam {
@@ -23,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
@@ -33,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;
@@ -81,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);
@@ -103,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 {
 
@@ -142,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
@@ -152,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;
@@ -200,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);
@@ -222,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/testBetweenFactorEM.cpp

@@ -10,7 +10,6 @@
 #include <gtsam_unstable/slam/BetweenFactorEM.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/Values.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/base/numericalDerivative.h>
 
 #include <gtsam/slam/BetweenFactor.h>

gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp

@@ -21,7 +21,6 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/LieVector.h>
 #include <CppUnitLite/TestHarness.h>
 #include <iostream>
 
@@ -55,7 +54,7 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor)
   SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9));
 
   // Constructor
-  EquivInertialNavFactor_GlobalVel<Pose3, LieVector, imuBias::ConstantBias> factor(
+  EquivInertialNavFactor_GlobalVel<Pose3, Vector3, imuBias::ConstantBias> factor(
       poseKey1, velKey1, biasKey1, poseKey2, velKey2,
           delta_pos_in_t0, delta_vel_in_t0, delta_angles, delta_t,
           g, rho, omega_earth, imu_model, Jacobian_wrt_t0_Overall, bias1);

gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp

@@ -23,86 +23,76 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/inference/Key.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/base/TestableAssertions.h>
 
 using namespace std;
 using namespace gtsam;
 
-Rot3 world_R_ECEF(
-    0.31686,      0.51505,      0.79645,
-    0.85173,     -0.52399,            0,
-    0.41733,      0.67835,     -0.60471);
+Rot3 world_R_ECEF(0.31686, 0.51505, 0.79645, 0.85173, -0.52399, 0, 0.41733,
+    0.67835, -0.60471);
 
-Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
+static const Vector3 world_g(0.0, 0.0, 9.81);
+static const Vector3 world_rho(0.0, -1.5724e-05, 0.0); // NED system
+static const Vector3 ECEF_omega_earth(0.0, 0.0, 7.292115e-5);
+static const Vector3 world_omega_earth = world_R_ECEF.matrix()
+    * ECEF_omega_earth;
 
 /* ************************************************************************* */
-Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1,
-    const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2,
-    const InertialNavFactor_GlobalVelocity<Pose3, LieVector,
-        imuBias::ConstantBias>& factor) {
+Pose3 predictionErrorPose(const Pose3& p1, const Vector3& v1,
+    const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2,
+    const InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias>& factor) {
   return Pose3::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).head(6));
 }
 
-Vector predictionErrorVel(const Pose3& p1, const LieVector& v1,
-    const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2,
-    const InertialNavFactor_GlobalVelocity<Pose3, LieVector,
-        imuBias::ConstantBias>& factor) {
+Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
+    const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2,
+    const InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias>& factor) {
   return factor.evaluateError(p1, v1, b1, p2, v2).tail(3);
 }
 
-/* ************************************************************************* */
-TEST( InertialNavFactor_GlobalVelocity, Constructor)
-{
+/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, Constructor) {
   Key Pose1(11);
   Key Pose2(12);
   Key Vel1(21);
   Key Vel2(22);
   Key Bias1(31);
 
-  Vector measurement_acc((Vector(3) << 0.1,0.2,0.4));
-  Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
+  Vector3 measurement_acc(0.1, 0.2, 0.4);
+  Vector3 measurement_gyro(-0.2, 0.5, 0.03);
 
   double measurement_dt(0.1);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
+      Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
+      measurement_dt, world_g, world_rho, world_omega_earth, model);
 }
 
-/* ************************************************************************* */
-TEST( InertialNavFactor_GlobalVelocity, Equals)
-{
+/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, Equals) {
   Key Pose1(11);
   Key Pose2(12);
   Key Vel1(21);
   Key Vel2(22);
   Key Bias1(31);
 
-  Vector measurement_acc((Vector(3) << 0.1,0.2,0.4));
-  Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
+  Vector3 measurement_acc(0.1, 0.2, 0.4);
+  Vector3 measurement_gyro(-0.2, 0.5, 0.03);
 
   double measurement_dt(0.1);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
+      Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
+      measurement_dt, world_g, world_rho, world_omega_earth, model);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> g(
+      Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
+      measurement_dt, world_g, world_rho, world_omega_earth, model);
   CHECK(assert_equal(f, g, 1e-5));
 }
 
-/* ************************************************************************* */
-TEST( InertialNavFactor_GlobalVelocity, Predict)
-{
+/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, Predict) {
   Key PoseKey1(11);
   Key PoseKey2(12);
   Key VelKey1(21);
@@ -110,36 +100,32 @@ TEST( InertialNavFactor_GlobalVelocity, Predict)
   Key BiasKey1(31);
 
   double measurement_dt(0.1);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
-
   // First test: zero angular motion, some acceleration
   Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.3 - 9.81));
   Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0));
 
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
+      PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
+      measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
+      model);
 
   Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
-  LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
+  Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40));
   imuBias::ConstantBias Bias1;
   Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
-  LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43));
+  Vector3 expectedVel2((Vector(3) << 0.51, -0.48, 0.43));
   Pose3 actualPose2;
-  LieVector actualVel2;
+  Vector3 actualVel2;
   f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2);
 
   CHECK(assert_equal(expectedPose2, actualPose2, 1e-5));
-  CHECK(assert_equal(expectedVel2, actualVel2, 1e-5));
+  CHECK(assert_equal((Vector)expectedVel2, actualVel2, 1e-5));
 }
 
-/* ************************************************************************* */
-TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
-{
+/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) {
   Key PoseKey1(11);
   Key PoseKey2(12);
   Key VelKey1(21);
@@ -147,24 +133,22 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
   Key BiasKey1(31);
 
   double measurement_dt(0.1);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
-
   // First test: zero angular motion, some acceleration
   Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.3 - 9.81));
   Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0));
 
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
+      PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
+      measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
+      model);
 
   Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
   Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
-  LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
-  LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43));
+  Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40));
+  Vector3 Vel2((Vector(3) << 0.51, -0.48, 0.43));
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@@ -173,9 +157,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
 }
 
-/* ************************************************************************* */
-TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
-{
+/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRot) {
   Key PoseKey1(11);
   Key PoseKey2(12);
   Key VelKey1(21);
@@ -183,10 +165,6 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
   Key BiasKey1(31);
 
   double measurement_dt(0.1);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
@@ -194,12 +172,16 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
   Vector measurement_acc((Vector(3) << 0.0, 0.0, 0.0 - 9.81));
   Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3));
 
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
+      PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
+      measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
+      model);
 
   Pose3 Pose1(Rot3(), Point3(2.0, 1.0, 3.0));
-  Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0));
-  LieVector Vel1((Vector(3) << 0.0, 0.0, 0.0));
-  LieVector Vel2((Vector(3) << 0.0, 0.0, 0.0));
+  Pose3 Pose2(Rot3::Expmap(measurement_gyro * measurement_dt),
+      Point3(2.0, 1.0, 3.0));
+  Vector3 Vel1((Vector(3) << 0.0, 0.0, 0.0));
+  Vector3 Vel2((Vector(3) << 0.0, 0.0, 0.0));
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@@ -208,9 +190,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
 }
 
-/* ************************************************************************* */
-TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
-{
+/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) {
   Key PoseKey1(11);
   Key PoseKey2(12);
   Key VelKey1(21);
@@ -218,32 +198,30 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
   Key BiasKey1(31);
 
   double measurement_dt(0.1);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
   // Second test: zero angular motion, some acceleration - generated in matlab
-  Vector measurement_acc((Vector(3) << 6.501390843381716,  -6.763926150509185,  -2.300389940090343));
+  Vector measurement_acc(
+      (Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343));
   Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3));
 
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
+      PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
+      measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
+      model);
 
-  Rot3 R1(0.487316618,   0.125253866,    0.86419557,
-       0.580273724,   0.693095498,  -0.427669306,
-      -0.652537293,   0.709880342,   0.265075427);
+  Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498,
+      -0.427669306, -0.652537293, 0.709880342, 0.265075427);
   Point3 t1(2.0, 1.0, 3.0);
   Pose3 Pose1(R1, t1);
-  LieVector Vel1((Vector(3) << 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);
+  Vector3 Vel1((Vector(3) << 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));
   Pose3 Pose2(R2, t2);
   Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
-  LieVector Vel2 = Vel1.compose( dv );
+  Vector3 Vel2 = Vel1 + dv;
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@@ -253,16 +231,15 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
 }
 
-
 ///* VADIM - START ************************************************************************* */
-//LieVector predictionRq(const LieVector angles, const LieVector q) {
+//Vector3 predictionRq(const Vector3 angles, const Vector3 q) {
 //  return (Rot3().RzRyRx(angles) * q).vector();
 //}
 //
 //TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) {
-//  LieVector angles((Vector(3) << 3.001, -1.0004, 2.0005));
+//  Vector3 angles((Vector(3) << 3.001, -1.0004, 2.0005));
 //  Rot3 R1(Rot3().RzRyRx(angles));
-//  LieVector q((Vector(3) << 5.8, -2.2, 4.105));
+//  Vector3 q((Vector(3) << 5.8, -2.2, 4.105));
 //  Rot3 qx(0.0, -q[2], q[1],
 //      q[2], 0.0, -q[0],
 //      -q[1], q[0],0.0);
@@ -270,9 +247,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
 //
 //  Matrix J_expected;
 //
-//  LieVector v(predictionRq(angles, q));
+//  Vector3 v(predictionRq(angles, q));
 //
-//  J_expected = numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionRq, _1, q), angles);
+//  J_expected = numericalDerivative11<Vector3, Vector3>(boost::bind(&predictionRq, _1, q), angles);
 //
 //  cout<<"J_hyp"<<J_hyp<<endl;
 //  cout<<"J_expected"<<J_expected<<endl;
@@ -281,8 +258,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
 //}
 ///* VADIM - END ************************************************************************* */
 
-/* ************************************************************************* */
-TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
+/* ************************************************************************* */TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
 
   Key PoseKey1(11);
   Key PoseKey2(12);
@@ -291,35 +267,36 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
   Key BiasKey1(31);
 
   double measurement_dt(0.01);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
-  Vector measurement_acc((Vector(3) << 6.501390843381716,  -6.763926150509185,  -2.300389940090343));
+  Vector measurement_acc(
+      (Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343));
   Vector measurement_gyro((Vector(3) << 3.14, 3.14 / 2, -3.14));
 
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> factor(
+      PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
+      measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
+      model);
 
-  Rot3 R1(0.487316618,   0.125253866,    0.86419557,
-       0.580273724,   0.693095498,  -0.427669306,
-      -0.652537293,   0.709880342,   0.265075427);
+  Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498,
+      -0.427669306, -0.652537293, 0.709880342, 0.265075427);
   Point3 t1(2.0, 1.0, 3.0);
   Pose3 Pose1(R1, t1);
-  LieVector Vel1((Vector(3) << 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);
+  Vector3 Vel1((Vector(3) << 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(2.052670960415706, 0.977252139079380, 2.942482135362800);
   Pose3 Pose2(R2, t2);
-  LieVector Vel2((Vector(3) << 0.510000000000000,  -0.480000000000000,   0.430000000000000));
+  Vector3 Vel2(
+      (Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000));
   imuBias::ConstantBias Bias1;
 
   Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
 
-  Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual));
+  Vector ActualErr(
+      factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual,
+          H2_actual, H3_actual, H4_actual, H5_actual));
 
   // Checking for Pose part in the jacobians
   // ******
@@ -330,12 +307,23 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
   Matrix H5_actualPose(H5_actual.block(0, 0, 6, H5_actual.cols()));
 
   // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
-  Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose;
-  H1_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
-  H2_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
-  H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
-  H4_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
-  H5_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
+  Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose,
+      H5_expectedPose;
+  H1_expectedPose = numericalDerivative11<Pose3, Pose3>(
+      boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor),
+      Pose1);
+  H2_expectedPose = numericalDerivative11<Pose3, Vector3>(
+      boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor),
+      Vel1);
+  H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(
+      boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor),
+      Bias1);
+  H4_expectedPose = numericalDerivative11<Pose3, Pose3>(
+      boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor),
+      Pose2);
+  H5_expectedPose = numericalDerivative11<Pose3, Vector3>(
+      boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor),
+      Vel2);
 
   // Verify they are equal for this choice of state
   CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
@@ -353,12 +341,23 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
   Matrix H5_actualVel(H5_actual.block(6, 0, 3, H5_actual.cols()));
 
   // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
-  Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel;
-  H1_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
-  H2_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
-  H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
-  H4_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
-  H5_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
+  Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel,
+      H5_expectedVel;
+  H1_expectedVel = numericalDerivative11<Vector, Pose3>(
+      boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor),
+      Pose1);
+  H2_expectedVel = numericalDerivative11<Vector, Vector3>(
+      boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor),
+      Vel1);
+  H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(
+      boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor),
+      Bias1);
+  H4_expectedVel = numericalDerivative11<Vector, Pose3>(
+      boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor),
+      Pose2);
+  H5_expectedVel = numericalDerivative11<Vector, Vector3>(
+      boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor),
+      Vel2);
 
   // Verify they are equal for this choice of state
   CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
@@ -368,12 +367,7 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
   CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
 }
 
-
-
-
-/* ************************************************************************* */
-TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform)
-{
+/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) {
   Key Pose1(11);
   Key Pose2(12);
   Key Vel1(21);
@@ -384,22 +378,18 @@ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform)
   Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
 
   double measurement_dt(0.1);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
   Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation
 
-
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
+      Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
+      measurement_dt, world_g, world_rho, world_omega_earth, model,
+      body_P_sensor);
 }
 
-/* ************************************************************************* */
-TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform)
-{
+/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) {
   Key Pose1(11);
   Key Pose2(12);
   Key Vel1(21);
@@ -410,24 +400,23 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform)
   Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
 
   double measurement_dt(0.1);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
   Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation
 
-
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
+      Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
+      measurement_dt, world_g, world_rho, world_omega_earth, model,
+      body_P_sensor);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> g(
+      Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
+      measurement_dt, world_g, world_rho, world_omega_earth, model,
+      body_P_sensor);
   CHECK(assert_equal(f, g, 1e-5));
 }
 
-/* ************************************************************************* */
-TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
-{
+/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) {
   Key PoseKey1(11);
   Key PoseKey2(12);
   Key VelKey1(21);
@@ -435,39 +424,38 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
   Key BiasKey1(31);
 
   double measurement_dt(0.1);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
   Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
 
-
   // First test: zero angular motion, some acceleration
   Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation
   Matrix omega__cross = skewSymmetric(measurement_gyro);
-  Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector();  // Measured in ENU orientation
+  Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3 + 9.81)
+      + omega__cross * omega__cross
+          * body_P_sensor.rotation().inverse().matrix()
+          * body_P_sensor.translation().vector(); // Measured in ENU orientation
 
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
+      PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
+      measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
+      model, body_P_sensor);
 
   Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
-  LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
+  Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40));
   imuBias::ConstantBias Bias1;
   Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
-  LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43));
+  Vector3 expectedVel2((Vector(3) << 0.51, -0.48, 0.43));
   Pose3 actualPose2;
-  LieVector actualVel2;
+  Vector3 actualVel2;
   f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2);
 
   CHECK(assert_equal(expectedPose2, actualPose2, 1e-5));
-  CHECK(assert_equal(expectedVel2, actualVel2, 1e-5));
+  CHECK(assert_equal((Vector)expectedVel2, actualVel2, 1e-5));
 }
 
-/* ************************************************************************* */
-TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
-{
+/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) {
   Key PoseKey1(11);
   Key PoseKey2(12);
   Key VelKey1(21);
@@ -475,27 +463,28 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
   Key BiasKey1(31);
 
   double measurement_dt(0.1);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
   Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
 
-
   // First test: zero angular motion, some acceleration
   Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation
   Matrix omega__cross = skewSymmetric(measurement_gyro);
-  Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector();  // Measured in ENU orientation
+  Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3 + 9.81)
+      + omega__cross * omega__cross
+          * body_P_sensor.rotation().inverse().matrix()
+          * body_P_sensor.translation().vector(); // Measured in ENU orientation
 
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
+      PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
+      measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
+      model, body_P_sensor);
 
   Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
   Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
-  LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40));
-  LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43));
+  Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40));
+  Vector3 Vel2((Vector(3) << 0.51, -0.48, 0.43));
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@@ -504,9 +493,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
 }
 
-/* ************************************************************************* */
-TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
-{
+/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) {
   Key PoseKey1(11);
   Key PoseKey2(12);
   Key VelKey1(21);
@@ -514,27 +501,31 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
   Key BiasKey1(31);
 
   double measurement_dt(0.1);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
   Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
 
-
   // Second test: zero angular motion, some acceleration
   Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation
   Matrix omega__cross = skewSymmetric(measurement_gyro);
-  Vector measurement_acc = (Vector(3) << 0.0, 0.0, 0.0+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector();  // Measured in ENU orientation
+  Vector measurement_acc = (Vector(3) << 0.0, 0.0, 0.0 + 9.81)
+      + omega__cross * omega__cross
+          * body_P_sensor.rotation().inverse().matrix()
+          * body_P_sensor.translation().vector(); // Measured in ENU orientation
 
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
+      PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
+      measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
+      model, body_P_sensor);
 
   Pose3 Pose1(Rot3(), Point3(2.0, 1.0, 3.0));
-  Pose3 Pose2(Rot3::Expmap(body_P_sensor.rotation().matrix()*measurement_gyro*measurement_dt), Point3(2.0, 1.0, 3.0));
-  LieVector Vel1((Vector(3) << 0.0,0.0,0.0));
-  LieVector Vel2((Vector(3) << 0.0,0.0,0.0));
+  Pose3 Pose2(
+      Rot3::Expmap(
+          body_P_sensor.rotation().matrix() * measurement_gyro
+              * measurement_dt), Point3(2.0, 1.0, 3.0));
+  Vector3 Vel1((Vector(3) << 0.0, 0.0, 0.0));
+  Vector3 Vel2((Vector(3) << 0.0, 0.0, 0.0));
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@@ -543,9 +534,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
 }
 
-/* ************************************************************************* */
-TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
-{
+/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) {
   Key PoseKey1(11);
   Key PoseKey2(12);
   Key VelKey1(21);
@@ -553,36 +542,40 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
   Key BiasKey1(31);
 
   double measurement_dt(0.1);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
   Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
 
-
   // Second test: zero angular motion, some acceleration - generated in matlab
   Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation
   Matrix omega__cross = skewSymmetric(measurement_gyro);
-  Vector measurement_acc = (Vector(3) << -6.763926150509185,  6.501390843381716,  +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector();  // Measured in ENU orientation
+  Vector measurement_acc =
+      (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343)
+          + omega__cross * omega__cross
+              * body_P_sensor.rotation().inverse().matrix()
+              * body_P_sensor.translation().vector(); // Measured in ENU orientation
 
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
+      PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
+      measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
+      model, body_P_sensor);
 
-  Rot3 R1(0.487316618,   0.125253866,   0.86419557,
-       0.580273724,  0.693095498, -0.427669306,
-      -0.652537293,  0.709880342,  0.265075427);
+  Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498,
+      -0.427669306, -0.652537293, 0.709880342, 0.265075427);
   Point3 t1(2.0, 1.0, 3.0);
   Pose3 Pose1(R1, t1);
-  LieVector Vel1((Vector(3) << 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);
+  Vector3 Vel1((Vector(3) << 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));
   Pose3 Pose2(R2, t2);
-  Vector dv = measurement_dt * (R1.matrix() * body_P_sensor.rotation().matrix() * (Vector(3) << -6.763926150509185,  6.501390843381716,  +2.300389940090343) + world_g);
-  LieVector Vel2 = Vel1.compose( dv );
+  Vector dv =
+      measurement_dt
+          * (R1.matrix() * body_P_sensor.rotation().matrix()
+              * (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343)
+              + world_g);
+  Vector3 Vel2 = Vel1 + dv;
   imuBias::ConstantBias Bias1;
 
   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@@ -592,8 +585,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
 }
 
-/* ************************************************************************* */
-TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
+/* ************************************************************************* */TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
 
   Key PoseKey1(11);
   Key PoseKey2(12);
@@ -602,40 +594,41 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
   Key BiasKey1(31);
 
   double measurement_dt(0.01);
-  Vector world_g((Vector(3) << 0.0, 0.0, 9.81));
-  Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
-  Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5));
-  Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
 
   SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
 
   Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
 
-
   Vector measurement_gyro((Vector(3) << 3.14 / 2, 3.14, +3.14)); // Measured in ENU orientation
   Matrix omega__cross = skewSymmetric(measurement_gyro);
-  Vector measurement_acc = (Vector(3) << -6.763926150509185,  6.501390843381716,  +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector();  // Measured in ENU orientation
+  Vector measurement_acc =
+      (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343)
+          + omega__cross * omega__cross
+              * body_P_sensor.rotation().inverse().matrix()
+              * body_P_sensor.translation().vector(); // Measured in ENU orientation
 
+  InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> factor(
+      PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
+      measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
+      model, body_P_sensor);
 
-  InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor);
-
-  Rot3 R1(0.487316618,   0.125253866,   0.86419557,
-       0.580273724,  0.693095498, -0.427669306,
-      -0.652537293,  0.709880342,  0.265075427);
+  Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498,
+      -0.427669306, -0.652537293, 0.709880342, 0.265075427);
   Point3 t1(2.0, 1.0, 3.0);
   Pose3 Pose1(R1, t1);
-  LieVector Vel1((Vector(3) << 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);
+  Vector3 Vel1(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(2.052670960415706, 0.977252139079380, 2.942482135362800);
   Pose3 Pose2(R2, t2);
-  LieVector Vel2((Vector(3) << 0.510000000000000,  -0.480000000000000,   0.430000000000000));
+  Vector3 Vel2(0.510000000000000, -0.480000000000000, 0.430000000000000);
   imuBias::ConstantBias Bias1;
 
   Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
 
-  Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual));
+  Vector ActualErr(
+      factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual,
+          H2_actual, H3_actual, H4_actual, H5_actual));
 
   // Checking for Pose part in the jacobians
   // ******
@@ -646,12 +639,23 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
   Matrix H5_actualPose(H5_actual.block(0, 0, 6, H5_actual.cols()));
 
   // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
-  Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose;
-  H1_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
-  H2_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
-  H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
-  H4_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
-  H5_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
+  Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose,
+      H5_expectedPose;
+  H1_expectedPose = numericalDerivative11<Pose3, Pose3>(
+      boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor),
+      Pose1);
+  H2_expectedPose = numericalDerivative11<Pose3, Vector3>(
+      boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor),
+      Vel1);
+  H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(
+      boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor),
+      Bias1);
+  H4_expectedPose = numericalDerivative11<Pose3, Pose3>(
+      boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor),
+      Pose2);
+  H5_expectedPose = numericalDerivative11<Pose3, Vector3>(
+      boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor),
+      Vel2);
 
   // Verify they are equal for this choice of state
   CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
@@ -669,12 +673,23 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
   Matrix H5_actualVel(H5_actual.block(6, 0, 3, H5_actual.cols()));
 
   // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
-  Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel;
-  H1_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
-  H2_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
-  H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
-  H4_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
-  H5_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
+  Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel,
+      H5_expectedVel;
+  H1_expectedVel = numericalDerivative11<Vector, Pose3>(
+      boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor),
+      Pose1);
+  H2_expectedVel = numericalDerivative11<Vector, Vector3>(
+      boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor),
+      Vel1);
+  H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(
+      boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor),
+      Bias1);
+  H4_expectedVel = numericalDerivative11<Vector, Pose3>(
+      boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor),
+      Pose2);
+  H5_expectedVel = numericalDerivative11<Vector, Vector3>(
+      boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor),
+      Vel2);
 
   // Verify they are equal for this choice of state
   CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
@@ -685,5 +700,8 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
 }
 
 /* ************************************************************************* */
-  int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

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<LieScalar>(Symbol('d',1)));
+      result2.at<Vector5>(Symbol('l',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");
-//  result.at<LieVector>(landmarkKey).print("Landmark After:\n");
+//  values.at<Vector6>(landmarkKey).print("Landmark Before:\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");
-//  result.at<LieVector>(landmarkKey).print("Landmark After:\n");
+//  values.at<Vector3>(landmarkKey).print("Landmark Before:\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));
 }
 
 

gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp

@@ -10,7 +10,6 @@
 #include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/Values.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/base/numericalDerivative.h>
 
 #include <gtsam/slam/BetweenFactor.h>
@@ -23,26 +22,21 @@
 using namespace std;
 using namespace gtsam;
 
-// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below
-// to reenable the test.
-//#if 0
 /* ************************************************************************* */
-LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor<gtsam::Pose2>& factor){
+Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor<gtsam::Pose2>& factor){
   gtsam::Values values;
   values.insert(key, org1_T_org2);
-  //  LieVector err = factor.whitenedError(values);
-  //  return err;
-  return LieVector::Expmap(factor.whitenedError(values));
+  return factor.whitenedError(values);
 }
 
 /* ************************************************************************* */
-//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor<gtsam::Pose2>& factor){
+//Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor<gtsam::Pose2>& factor){
 //  gtsam::Values values;
 //  values.insert(keyA, p1);
 //  values.insert(keyB, p2);
-//  //  LieVector err = factor.whitenedError(values);
+//  //  Vector err = factor.whitenedError(values);
 //  //  return err;
-//  return LieVector::Expmap(factor.whitenedError(values));
+//  return Vector::Expmap(factor.whitenedError(values));
 //}
 
 /* ************************************************************************* */
@@ -237,7 +231,7 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian)
   Matrix H1_actual = H_actual[0];
 
   double stepsize = 1.0e-9;
-  Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1,  key, g), orgA_T_orgB, stepsize);
+  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1,  key, g), orgA_T_orgB, stepsize);
 //  CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
 }
 
@@ -291,12 +285,12 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian)
 ////  CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
 //
 //  double stepsize = 1.0e-9;
-//  Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
-//  Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
+//  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
+//  Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
 //
 //
 //  // try to check numerical derivatives of a standard between factor
-//  Matrix H1_expected_stnd = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
+//  Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
 //  CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
 //
 //
@@ -305,8 +299,6 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian)
 //
 //}
 
-//#endif
-
 /* ************************************************************************* */
   int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp

@@ -10,7 +10,6 @@
 #include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/Values.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/base/numericalDerivative.h>
 
 #include <gtsam/slam/BetweenFactor.h>
@@ -23,26 +22,21 @@
 using namespace std;
 using namespace gtsam;
 
-// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below
-// to reenable the test.
-//#if 0
 /* ************************************************************************* */
-LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM<gtsam::Pose2>& factor){
+Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM<gtsam::Pose2>& factor){
   gtsam::Values values;
   values.insert(key, org1_T_org2);
-  //  LieVector err = factor.whitenedError(values);
-  //  return err;
-  return LieVector::Expmap(factor.whitenedError(values));
+  return factor.whitenedError(values);
 }
 
 /* ************************************************************************* */
-//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor<gtsam::Pose2>& factor){
+//Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor<gtsam::Pose2>& factor){
 //  gtsam::Values values;
 //  values.insert(keyA, p1);
 //  values.insert(keyB, p2);
-//  //  LieVector err = factor.whitenedError(values);
+//  //  Vector err = factor.whitenedError(values);
 //  //  return err;
-//  return LieVector::Expmap(factor.whitenedError(values));
+//  return Vector::Expmap(factor.whitenedError(values));
 //}
 
 /* ************************************************************************* */
@@ -266,7 +260,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
   Matrix H1_actual = H_actual[0];
 
   double stepsize = 1.0e-9;
-  Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1,  key, g), orgA_T_orgB, stepsize);
+  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1,  key, g), orgA_T_orgB, stepsize);
 //  CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
 }
 /////* ************************************************************************** */
@@ -316,12 +310,12 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
 ////  CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
 //
 //  double stepsize = 1.0e-9;
-//  Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
-//  Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
+//  Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
+//  Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
 //
 //
 //  // try to check numerical derivatives of a standard between factor
-//  Matrix H1_expected_stnd = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
+//  Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
 //  CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
 //
 //
@@ -330,8 +324,6 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
 //
 //}
 
-//#endif
-
 /* ************************************************************************* */
   int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp

@@ -21,7 +21,6 @@
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/LieVector.h>
 #include <gtsam/inference/Key.h>
 
 using namespace std;

tests/testGaussianISAM2.cpp

@@ -6,24 +6,24 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <gtsam/base/debug.h>
-#include <gtsam/base/TestableAssertions.h>
-#include <gtsam/base/LieVector.h>
-#include <gtsam/base/treeTraversal-inst.h>
+#include <tests/smallExample.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/BearingRangeFactor.h>
 #include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/inference/Ordering.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/GaussianBayesTree.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/Marginals.h>
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/slam/BearingRangeFactor.h>
-#include <tests/smallExample.h>
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/GaussianBayesTree.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/inference/Ordering.h>
+#include <gtsam/base/debug.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/treeTraversal-inst.h>
+#include <gtsam/base/LieScalar.h>
 
 #include <boost/foreach.hpp>
 #include <boost/assign/list_of.hpp>
@@ -35,6 +35,9 @@ using namespace std;
 using namespace gtsam;
 using boost::shared_ptr;
 
+static const SharedNoiseModel model;
+static const LieScalar Zero(0);
+
 //  SETDEBUG("ISAM2 update", true);
 //  SETDEBUG("ISAM2 update verbose", true);
 //  SETDEBUG("ISAM2 recalculate", true);
@@ -203,11 +206,11 @@ struct ConsistencyVisitor
     consistent(true), isam(isam) {}
   int operator()(const ISAM2::sharedClique& node, int& parentData)
   {
-    if(std::find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end())
+    if(find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end())
     {
       if(node->parent_.expired())
         consistent = false;
-      if(std::find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end())
+      if(find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end())
         consistent = false;
     }
     BOOST_FOREACH(Key j, node->conditional()->frontals())
@@ -223,7 +226,7 @@ struct ConsistencyVisitor
 bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) {
 
   TestResult& result_ = result;
-  const std::string name_ = test.getName();
+  const string name_ = test.getName();
 
   Values actual = isam.calculateEstimate();
   Values expected = fullinit.retract(fullgraph.linearize(fullinit)->optimize());
@@ -285,19 +288,19 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
 TEST(ISAM2, AddFactorsStep1)
 {
   NonlinearFactorGraph nonlinearFactors;
-  nonlinearFactors += PriorFactor<LieVector>(10, LieVector(), gtsam::SharedNoiseModel());
+  nonlinearFactors += PriorFactor<LieScalar>(10, Zero, model);
   nonlinearFactors += NonlinearFactor::shared_ptr();
-  nonlinearFactors += PriorFactor<LieVector>(11, LieVector(), gtsam::SharedNoiseModel());
+  nonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
 
   NonlinearFactorGraph newFactors;
-  newFactors += PriorFactor<LieVector>(1, LieVector(), gtsam::SharedNoiseModel());
-  newFactors += PriorFactor<LieVector>(2, LieVector(), gtsam::SharedNoiseModel());
+  newFactors += PriorFactor<LieScalar>(1, Zero, model);
+  newFactors += PriorFactor<LieScalar>(2, Zero, model);
 
   NonlinearFactorGraph expectedNonlinearFactors;
-  expectedNonlinearFactors += PriorFactor<LieVector>(10, LieVector(), gtsam::SharedNoiseModel());
-  expectedNonlinearFactors += PriorFactor<LieVector>(1, LieVector(), gtsam::SharedNoiseModel());
-  expectedNonlinearFactors += PriorFactor<LieVector>(11, LieVector(), gtsam::SharedNoiseModel());
-  expectedNonlinearFactors += PriorFactor<LieVector>(2, LieVector(), gtsam::SharedNoiseModel());
+  expectedNonlinearFactors += PriorFactor<LieScalar>(10, Zero, model);
+  expectedNonlinearFactors += PriorFactor<LieScalar>(1, Zero, model);
+  expectedNonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
+  expectedNonlinearFactors += PriorFactor<LieScalar>(2, Zero, model);
 
   const FastVector<size_t> expectedNewFactorIndices = list_of(1)(3);
 
@@ -694,18 +697,17 @@ namespace {
 TEST(ISAM2, marginalizeLeaves1)
 {
   ISAM2 isam;
-
   NonlinearFactorGraph factors;
-  factors += PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1));
+  factors += PriorFactor<LieScalar>(0, Zero, model);
 
-  factors += BetweenFactor<LieVector>(0, 1, LieVector(0.0), noiseModel::Unit::Create(1));
-  factors += BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1));
-  factors += BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1));
+  factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
+  factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
+  factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
 
   Values values;
-  values.insert(0, LieVector(0.0));
-  values.insert(1, LieVector(0.0));
-  values.insert(2, LieVector(0.0));
+  values.insert(0, Zero);
+  values.insert(1, Zero);
+  values.insert(2, Zero);
 
   FastMap<Key,int> constrainedKeys;
   constrainedKeys.insert(make_pair(0,0));
@@ -724,18 +726,18 @@ TEST(ISAM2, marginalizeLeaves2)
   ISAM2 isam;
 
   NonlinearFactorGraph factors;
-  factors += PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1));
+  factors += PriorFactor<LieScalar>(0, Zero, model);
 
-  factors += BetweenFactor<LieVector>(0, 1, LieVector(0.0), noiseModel::Unit::Create(1));
-  factors += BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1));
-  factors += BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1));
-  factors += BetweenFactor<LieVector>(2, 3, LieVector(0.0), noiseModel::Unit::Create(1));
+  factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
+  factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
+  factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
+  factors += BetweenFactor<LieScalar>(2, 3, Zero, model);
 
   Values values;
-  values.insert(0, LieVector(0.0));
-  values.insert(1, LieVector(0.0));
-  values.insert(2, LieVector(0.0));
-  values.insert(3, LieVector(0.0));
+  values.insert(0, Zero);
+  values.insert(1, Zero);
+  values.insert(2, Zero);
+  values.insert(3, Zero);
 
   FastMap<Key,int> constrainedKeys;
   constrainedKeys.insert(make_pair(0,0));
@@ -755,25 +757,25 @@ TEST(ISAM2, marginalizeLeaves3)
   ISAM2 isam;
 
   NonlinearFactorGraph factors;
-  factors += PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1));
+  factors += PriorFactor<LieScalar>(0, Zero, model);
 
-  factors += BetweenFactor<LieVector>(0, 1, LieVector(0.0), noiseModel::Unit::Create(1));
-  factors += BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1));
-  factors += BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1));
+  factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
+  factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
+  factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
 
-  factors += BetweenFactor<LieVector>(2, 3, LieVector(0.0), noiseModel::Unit::Create(1));
+  factors += BetweenFactor<LieScalar>(2, 3, Zero, model);
 
-  factors += BetweenFactor<LieVector>(3, 4, LieVector(0.0), noiseModel::Unit::Create(1));
-  factors += BetweenFactor<LieVector>(4, 5, LieVector(0.0), noiseModel::Unit::Create(1));
-  factors += BetweenFactor<LieVector>(3, 5, LieVector(0.0), noiseModel::Unit::Create(1));
+  factors += BetweenFactor<LieScalar>(3, 4, Zero, model);
+  factors += BetweenFactor<LieScalar>(4, 5, Zero, model);
+  factors += BetweenFactor<LieScalar>(3, 5, Zero, model);
 
   Values values;
-  values.insert(0, LieVector(0.0));
-  values.insert(1, LieVector(0.0));
-  values.insert(2, LieVector(0.0));
-  values.insert(3, LieVector(0.0));
-  values.insert(4, LieVector(0.0));
-  values.insert(5, LieVector(0.0));
+  values.insert(0, Zero);
+  values.insert(1, Zero);
+  values.insert(2, Zero);
+  values.insert(3, Zero);
+  values.insert(4, Zero);
+  values.insert(5, Zero);
 
   FastMap<Key,int> constrainedKeys;
   constrainedKeys.insert(make_pair(0,0));
@@ -795,14 +797,14 @@ TEST(ISAM2, marginalizeLeaves4)
   ISAM2 isam;
 
   NonlinearFactorGraph factors;
-  factors += PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1));
-  factors += BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1));
-  factors += BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1));
+  factors += PriorFactor<LieScalar>(0, Zero, model);
+  factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
+  factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
 
   Values values;
-  values.insert(0, LieVector(0.0));
-  values.insert(1, LieVector(0.0));
-  values.insert(2, LieVector(0.0));
+  values.insert(0, Zero);
+  values.insert(1, Zero);
+  values.insert(2, Zero);
 
   FastMap<Key,int> constrainedKeys;
   constrainedKeys.insert(make_pair(0,0));

tests/testNonlinearFactor.cpp

@@ -27,7 +27,6 @@
 
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Matrix.h>
-#include <gtsam/base/LieVector.h>
 #include <tests/smallExample.h>
 #include <tests/simulated2D.h>
 #include <gtsam/linear/GaussianFactor.h>
@@ -235,13 +234,13 @@ TEST( NonlinearFactor, linearize_constraint2 )
 }
 
 /* ************************************************************************* */
-class TestFactor4 : public NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> {
+class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
 public:
-  typedef NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> Base;
+  typedef NoiseModelFactor4<double, double, double, double> Base;
   TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4)) {}
 
   virtual Vector
-    evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4,
+    evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none,
         boost::optional<Matrix&> H3 = boost::none,
@@ -264,10 +263,10 @@ public:
 TEST(NonlinearFactor, NoiseModelFactor4) {
   TestFactor4 tf;
   Values tv;
-  tv.insert(X(1), LieVector((Vector(1) << 1.0)));
-  tv.insert(X(2), LieVector((Vector(1) << 2.0)));
-  tv.insert(X(3), LieVector((Vector(1) << 3.0)));
-  tv.insert(X(4), LieVector((Vector(1) << 4.0)));
+  tv.insert(X(1), double((1.0)));
+  tv.insert(X(2), double((2.0)));
+  tv.insert(X(3), double((3.0)));
+  tv.insert(X(4), double((4.0)));
   EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
   JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
@@ -283,9 +282,9 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
 }
 
 /* ************************************************************************* */
-class TestFactor5 : public NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> {
+class TestFactor5 : public NoiseModelFactor5<double, double, double, double, double> {
 public:
-  typedef NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base;
+  typedef NoiseModelFactor5<double, double, double, double, double> Base;
   TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5)) {}
 
   virtual Vector
@@ -310,11 +309,11 @@ public:
 TEST(NonlinearFactor, NoiseModelFactor5) {
   TestFactor5 tf;
   Values tv;
-  tv.insert(X(1), LieVector((Vector(1) << 1.0)));
-  tv.insert(X(2), LieVector((Vector(1) << 2.0)));
-  tv.insert(X(3), LieVector((Vector(1) << 3.0)));
-  tv.insert(X(4), LieVector((Vector(1) << 4.0)));
-  tv.insert(X(5), LieVector((Vector(1) << 5.0)));
+  tv.insert(X(1), double((1.0)));
+  tv.insert(X(2), double((2.0)));
+  tv.insert(X(3), double((3.0)));
+  tv.insert(X(4), double((4.0)));
+  tv.insert(X(5), double((5.0)));
   EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
   JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
@@ -332,9 +331,9 @@ TEST(NonlinearFactor, NoiseModelFactor5) {
 }
 
 /* ************************************************************************* */
-class TestFactor6 : public NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> {
+class TestFactor6 : public NoiseModelFactor6<double, double, double, double, double, double> {
 public:
-  typedef NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base;
+  typedef NoiseModelFactor6<double, double, double, double, double, double> Base;
   TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {}
 
   virtual Vector
@@ -362,12 +361,12 @@ public:
 TEST(NonlinearFactor, NoiseModelFactor6) {
   TestFactor6 tf;
   Values tv;
-  tv.insert(X(1), LieVector((Vector(1) << 1.0)));
-  tv.insert(X(2), LieVector((Vector(1) << 2.0)));
-  tv.insert(X(3), LieVector((Vector(1) << 3.0)));
-  tv.insert(X(4), LieVector((Vector(1) << 4.0)));
-  tv.insert(X(5), LieVector((Vector(1) << 5.0)));
-  tv.insert(X(6), LieVector((Vector(1) << 6.0)));
+  tv.insert(X(1), double((1.0)));
+  tv.insert(X(2), double((2.0)));
+  tv.insert(X(3), double((3.0)));
+  tv.insert(X(4), double((4.0)));
+  tv.insert(X(5), double((5.0)));
+  tv.insert(X(6), double((6.0)));
   EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv)));
   DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
   JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));