diff --git a/.cproject b/.cproject index a596e90bf..13912e713 100644 --- a/.cproject +++ b/.cproject @@ -600,6 +600,7 @@ make + tests/testBayesTree.run true false @@ -607,6 +608,7 @@ make + testBinaryBayesNet.run true false @@ -654,6 +656,7 @@ make + testSymbolicBayesNet.run true false @@ -661,6 +664,7 @@ make + tests/testSymbolicFactor.run true false @@ -668,6 +672,7 @@ make + testSymbolicFactorGraph.run true false @@ -683,6 +688,7 @@ make + tests/testBayesTree true false @@ -736,14 +742,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - make -j5 @@ -1114,6 +1112,7 @@ make + testErrors.run true false @@ -1343,6 +1342,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1425,7 +1464,6 @@ make - testSimulated2DOriented.run true false @@ -1465,7 +1503,6 @@ make - testSimulated2D.run true false @@ -1473,7 +1510,6 @@ make - testSimulated3D.run true false @@ -1487,46 +1523,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1784,6 +1780,7 @@ cpack + -G DEB true false @@ -1791,6 +1788,7 @@ cpack + -G RPM true false @@ -1798,6 +1796,7 @@ cpack + -G TGZ true false @@ -1805,6 +1804,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2579,6 +2579,7 @@ make + testGraph.run true false @@ -2586,6 +2587,7 @@ make + testJunctionTree.run true false @@ -2593,6 +2595,7 @@ make + testSymbolicBayesNetB.run true false @@ -3112,7 +3115,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index bc8c1f5c8..f7130d611 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -268,7 +268,7 @@ public: VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12; // Predict - return Vel1.compose( VelDelta ); + return Vel1 + VelDelta; } @@ -294,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, @@ -343,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); } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index cd203c96b..908239d93 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -199,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 { @@ -220,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 */ @@ -270,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/tests/testEquivInertialNavFactor_GlobalVel.cpp b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp index 7667dc7c3..c0da655c6 100644 --- a/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp +++ b/gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp @@ -54,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 a91a5b05b..ad4efe26c 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -28,80 +28,71 @@ 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); @@ -109,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); @@ -146,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)); @@ -172,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); @@ -182,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)); @@ -207,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); @@ -217,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)); @@ -252,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); @@ -269,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)); @@ -345,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)); @@ -367,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); @@ -383,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); @@ -409,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); @@ -434,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); @@ -474,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)); @@ -503,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); @@ -513,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)); @@ -542,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); @@ -552,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)); @@ -591,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); @@ -601,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)); @@ -661,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)); @@ -684,5 +700,8 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { } /* ************************************************************************* */ - int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */