From 244ea20f9c5061552fdb143b016344db5d711b6a Mon Sep 17 00:00:00 2001
From: Vadim Indelman <indelman@gmail.com>
Date: Mon, 8 Apr 2013 22:49:55 +0000
Subject: [PATCH] Clean-up.

---
 .../EquivInertialNavFactor_GlobalVel.h        | 25 ++-----------------
 1 file changed, 2 insertions(+), 23 deletions(-)

diff --git a/gtsam_unstable/navigation/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/navigation/EquivInertialNavFactor_GlobalVel.h
index 910cc8d63..16063844e 100644
--- a/gtsam_unstable/navigation/EquivInertialNavFactor_GlobalVel.h
+++ b/gtsam_unstable/navigation/EquivInertialNavFactor_GlobalVel.h
@@ -221,7 +221,7 @@ public:
 		/* Rotation term */
 		Vector body_deltaAngles_body = delta_angles;
 
-	    // Convert earth-related terms into the body frame
+		// Convert earth-related terms into the body frame
 		Matrix body_R_world(world_P1_body.rotation().inverse().matrix());
 		Vector body_rho = body_R_world * world_rho;
 		Vector body_omega_earth = body_R_world * world_omega_earth;
@@ -229,14 +229,6 @@ public:
 		// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
 		body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12;
 
-		// debug
-		//		std::cout<<"Pose1.rotation(): "<<std::endl<<Pose1.rotation().matrix()<<std::endl;
-		//		std::cout<<"delta_angles before earth terms: "<<delta_angles.transpose()<<std::endl;
-		//    std::cout<<"delta_angles after earth terms : "<<body_deltaAngles_body.transpose()<<std::endl;
-		//    std::cout<<"body_rho + body_omega_earth (should include dt_) : "<<body_rho.transpose() + body_omega_earth.transpose()<<std::endl;
-		//    Rot3 R = Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body);
-		//    std::cout<<"calculated Pose2.rotation(): "<<std::endl<<R.matrix()<<std::endl;
-
 		return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body));
 
 	}
@@ -276,15 +268,6 @@ public:
 		// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
 		VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12;
 
-		// debug
-		//		std::cout<<"... inside predictVelocity_inertial"<<std::endl;
-		//		std::cout<<"Vel1: "<<Vel1.transpose()   <<std::endl;
-		//		std::cout<<"Vel1.compose(VelDelta): "<<Vel1.compose(VelDelta).transpose()   <<std::endl;
-		//		std::cout<<"VelDelta before earth related terms: "<<world_deltaVel_body.transpose()   <<std::endl;
-		//		std::cout<<"VelDelta after earth related terms : "<<VelDelta.transpose()		<<std::endl;
-		//    std::cout<<"world_P1_body.rotation().matrix() : "<<std::endl<<world_P1_body.rotation().matrix()<<std::endl;
-
-
 		// Predict
 		return Vel1.compose( VelDelta );
 
@@ -424,17 +407,13 @@ public:
   }
 
 
-
-
-
-
   static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt,
       Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
 			const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
 			Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(),
 			boost::optional<POSE> p_body_P_sensor = boost::none){
 		// Note: all delta terms refer to an IMU\sensor system at t0
-		// Note: Earth-related terms are not accounted here but are incorporate in predict functions.
+		// Note: Earth-related terms are not accounted here but are incorporated in predict functions.
 
 		POSE body_P_sensor = POSE();
 		bool flag_use_body_P_sensor = false;