From 492c607f9ecfa2a1cf60e1e6898c0997dc3d9611 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 3 Nov 2014 09:27:40 +0100 Subject: [PATCH] No more Lie types --- gtsam/geometry/tests/testPoint3.cpp | 7 +- gtsam/navigation/ImuFactor.h | 15 ++- gtsam/nonlinear/WhiteNoiseFactor.h | 7 +- gtsam/nonlinear/tests/testValues.cpp | 101 ++++++++++----------- gtsam/slam/EssentialMatrixFactor.h | 13 ++- gtsam_unstable/dynamics/SimpleHelicopter.h | 23 +++-- tests/testGaussianISAM2.cpp | 89 +++++++++--------- 7 files changed, 124 insertions(+), 131 deletions(-) diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a791fd8db..4a07fe815 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include @@ -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(norm_proxy, point); + Matrix expectedH = numericalDerivative11(norm_proxy, point); EXPECT(assert_equal(expectedH, actualH, 1e-8)); } diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index d68af4f8b..ea8a2cb8c 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -21,7 +21,6 @@ #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 ImuFactor: public NoiseModelFactor5 { + class ImuFactor: public NoiseModelFactor5 { 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(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); 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(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(9,9); @@ -285,7 +284,7 @@ namespace gtsam { private: typedef ImuFactor This; - typedef NoiseModelFactor5 Base; + typedef NoiseModelFactor5 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 H1 = boost::none, boost::optional 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 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 diff --git a/gtsam/nonlinear/WhiteNoiseFactor.h b/gtsam/nonlinear/WhiteNoiseFactor.h index 852ad147c..a7987d73b 100644 --- a/gtsam/nonlinear/WhiteNoiseFactor.h +++ b/gtsam/nonlinear/WhiteNoiseFactor.h @@ -19,7 +19,6 @@ #include #include -#include #include 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(meanKey_), x.at(precisionKey_)); + return f(z_, x.at(meanKey_), x.at(precisionKey_)); } /** @@ -155,8 +154,8 @@ namespace gtsam { /// linearize returns a Hessianfactor that is an approximation of error(p) virtual boost::shared_ptr linearize(const Values& x) const { - double u = x.at(meanKey_); - double p = x.at(precisionKey_); + double u = x.at(meanKey_); + double p = x.at(precisionKey_); Key j1 = meanKey_; Key j2 = precisionKey_; return linearize(z_, u, p, j1, j2); diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 9b9e8d0e0..b374a67f5 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include // for operator += @@ -74,7 +73,7 @@ struct dimension : public boost::integral_constant {}; 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(key1))); + CHECK(assert_equal(v1, cfg.at(key1))); cfg.update(key1, v2); CHECK(cfg.size() == 1); - CHECK(assert_equal(v2, cfg.at(key1))); + CHECK(assert_equal(v2, cfg.at(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 (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 (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 (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 v = config0.exists(key1); - CHECK(assert_equal((Vector(1) << 1.),*v)); + boost::optional v = config0.exists(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)); } diff --git a/gtsam/slam/EssentialMatrixFactor.h b/gtsam/slam/EssentialMatrixFactor.h index 557565a6e..87d847af2 100644 --- a/gtsam/slam/EssentialMatrixFactor.h +++ b/gtsam/slam/EssentialMatrixFactor.h @@ -10,7 +10,6 @@ #include #include #include -#include #include 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 { +class EssentialMatrixFactor2: public NoiseModelFactor2 { 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 Base; + typedef NoiseModelFactor2 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 DE = boost::none, boost::optional Dd = boost::none) const { @@ -237,7 +235,8 @@ public: */ template EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, - const Rot3& cRb, const SharedNoiseModel& model, boost::shared_ptr K) : + const Rot3& cRb, const SharedNoiseModel& model, + boost::shared_ptr 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 DE = boost::none, boost::optional Dd = boost::none) const { if (!DE) { diff --git a/gtsam_unstable/dynamics/SimpleHelicopter.h b/gtsam_unstable/dynamics/SimpleHelicopter.h index 52dcea2db..06d485a88 100644 --- a/gtsam_unstable/dynamics/SimpleHelicopter.h +++ b/gtsam_unstable/dynamics/SimpleHelicopter.h @@ -9,7 +9,6 @@ #include #include -#include #include 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 { +class Reconstruction : public NoiseModelFactor3 { double h_; // time step - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 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 H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { @@ -74,7 +73,7 @@ public: /** * Implement the Discrete Euler-Poincare' equation: */ -class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3 { +class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3 { double h_; /// time step Matrix Inertia_; /// Inertia tensors Inertia = [ J 0; 0 M ] @@ -87,7 +86,7 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3 Base; + typedef NoiseModelFactor3 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 H1 = boost::none, boost::optional H2 = boost::none, boost::optional 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 H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { if (H1) { (*H1) = numericalDerivative31( - boost::function( + boost::function( 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( + boost::function( 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( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) ), xik, xik_1, gk, 1e-5 diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 55329d8e9..275d943e8 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -8,7 +8,6 @@ #include #include -#include #include #include #include @@ -285,19 +284,19 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c TEST(ISAM2, AddFactorsStep1) { NonlinearFactorGraph nonlinearFactors; - nonlinearFactors += PriorFactor(10, LieVector(), gtsam::SharedNoiseModel()); + nonlinearFactors += PriorFactor(10, 0.0, gtsam::SharedNoiseModel()); nonlinearFactors += NonlinearFactor::shared_ptr(); - nonlinearFactors += PriorFactor(11, LieVector(), gtsam::SharedNoiseModel()); + nonlinearFactors += PriorFactor(11, 0.0, gtsam::SharedNoiseModel()); NonlinearFactorGraph newFactors; - newFactors += PriorFactor(1, LieVector(), gtsam::SharedNoiseModel()); - newFactors += PriorFactor(2, LieVector(), gtsam::SharedNoiseModel()); + newFactors += PriorFactor(1, 0.0, gtsam::SharedNoiseModel()); + newFactors += PriorFactor(2, 0.0, gtsam::SharedNoiseModel()); NonlinearFactorGraph expectedNonlinearFactors; - expectedNonlinearFactors += PriorFactor(10, LieVector(), gtsam::SharedNoiseModel()); - expectedNonlinearFactors += PriorFactor(1, LieVector(), gtsam::SharedNoiseModel()); - expectedNonlinearFactors += PriorFactor(11, LieVector(), gtsam::SharedNoiseModel()); - expectedNonlinearFactors += PriorFactor(2, LieVector(), gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(10, 0.0, gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(1, 0.0, gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(11, 0.0, gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(2, 0.0, gtsam::SharedNoiseModel()); const FastVector expectedNewFactorIndices = list_of(1)(3); @@ -696,16 +695,16 @@ TEST(ISAM2, marginalizeLeaves1) ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += PriorFactor(0, 0.0, noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 1, 0.0, noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, 0.0, noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, 0.0, noiseModel::Unit::Create(1)); Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); + values.insert(0, 0.0); + values.insert(1, 0.0); + values.insert(2, 0.0); FastMap constrainedKeys; constrainedKeys.insert(make_pair(0,0)); @@ -724,18 +723,18 @@ TEST(ISAM2, marginalizeLeaves2) ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += PriorFactor(0, 0.0, noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 1, 0.0, noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, 0.0, noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, 0.0, noiseModel::Unit::Create(1)); + factors += BetweenFactor(2, 3, 0.0, noiseModel::Unit::Create(1)); 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, 0.0); + values.insert(1, 0.0); + values.insert(2, 0.0); + values.insert(3, 0.0); FastMap constrainedKeys; constrainedKeys.insert(make_pair(0,0)); @@ -755,25 +754,25 @@ TEST(ISAM2, marginalizeLeaves3) ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += PriorFactor(0, 0.0, noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 1, 0.0, noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, 0.0, noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, 0.0, noiseModel::Unit::Create(1)); - factors += BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(2, 3, 0.0, noiseModel::Unit::Create(1)); - factors += BetweenFactor(3, 4, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(4, 5, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(3, 5, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(3, 4, 0.0, noiseModel::Unit::Create(1)); + factors += BetweenFactor(4, 5, 0.0, noiseModel::Unit::Create(1)); + factors += BetweenFactor(3, 5, 0.0, noiseModel::Unit::Create(1)); 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, 0.0); + values.insert(1, 0.0); + values.insert(2, 0.0); + values.insert(3, 0.0); + values.insert(4, 0.0); + values.insert(5, 0.0); FastMap constrainedKeys; constrainedKeys.insert(make_pair(0,0)); @@ -795,14 +794,14 @@ TEST(ISAM2, marginalizeLeaves4) ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += PriorFactor(0, 0.0, noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, 0.0, noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, 0.0, noiseModel::Unit::Create(1)); Values values; - values.insert(0, LieVector(0.0)); - values.insert(1, LieVector(0.0)); - values.insert(2, LieVector(0.0)); + values.insert(0, 0.0); + values.insert(1, 0.0); + values.insert(2, 0.0); FastMap constrainedKeys; constrainedKeys.insert(make_pair(0,0));