formatting changes

gtsam/navigation/ImuFactor.h

@@ -138,31 +138,23 @@ public:
     void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
         boost::optional<const Pose3&> body_P_sensor = boost::none);
 
-    // TODO: move to testImuFactor
     /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
     // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
     static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
         const Vector3& delta_angles, const Vector& delta_vel_in_t0){
-
       // Note: all delta terms refer to an IMU\sensor system at t0
-
       Vector body_t_a_body = msr_acc_t;
       Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-
       return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
     }
 
     // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
     static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
         const Vector3& delta_angles){
-
       // Note: all delta terms refer to an IMU\sensor system at t0
-
       // Calculate the corrected measurements using the Bias object
       Vector body_t_omega_body= msr_gyro_t;
-
       Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-
       R_t_to_t0    = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
       return Rot3::Logmap(R_t_to_t0);
     }

gtsam/navigation/tests/testImuFactor.cpp

@@ -39,15 +39,13 @@ using symbol_shorthand::B;
 namespace {
 Vector callEvaluateError(const ImuFactor& factor,
     const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
-    const imuBias::ConstantBias& bias)
+    const imuBias::ConstantBias& bias){
-{
   return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias);
 }
 
 Rot3 evaluateRotationError(const ImuFactor& factor,
     const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
-    const imuBias::ConstantBias& bias)
+    const imuBias::ConstantBias& bias){
-{
   return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
 }
 
@@ -56,9 +54,7 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
     const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas,
     const list<double>& deltaTs,
-    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
+    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)  ){
-    )
-{
   ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
       Matrix3::Identity(), Matrix3::Identity());
 
@@ -68,7 +64,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
   for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
     result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
   }
-
   return result;
 }
 
@@ -77,8 +72,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
     const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas,
     const list<double>& deltaTs,
-    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
+    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
-{
   return evaluatePreintegratedMeasurements(bias,
       measuredAccs, measuredOmegas, deltaTs).deltaPij();
 }
@@ -99,20 +93,16 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
     const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas,
     const list<double>& deltaTs,
-    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
+    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
-{
   return Rot3(evaluatePreintegratedMeasurements(bias,
       measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij());
 }
 
-Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT)
+Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){
-{
   return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
 }
 
-
+Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){
-Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta)
-{
   return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
 }
 
@@ -212,7 +202,6 @@ TEST( ImuFactor, Error )
   Matrix H1a, H2a, H3a, H4a, H5a;
   (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
 
-
   // positions and velocities
   Matrix H1etop6 =  H1e.topRows(6);
   Matrix H1atop6 =  H1a.topRows(6);
@@ -230,7 +219,7 @@ TEST( ImuFactor, Error )
   EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5));  // 1e-5 needs to be added only when using quaternions for rotations
 
   EXPECT(assert_equal(H4e, H4a));
-//  EXPECT(assert_equal(H5e, H5a));
+  // EXPECT(assert_equal(H5e, H5a));
 }
 
 /* ************************************************************************* */
@@ -243,7 +232,6 @@ TEST( ImuFactor, ErrorWithBiases )
 //  Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
 //  Vector3 v2(Vector3(0.5, 0.0, 0.0));
 
-
   imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
   Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
   Vector3 v1(Vector3(0.5, 0.0, 0.0));
@@ -260,8 +248,8 @@ TEST( ImuFactor, ErrorWithBiases )
   ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
     pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
-//  ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
+  //  ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
-//    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+  //    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
     // Create factor
     ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
@@ -272,7 +260,7 @@ TEST( ImuFactor, ErrorWithBiases )
 
     // Expected error
     Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
-//    EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
+    // EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
 
     // Expected Jacobians
     Matrix H1e = numericalDerivative11<Vector,Pose3>(
@@ -315,7 +303,6 @@ TEST( ImuFactor, PartialDerivativeExpmap )
   Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
   double deltaT = 0.5;
 
-
   // Compute numerical derivatives
   Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind(
       &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega));
@@ -326,7 +313,6 @@ TEST( ImuFactor, PartialDerivativeExpmap )
 
   // Compare Jacobians
   EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3));  // 1e-3 needs to be added only when using quaternions for rotations
-
 }
 
 /* ************************************************************************* */
@@ -349,9 +335,6 @@ TEST( ImuFactor, PartialDerivativeLogmap )
   const Matrix3  actualDelFdeltheta = Matrix3::Identity() +
        0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
 
-//  std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl;
-//  std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl;
-
   // Compare Jacobians
   EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
 
@@ -495,7 +478,6 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
 //  tictoc_print_();
 //}
 
-
 /* ************************************************************************* */
 TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
 {
@@ -515,15 +497,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
 
   const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0));
 
-//  ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
-//        Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmega);
-
-
   ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
         Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
 
-
-
   pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
     // Create factor
@@ -560,6 +536,7 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
     EXPECT(assert_equal(H5e, H5a));
 }
 
+/* ************************************************************************* */
 TEST(ImuFactor, PredictPositionAndVelocity){
   imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
 
@@ -593,6 +570,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){
 
 }
 
+/* ************************************************************************* */
 TEST(ImuFactor, PredictRotation) {
   imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)