From b5327673fbf00d5d3a0115012c70ec0557c76f1a Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 3 Nov 2014 11:02:15 +0100 Subject: [PATCH] Get rid of LieVector --- gtsam/navigation/CombinedImuFactor.h | 19 +++--- .../tests/testCombinedImuFactor.cpp | 1 - gtsam/nonlinear/tests/testValues.cpp | 4 +- .../slam/tests/testEssentialMatrixFactor.cpp | 66 +++++++++---------- gtsam_unstable/dynamics/FullIMUFactor.h | 9 ++- gtsam_unstable/dynamics/IMUFactor.h | 1 - gtsam_unstable/geometry/InvDepthCamera3.h | 2 - gtsam_unstable/gtsam_unstable.h | 1 - .../slam/EquivInertialNavFactor_GlobalVel.h | 15 ++--- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 1 - .../slam/InertialNavFactor_GlobalVelocity.h | 1 - gtsam_unstable/slam/InvDepthFactorVariant1.h | 1 - gtsam_unstable/slam/InvDepthFactorVariant2.h | 1 - gtsam_unstable/slam/InvDepthFactorVariant3.h | 1 - .../slam/tests/testBetweenFactorEM.cpp | 1 - .../testEquivInertialNavFactor_GlobalVel.cpp | 1 - .../testInertialNavFactor_GlobalVelocity.cpp | 1 - .../testTransformBtwRobotsUnaryFactor.cpp | 1 - .../testTransformBtwRobotsUnaryFactorEM.cpp | 1 - .../timeInertialNavFactor_GlobalVelocity.cpp | 1 - tests/testNonlinearFactor.cpp | 1 - 21 files changed, 55 insertions(+), 75 deletions(-) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 725274acc..e82bea423 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -17,11 +17,10 @@ #pragma once /* GTSAM includes */ -#include -#include #include #include -#include +#include +#include #include /* 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 { + class CombinedImuFactor: public NoiseModelFactor6 { 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(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); + // Matrix H_vel_angles = numericalDerivative11(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(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); + // Matrix H_angles_angles = numericalDerivative11(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 Base; + typedef NoiseModelFactor6 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 H1 = boost::none, boost::optional 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 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 diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 279c20e69..ed3b95150 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index b374a67f5..60639e8b7 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/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(key1))); + CHECK(assert_equal((Vector)v1, cfg.at(key1))); cfg.update(key1, v2); CHECK(cfg.size() == 1); - CHECK(assert_equal(v2, cfg.at(key1))); + CHECK(assert_equal((Vector)v2, cfg.at(key1))); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index c889fb1e9..b01db3ffe 100644 --- a/gtsam/slam/tests/testEssentialMatrixFactor.cpp +++ b/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 camera2(data.cameras[1].pose(),Cal3_S2()); +PinholeCamera 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( + Hexpected = numericalDerivative11( 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 f = - boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, - boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + boost::function f = boost::bind( + &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, + boost::none); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(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(100); EXPECT(assert_equal(trueE, actual, 1e-1)); for (size_t i = 0; i < 5; i++) - EXPECT(assert_equal(truth.at(i), result.at(i), 1e-1)); + EXPECT(assert_equal(truth.at(i), result.at(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 f = - boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, - boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + boost::function f = boost::bind( + &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, + boost::none); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(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(100); EXPECT(assert_equal(bodyE, actual, 1e-1)); for (size_t i = 0; i < 5; i++) - EXPECT(assert_equal(truth.at(i), result.at(i), 1e-1)); + EXPECT(assert_equal(truth.at(i), result.at(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 // K = boost::make_shared(500, 0, 0); -PinholeCamera camera2(data.cameras[1].pose(),*K); +PinholeCamera 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 f = - boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, - boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, trueE, d); - Hexpected2 = numericalDerivative22(f, trueE, d); + boost::function f = boost::bind( + &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none, + boost::none); + Hexpected1 = numericalDerivative21(f, trueE, d); + Hexpected2 = numericalDerivative22(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(100); EXPECT(assert_equal(trueE, actual, 1e-1)); for (size_t i = 0; i < data.number_tracks(); i++) - EXPECT(assert_equal(truth.at(i), result.at(i), 1e-1)); + EXPECT(assert_equal(truth.at(i), result.at(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 f = - boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, - boost::none, boost::none); - Hexpected1 = numericalDerivative21(f, bodyE, d); - Hexpected2 = numericalDerivative22(f, bodyE, d); + boost::function f = boost::bind( + &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none, + boost::none); + Hexpected1 = numericalDerivative21(f, bodyE, d); + Hexpected2 = numericalDerivative22(f, bodyE, d); // Verify the Jacobian is correct EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index b8dba6b61..55dd653b0 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -7,7 +7,6 @@ #pragma once #include -#include #include #include @@ -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( + if (H1) *H1 = numericalDerivative21( boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); - if (H2) *H2 = numericalDerivative22( + if (H2) *H2 = numericalDerivative22( 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); } }; diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 4ad0635cf..165f4fe19 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -7,7 +7,6 @@ #pragma once #include -#include #include #include diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 30f17fb7a..069f78b12 100644 --- a/gtsam_unstable/geometry/InvDepthCamera3.h +++ b/gtsam_unstable/geometry/InvDepthCamera3.h @@ -15,8 +15,6 @@ #include #include #include -#include -#include #include #include #include diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index f2223c2f4..27460bd72 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/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 #include virtual class Reconstruction : gtsam::NonlinearFactor { diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 41f216b77..bc8c1f5c8 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -22,7 +22,6 @@ #include #include #include -#include #include // 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(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); - Matrix H_pos_vel = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); + Matrix H_pos_pos = numericalDerivative11(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); + Matrix H_pos_vel = numericalDerivative11(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(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(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(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(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(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(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(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(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(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(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; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index dec1b2378..a38525bd9 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -22,7 +22,6 @@ #include #include #include -#include #include // Using numerical derivative to calculate d(Pose3::Expmap)/dw diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 76f6d02d5..cd203c96b 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -21,7 +21,6 @@ #include #include #include -#include #include // Using numerical derivative to calculate d(Pose3::Expmap)/dw diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index d2a784b0f..38bc24aa8 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -15,7 +15,6 @@ #include #include #include -#include #include namespace gtsam { diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 3c95a5010..a8b224b06 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -16,7 +16,6 @@ #include #include #include -#include #include namespace gtsam { diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 2f94227dc..2252ccbfd 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -15,7 +15,6 @@ #include #include #include -#include #include namespace gtsam { diff --git a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp index 9fb7942e7..0ea743d69 100644 --- a/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testBetweenFactorEM.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include #include diff --git a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 2a48aa73c..7667dc7c3 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 57a19ee78..a91a5b05b 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include using namespace std; diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index a063244a3..b38cf8518 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include #include diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index 1ffb2bebe..68671d0bd 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -10,7 +10,6 @@ #include #include #include -#include #include #include diff --git a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp index 0d514abcd..d5a851f1d 100644 --- a/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include using namespace std; diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 739fe52fb..832362dfd 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -27,7 +27,6 @@ #include #include -#include #include #include #include