Get rid of LieVector

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/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>

gtsam/nonlinear/tests/testValues.cpp

@@ -155,11 +155,11 @@ TEST( Values, update_element )
 
   cfg.insert(key1, v1);
   CHECK(cfg.size() == 1);
-  CHECK(assert_equal(v1, cfg.at<Vector3>(key1)));
+  CHECK(assert_equal((Vector)v1, cfg.at<Vector3>(key1)));
 
   cfg.update(key1, v2);
   CHECK(cfg.size() == 1);
-  CHECK(assert_equal(v2, cfg.at<Vector3>(key1)));
+  CHECK(assert_equal((Vector)v2, cfg.at<Vector3>(key1)));
 }
 
 /* ************************************************************************* */

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -36,7 +36,7 @@ SfM_data data;
 bool readOK = readBAL(filename, data);
 Rot3 c1Rc2 = data.cameras[1].pose().rotation();
 Point3 c1Tc2 = data.cameras[1].pose().translation();
-PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(),Cal3_S2());
+PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(), Cal3_S2());
 EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2));
 double baseline = 0.1; // actual baseline of the camera
 
@@ -96,7 +96,7 @@ TEST (EssentialMatrixFactor, factor) {
 
     // Use numerical derivatives to calculate the expected Jacobian
     Matrix Hexpected;
-    Hexpected = numericalDerivative11<Vector,EssentialMatrix>(
+    Hexpected = numericalDerivative11<Vector, EssentialMatrix>(
         boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
             boost::none), trueE);
 
@@ -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(assert_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(assert_equal(truth.at<double>(i), result.at<double>(i), 1e-1));
 
   // Check error at result
   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
@@ -314,7 +314,7 @@ Point2 pB(size_t i) {
 
 boost::shared_ptr<Cal3Bundler> //
 K = boost::make_shared<Cal3Bundler>(500, 0, 0);
-PinholeCamera<Cal3Bundler> camera2(data.cameras[1].pose(),*K);
+PinholeCamera<Cal3Bundler> camera2(data.cameras[1].pose(), *K);
 
 Vector vA(size_t i) {
   Point2 xy = K->calibrate(pA(i));
@@ -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(assert_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_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>
 
@@ -89,9 +88,9 @@ public:
     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<Vector3, 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<Vector3, 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) {
+  static Vector3 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
     Vector hx(9);
     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 Vector3(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>
 

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>

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/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
@@ -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

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 {

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 {

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 {

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>
 

gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp

@@ -23,7 +23,6 @@
 #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;

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>

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>

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/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>