diff --git a/.cproject b/.cproject index a596e90bf..4ab3bd70b 100644 --- a/.cproject +++ b/.cproject @@ -736,14 +736,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - make -j5 @@ -2265,6 +2257,14 @@ true true + + make + -j5 + testVelocityConstraint3.run + true + true + true + make -j1 @@ -2766,10 +2766,10 @@ true true - + make -j5 - testBetweenFactor.run + testPriorFactor.run true true true 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/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 17ad0bf42..90a9cceda 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/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)); diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index f70c3496a..1650df0ec 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/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)); 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/ImuBias.h b/gtsam/navigation/ImuBias.h index 9bc24c152..18713505e 100644 --- a/gtsam/navigation/ImuBias.h +++ b/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 */ 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/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index fc9d4f62b..96a0971c5 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -19,7 +19,6 @@ #include #include #include -#include 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 { +class MagFactor3: public NoiseModelFactor3 { 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(model, key1, key2, key3), // + NoiseModelFactor3(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 H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 279c20e69..5fca3343e 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include #include @@ -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; diff --git a/gtsam/nonlinear/NonlinearFactor.cpp b/gtsam/nonlinear/NonlinearFactor.cpp index f6f43b062..e9b97d644 100644 --- a/gtsam/nonlinear/NonlinearFactor.cpp +++ b/gtsam/nonlinear/NonlinearFactor.cpp @@ -18,6 +18,7 @@ #include #include +#include namespace gtsam { @@ -62,7 +63,8 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey( void NoiseModelFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const { Base::print(s, keyFormatter); - noiseModel_->print(" noise model: "); + 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()); - return 0.5 * noiseModel_->distance(b); + if (noiseModel_) + return 0.5 * noiseModel_->distance(b); + else + return 0.5 * b.squaredNorm(); } else { return 0.0; } @@ -115,7 +121,8 @@ boost::shared_ptr NoiseModelFactor::linearize( check(noiseModel_, b.size()); // Whiten the corresponding system now - noiseModel_->WhitenSystem(A, b); + if (noiseModel_) + noiseModel_->WhitenSystem(A, b); // Fill in terms, needed to create JacobianFactor below std::vector > terms(size()); @@ -125,7 +132,7 @@ boost::shared_ptr 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_)->unit())); 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..60639e8b7 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((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))); } /* ************************************************************************* */ @@ -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/slam/tests/testEssentialMatrixFactor.cpp b/gtsam/slam/tests/testEssentialMatrixFactor.cpp index c889fb1e9..46e283d34 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_DOUBLES_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_DOUBLES_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_DOUBLES_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/slam/tests/testPriorFactor.cpp b/gtsam/slam/tests/testPriorFactor.cpp new file mode 100644 index 000000000..b3e54bedb --- /dev/null +++ b/gtsam/slam/tests/testPriorFactor.cpp @@ -0,0 +1,28 @@ +/** + * @file testPriorFactor.cpp + * @brief Test PriorFactor + * @author Frank Dellaert + * @date Nov 4, 2014 + */ + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ + +// Constructor +TEST(PriorFactor, Constructor) { + SharedNoiseModel model; + PriorFactor factor(1, LieScalar(1.0), model); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam_unstable/dynamics/FullIMUFactor.h b/gtsam_unstable/dynamics/FullIMUFactor.h index b8dba6b61..1c35fc9b8 100644 --- a/gtsam_unstable/dynamics/FullIMUFactor.h +++ b/gtsam_unstable/dynamics/FullIMUFactor.h @@ -7,7 +7,6 @@ #pragma once #include -#include #include #include @@ -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 H1 = boost::none, boost::optional 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( + 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) { - 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; } }; diff --git a/gtsam_unstable/dynamics/IMUFactor.h b/gtsam_unstable/dynamics/IMUFactor.h index 4ad0635cf..5fb4d77aa 100644 --- a/gtsam_unstable/dynamics/IMUFactor.h +++ b/gtsam_unstable/dynamics/IMUFactor.h @@ -7,7 +7,6 @@ #pragma once #include -#include #include #include @@ -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 H1 = boost::none, boost::optional H2 = boost::none) const { - const Vector meas = z(); - if (H1) *H1 = numericalDerivative21( + const Vector6 meas = z(); + if (H1) *H1 = numericalDerivative21( boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); - if (H2) *H2 = numericalDerivative22( + if (H2) *H2 = numericalDerivative22( 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; } }; diff --git a/gtsam_unstable/dynamics/Pendulum.h b/gtsam_unstable/dynamics/Pendulum.h index 8cfc3cbcd..0100e17c7 100644 --- a/gtsam_unstable/dynamics/Pendulum.h +++ b/gtsam_unstable/dynamics/Pendulum.h @@ -9,7 +9,6 @@ #pragma once -#include #include 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 { +class PendulumFactor1: public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 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 H1 = boost::none, boost::optional H2 = boost::none, boost::optional 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 { +class PendulumFactor2: public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 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 H1 = boost::none, boost::optional H2 = boost::none, boost::optional 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 { +class PendulumFactorPk: public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 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 H1 = boost::none, boost::optional H2 = boost::none, boost::optional 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 { +class PendulumFactorPk1: public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 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 H1 = boost::none, boost::optional H2 = boost::none, boost::optional 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 diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 2246baee1..ed8d54696 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/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; diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index ae4ddade4..43d018752 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/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 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/gtsam_unstable/dynamics/VelocityConstraint3.h b/gtsam_unstable/dynamics/VelocityConstraint3.h index 83506e2d5..1a432ba1e 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint3.h +++ b/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 #include namespace gtsam { -class VelocityConstraint3 : public NoiseModelFactor3 { +class VelocityConstraint3 : public NoiseModelFactor3 { public: protected: - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 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 H1 = boost::none, boost::optional H2 = boost::none, boost::optional 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: diff --git a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp b/gtsam_unstable/dynamics/tests/testIMUSystem.cpp index be7078d9a..550b0e33c 100644 --- a/gtsam_unstable/dynamics/tests/testIMUSystem.cpp +++ b/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; diff --git a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp b/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp index ce176787c..5a4593270 100644 --- a/gtsam_unstable/dynamics/tests/testPendulumFactors.cpp +++ b/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)); diff --git a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp index db4b7c586..ac27ae563 100644 --- a/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp +++ b/gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp @@ -8,23 +8,16 @@ #include #include -/* ************************************************************************* */ using namespace gtsam; - -namespace { - - const double tol=1e-5; - const double dt = 1.0; - - LieScalar origin, - x1(1.0), x2(2.0), v(1.0); - -} +using namespace gtsam::symbol_shorthand; /* ************************************************************************* */ +// evaluateError TEST( testVelocityConstraint3, evaluateError) { - using namespace gtsam::symbol_shorthand; + const double tol = 1e-5; + const double dt = 1.0; + 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); +} /* ************************************************************************* */ diff --git a/gtsam_unstable/geometry/InvDepthCamera3.h b/gtsam_unstable/geometry/InvDepthCamera3.h index 30f17fb7a..23d698d86 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 @@ -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 H1 = boost::none, boost::optional H2 = boost::none, boost::optional 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 backproject(const gtsam::Point2& pi, const double depth) const { + inline std::pair 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: diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index 3385998b0..e72afdc25 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -30,9 +30,9 @@ TEST( InvDepthFactor, Project1) { InvDepthCamera3 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 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 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 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(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 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 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 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 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 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); } /* ************************************************************************* */ 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/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp index af562c1b2..1719abc86 100644 --- a/gtsam_unstable/slam/AHRS.cpp +++ b/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 diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index 41f216b77..f7130d611 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 @@ -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(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..908239d93 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 @@ -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); } diff --git a/gtsam_unstable/slam/InvDepthFactor3.h b/gtsam_unstable/slam/InvDepthFactor3.h index 29f9d4972..ae30a4a49 100644 --- a/gtsam_unstable/slam/InvDepthFactor3.h +++ b/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 H1=boost::none, boost::optional H2=boost::none, boost::optional H3=boost::none) const { diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index d2a784b0f..6bf0722a5 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -15,7 +15,6 @@ #include #include #include -#include #include namespace gtsam { @@ -23,7 +22,7 @@ namespace gtsam { /** * Binary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant1: public NoiseModelFactor2 { +class InvDepthFactorVariant1: public NoiseModelFactor2 { protected: // Keep a copy of measurement and calibration for I/O @@ -33,7 +32,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 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 H1=boost::none, boost::optional H2=boost::none) const { diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 3c95a5010..ae26b4240 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -16,7 +16,6 @@ #include #include #include -#include #include namespace gtsam { @@ -24,7 +23,7 @@ namespace gtsam { /** * Binary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant2: public NoiseModelFactor2 { +class InvDepthFactorVariant2: public NoiseModelFactor2 { protected: // Keep a copy of measurement and calibration for I/O @@ -35,7 +34,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 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 H1=boost::none, boost::optional H2=boost::none) const { diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index 2f94227dc..f805e26a4 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -15,7 +15,6 @@ #include #include #include -#include #include namespace gtsam { @@ -23,7 +22,7 @@ namespace gtsam { /** * Binary factor representing the first visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant3a: public NoiseModelFactor2 { +class InvDepthFactorVariant3a: public NoiseModelFactor2 { protected: // Keep a copy of measurement and calibration for I/O @@ -33,7 +32,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 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 H1=boost::none, boost::optional H2=boost::none) const { @@ -142,7 +141,7 @@ private: /** * Ternary factor representing a visual measurement using an inverse-depth parameterization */ -class InvDepthFactorVariant3b: public NoiseModelFactor3 { +class InvDepthFactorVariant3b: public NoiseModelFactor3 { protected: // Keep a copy of measurement and calibration for I/O @@ -152,7 +151,7 @@ protected: public: /// shorthand for base class type - typedef NoiseModelFactor3 Base; + typedef NoiseModelFactor3 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 H1=boost::none, boost::optional H2=boost::none, boost::optional H3=boost::none) const { - if(H1) { + if(H1) (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); - } - if(H2) { + + if(H2) (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); - } - if(H3) { + + if(H3) (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); - } return inverseDepthError(pose1, pose2, landmark); } 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..c0da655c6 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 @@ -55,7 +54,7 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor) SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9)); // Constructor - EquivInertialNavFactor_GlobalVel factor( + EquivInertialNavFactor_GlobalVel 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); diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 57a19ee78..ad4efe26c 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -23,86 +23,76 @@ #include #include #include -#include #include 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& factor) { +Pose3 predictionErrorPose(const Pose3& p1, const Vector3& v1, + const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2, + const InertialNavFactor_GlobalVelocity& 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& factor) { +Vector predictionErrorVel(const Pose3& p1, const Vector3& v1, + const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2, + const InertialNavFactor_GlobalVelocity& 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 f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity 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 f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); - InertialNavFactor_GlobalVelocity g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity f( + Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, + measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity 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_acc((Vector(3) << 0.1, 0.2, 0.3 - 9.81)); Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity 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_acc((Vector(3) << 0.1, 0.2, 0.3 - 9.81)); Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); - InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity 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,23 +165,23 @@ 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)); // Second test: zero angular motion, some acceleration - Vector measurement_acc((Vector(3) <<0.0,0.0,0.0-9.81)); + 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 f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity 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 Pose1(Rot3(), Point3(2.0, 1.0, 3.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 f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity 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); - Point3 t1(2.0,1.0,3.0); + 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); - Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); + 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(boost::bind(&predictionRq, _1, q), angles); +// J_expected = numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); // // cout<<"J_hyp"< factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); + InertialNavFactor_GlobalVelocity 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); - Point3 t1(2.0,1.0,3.0); + 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); - Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); + 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 // ****** - Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols())); - Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols())); - Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols())); - Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols())); - Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols())); + Matrix H1_actualPose(H1_actual.block(0, 0, 6, H1_actual.cols())); + Matrix H2_actualPose(H2_actual.block(0, 0, 6, H2_actual.cols())); + Matrix H3_actualPose(H3_actual.block(0, 0, 6, H3_actual.cols())); + Matrix H4_actualPose(H4_actual.block(0, 0, 6, H4_actual.cols())); + 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(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, + H5_expectedPose; + H1_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), + Pose1); + H2_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), + Vel1); + H3_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), + Bias1); + H4_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), + Pose2); + H5_expectedPose = numericalDerivative11( + 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)); @@ -346,19 +334,30 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) { // Checking for Vel part in the jacobians // ****** - Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols())); - Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols())); - Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols())); - Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols())); - Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); + Matrix H1_actualVel(H1_actual.block(6, 0, 3, H1_actual.cols())); + Matrix H2_actualVel(H2_actual.block(6, 0, 3, H2_actual.cols())); + Matrix H3_actualVel(H3_actual.block(6, 0, 3, H3_actual.cols())); + Matrix H4_actualVel(H4_actual.block(6, 0, 3, H4_actual.cols())); + 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(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, + H5_expectedVel; + H1_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), + Pose1); + H2_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), + Vel1); + H3_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), + Bias1); + H4_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), + Pose2); + H5_expectedVel = numericalDerivative11( + 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 + 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 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 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 + 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 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 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 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 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 - + 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 + 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 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 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 - + 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 + 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 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 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 - + 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 + 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 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 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 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)); + 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 - + 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 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 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); - Point3 t1(2.0,1.0,3.0); + 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); - Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) ); + 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,56 +594,68 @@ 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 + 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 + 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 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 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); - Point3 t1(2.0,1.0,3.0); + 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); - Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800); + 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 // ****** - Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols())); - Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols())); - Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols())); - Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols())); - Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols())); + Matrix H1_actualPose(H1_actual.block(0, 0, 6, H1_actual.cols())); + Matrix H2_actualPose(H2_actual.block(0, 0, 6, H2_actual.cols())); + Matrix H3_actualPose(H3_actual.block(0, 0, 6, H3_actual.cols())); + Matrix H4_actualPose(H4_actual.block(0, 0, 6, H4_actual.cols())); + 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(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, + H5_expectedPose; + H1_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), + Pose1); + H2_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), + Vel1); + H3_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), + Bias1); + H4_expectedPose = numericalDerivative11( + boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), + Pose2); + H5_expectedPose = numericalDerivative11( + 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)); @@ -662,19 +666,30 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { // Checking for Vel part in the jacobians // ****** - Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols())); - Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols())); - Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols())); - Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols())); - Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); + Matrix H1_actualVel(H1_actual.block(6, 0, 3, H1_actual.cols())); + Matrix H2_actualVel(H2_actual.block(6, 0, 3, H2_actual.cols())); + Matrix H3_actualVel(H3_actual.block(6, 0, 3, H3_actual.cols())); + Matrix H4_actualVel(H4_actual.block(6, 0, 3, H4_actual.cols())); + 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(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, + H5_expectedVel; + H1_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), + Pose1); + H2_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), + Vel1); + H3_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), + Bias1); + H4_expectedVel = numericalDerivative11( + boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), + Pose2); + H5_expectedVel = numericalDerivative11( + 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); +} /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp index 5ea9fe29d..bf15c7322 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactor3.cpp +++ b/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 InverseDepthFactor; +typedef InvDepthFactor3 InverseDepthFactor; typedef NonlinearEquality PoseConstraint; /* ************************************************************************* */ @@ -38,10 +38,10 @@ TEST( InvDepthFactor, optimize) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 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::invDepthTo3D( - result2.at(Symbol('l',1)), - result2.at(Symbol('d',1))); + result2.at(Symbol('l',1)), + result2.at(Symbol('d',1))); EXPECT(assert_equal(landmark, result2_lmk, 1e-9)); // TODO: need to add priors to make this work with diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 503a4b953..bb3b81481 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/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(landmarkKey); + Vector6 actual = result.at(landmarkKey); // values.at(poseKey1).print("Pose1 Before:\n"); @@ -84,22 +84,22 @@ TEST( InvDepthFactorVariant1, optimize) { // result.at(poseKey2).print("Pose2 After:\n"); // pose2.print("Pose2 Truth:\n"); // cout << endl << endl; -// values.at(landmarkKey).print("Landmark Before:\n"); -// result.at(landmarkKey).print("Landmark After:\n"); +// values.at(landmarkKey).print("Landmark Before:\n"); +// result.at(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(landmarkKey); + Vector6 landmarkBefore = values.at(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(landmarkKey); + Vector6 landmarkAfter = result.at(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); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index 49e5fc2aa..23f642a13 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/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(landmarkKey); + Vector3 actual = result.at(landmarkKey); // values.at(poseKey1).print("Pose1 Before:\n"); // result.at(poseKey1).print("Pose1 After:\n"); @@ -81,21 +81,21 @@ TEST( InvDepthFactorVariant2, optimize) { // result.at(poseKey2).print("Pose2 After:\n"); // pose2.print("Pose2 Truth:\n"); // std::cout << std::endl << std::endl; -// values.at(landmarkKey).print("Landmark Before:\n"); -// result.at(landmarkKey).print("Landmark After:\n"); +// values.at(landmarkKey).print("Landmark Before:\n"); +// result.at(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(landmarkKey); + Vector3 landmarkBefore = values.at(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(landmarkKey); + Vector3 landmarkAfter = result.at(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)); } diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index 11a91f687..e0be9c79c 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/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(landmarkKey); + Vector3 actual = result.at(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)); } diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index a063244a3..7e2aa507e 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 @@ -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& factor){ +Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor& 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& factor){ +//Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& 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(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); + Matrix H1_expected = gtsam::numericalDerivative11(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(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); -// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(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(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(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);} /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index 1ffb2bebe..146cdc06f 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 @@ -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& factor){ +Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM& 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& factor){ +//Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& 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(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); + Matrix H1_expected = gtsam::numericalDerivative11(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(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); -// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(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(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(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);} /* ************************************************************************* */ 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/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 55329d8e9..553a7fd5f 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -6,24 +6,24 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include -#include -#include -#include -#include #include #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -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(10, LieVector(), gtsam::SharedNoiseModel()); + nonlinearFactors += PriorFactor(10, Zero, model); nonlinearFactors += NonlinearFactor::shared_ptr(); - nonlinearFactors += PriorFactor(11, LieVector(), gtsam::SharedNoiseModel()); + nonlinearFactors += PriorFactor(11, Zero, model); NonlinearFactorGraph newFactors; - newFactors += PriorFactor(1, LieVector(), gtsam::SharedNoiseModel()); - newFactors += PriorFactor(2, LieVector(), gtsam::SharedNoiseModel()); + newFactors += PriorFactor(1, Zero, model); + newFactors += PriorFactor(2, Zero, model); 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, Zero, model); + expectedNonlinearFactors += PriorFactor(1, Zero, model); + expectedNonlinearFactors += PriorFactor(11, Zero, model); + expectedNonlinearFactors += PriorFactor(2, Zero, model); const FastVector expectedNewFactorIndices = list_of(1)(3); @@ -694,18 +697,17 @@ namespace { TEST(ISAM2, marginalizeLeaves1) { ISAM2 isam; - NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += PriorFactor(0, Zero, model); - 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, Zero, model); + factors += BetweenFactor(1, 2, Zero, model); + factors += BetweenFactor(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 constrainedKeys; constrainedKeys.insert(make_pair(0,0)); @@ -724,18 +726,18 @@ TEST(ISAM2, marginalizeLeaves2) ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += PriorFactor(0, Zero, model); - 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, Zero, model); + factors += BetweenFactor(1, 2, Zero, model); + factors += BetweenFactor(0, 2, Zero, model); + factors += BetweenFactor(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 constrainedKeys; constrainedKeys.insert(make_pair(0,0)); @@ -755,25 +757,25 @@ TEST(ISAM2, marginalizeLeaves3) ISAM2 isam; NonlinearFactorGraph factors; - factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += PriorFactor(0, Zero, model); - 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, Zero, model); + factors += BetweenFactor(1, 2, Zero, model); + factors += BetweenFactor(0, 2, Zero, model); - factors += BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(2, 3, Zero, model); - 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, Zero, model); + factors += BetweenFactor(4, 5, Zero, model); + factors += BetweenFactor(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 constrainedKeys; constrainedKeys.insert(make_pair(0,0)); @@ -795,14 +797,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, Zero, model); + factors += BetweenFactor(0, 2, Zero, model); + factors += BetweenFactor(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 constrainedKeys; constrainedKeys.insert(make_pair(0,0)); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 739fe52fb..025a3c12e 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -27,7 +27,6 @@ #include #include -#include #include #include #include @@ -235,13 +234,13 @@ TEST( NonlinearFactor, linearize_constraint2 ) } /* ************************************************************************* */ -class TestFactor4 : public NoiseModelFactor4 { +class TestFactor4 : public NoiseModelFactor4 { public: - typedef NoiseModelFactor4 Base; + typedef NoiseModelFactor4 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 H1 = boost::none, boost::optional H2 = boost::none, boost::optional 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(tf.linearize(tv))); @@ -283,9 +282,9 @@ TEST(NonlinearFactor, NoiseModelFactor4) { } /* ************************************************************************* */ -class TestFactor5 : public NoiseModelFactor5 { +class TestFactor5 : public NoiseModelFactor5 { public: - typedef NoiseModelFactor5 Base; + typedef NoiseModelFactor5 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(tf.linearize(tv))); @@ -332,9 +331,9 @@ TEST(NonlinearFactor, NoiseModelFactor5) { } /* ************************************************************************* */ -class TestFactor6 : public NoiseModelFactor6 { +class TestFactor6 : public NoiseModelFactor6 { public: - typedef NoiseModelFactor6 Base; + typedef NoiseModelFactor6 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(tf.linearize(tv)));