added imu factor (global velocity)
							parent
							
								
									cc08659f7b
								
							
						
					
					
						commit
						4f5b9f2074
					
				|  | @ -0,0 +1,184 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file ImuBias.h | ||||
|  * @date  Feb 2, 2012 | ||||
|  * @author Vadim Indelman, Stephen Williams | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <boost/serialization/nvp.hpp> | ||||
| #include <gtsam/base/Matrix.h> | ||||
| #include <gtsam/base/Vector.h> | ||||
| #include <gtsam/base/DerivedValue.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| 
 | ||||
| /*
 | ||||
|  * NOTES: | ||||
|  * - Earth-rate correction: | ||||
|  * 		+ Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defened by the user). | ||||
|  * 		+ R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. | ||||
|  *		+ A relatively small distance is traveled w.r.t. to initial pose is assumed, since R_ECEF_to_G is constant. | ||||
|  *		    Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. | ||||
|  * | ||||
|  *	- Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G. | ||||
|  */ | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 	/// All noise models live in the noiseModel namespace
 | ||||
| 	namespace imuBias { | ||||
| 
 | ||||
|   class ConstantBias : public DerivedValue<ConstantBias> { | ||||
| 	private: | ||||
| 		Vector bias_acc_; | ||||
| 		Vector bias_gyro_; | ||||
| 
 | ||||
| 	public: | ||||
| 
 | ||||
|     ConstantBias(): | ||||
|       bias_acc_(Vector_(3, 0.0, 0.0, 0.0)),  bias_gyro_(Vector_(3, 0.0, 0.0, 0.0)) { | ||||
|     } | ||||
| 
 | ||||
|     ConstantBias(const Vector& bias_acc, const Vector& bias_gyro): | ||||
|       bias_acc_(bias_acc),  bias_gyro_(bias_gyro) { | ||||
|     } | ||||
| 
 | ||||
| 		Vector CorrectAcc(Vector measurment, boost::optional<Matrix&> H=boost::none) const { | ||||
| 		  if (H){ | ||||
| 				Matrix zeros3_3(zeros(3,3)); | ||||
| 				Matrix m_eye3(-eye(3)); | ||||
| 
 | ||||
| 				*H = collect(2, &m_eye3, &zeros3_3); | ||||
| 			} | ||||
| 
 | ||||
| 			return measurment - bias_acc_; | ||||
| 		} | ||||
| 
 | ||||
| 
 | ||||
| 		Vector CorrectGyro(Vector measurment, boost::optional<Matrix&> H=boost::none) const { | ||||
| 			if (H){ | ||||
| 				Matrix zeros3_3(zeros(3,3)); | ||||
| 				Matrix m_eye3(-eye(3)); | ||||
| 
 | ||||
| 				*H = collect(2, &zeros3_3, &m_eye3); | ||||
| 			} | ||||
| 
 | ||||
| 			return measurment - bias_gyro_; | ||||
| 		} | ||||
| 
 | ||||
| 		// H1: Jacobian w.r.t. IMUBias
 | ||||
| 		// H2: Jacobian w.r.t. pose
 | ||||
| 		Vector CorrectGyroWithEarthRotRate(Vector measurement, const Pose3& pose, const Vector& w_earth_rate_G, | ||||
| 				boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const { | ||||
| 
 | ||||
| 			Matrix R_G_to_I( pose.rotation().matrix().transpose() ); | ||||
| 			Vector w_earth_rate_I = R_G_to_I * w_earth_rate_G; | ||||
| 
 | ||||
| 			if (H1){ | ||||
| 				Matrix zeros3_3(zeros(3,3)); | ||||
| 				Matrix m_eye3(-eye(3)); | ||||
| 
 | ||||
| 				*H1 = collect(2, &zeros3_3, &m_eye3); | ||||
| 			} | ||||
| 
 | ||||
| 			if (H2){ | ||||
| 				Matrix zeros3_3(zeros(3,3)); | ||||
| 				Matrix H = -skewSymmetric(w_earth_rate_I); | ||||
| 
 | ||||
| 				*H2 = collect(2, &H, &zeros3_3); | ||||
| 			} | ||||
| 
 | ||||
| 			//TODO: Make sure H2 is correct.
 | ||||
| 
 | ||||
| 			return measurement - bias_gyro_ - w_earth_rate_I; | ||||
| 
 | ||||
| //			Vector bias_gyro_temp(Vector_(3, -bias_gyro_(0), bias_gyro_(1), bias_gyro_(2)));
 | ||||
| //			return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G;
 | ||||
| 		} | ||||
| 
 | ||||
| 		/** Expmap around identity */ | ||||
| 		static inline ConstantBias Expmap(const Vector& v) { return ConstantBias(v.head(3), v.tail(3)); } | ||||
| 
 | ||||
| 		/** Logmap around identity - just returns with default cast back */ | ||||
| 		static inline Vector Logmap(const ConstantBias& p) { return concatVectors(2, &p.bias_acc_, &p.bias_gyro_); } | ||||
| 
 | ||||
| 		/** Update the LieVector with a tangent space update */ | ||||
| 		inline ConstantBias retract(const Vector& v) const { return ConstantBias(bias_acc_ + v.head(3), bias_gyro_ + v.tail(3)); } | ||||
| 
 | ||||
| 		/** @return the local coordinates of another object */ | ||||
| 		inline Vector localCoordinates(const ConstantBias& t2) const { | ||||
| 			Vector delta_acc(t2.bias_acc_ - bias_acc_); | ||||
| 			Vector delta_gyro(t2.bias_gyro_ - bias_gyro_); | ||||
| 			return concatVectors(2, &delta_acc, &delta_gyro); | ||||
| 		} | ||||
| 
 | ||||
| 		/** Returns dimensionality of the tangent space */ | ||||
| 		inline size_t dim() const { return this->bias_acc_.size() + this->bias_gyro_.size(); } | ||||
| 
 | ||||
| 		/// print with optional string
 | ||||
| 		void print(const std::string& s = "") const { | ||||
| 			// explicit printing for now.
 | ||||
| 			std::cout << s + ".bias_acc [" << bias_acc_.transpose() << "]" << std::endl; | ||||
| 			std::cout << s + ".bias_gyro [" << bias_gyro_.transpose() << "]" << std::endl; | ||||
| 		} | ||||
| 
 | ||||
| 		/** equality up to tolerance */ | ||||
| 		inline bool equals(const ConstantBias& expected, double tol=1e-5) const { | ||||
| 			return gtsam::equal(bias_acc_, expected.bias_acc_, tol) && gtsam::equal(bias_gyro_, expected.bias_gyro_, tol); | ||||
| 		} | ||||
| 
 | ||||
| 		/** get bias_acc */ | ||||
| 		const Vector& bias_acc() const { return bias_acc_; } | ||||
| 
 | ||||
| 		/** get bias_gyro */ | ||||
| 		const Vector& bias_gyro() const { return bias_gyro_; } | ||||
| 
 | ||||
| 
 | ||||
| 		ConstantBias compose(const ConstantBias& b2, | ||||
| 				boost::optional<gtsam::Matrix&> H1=boost::none, | ||||
| 				boost::optional<gtsam::Matrix&> H2=boost::none) const { | ||||
| 			if(H1) *H1 = eye(dim()); | ||||
| 			if(H2) *H2 = eye(b2.dim()); | ||||
| 
 | ||||
| 			return ConstantBias(bias_acc_ + b2.bias_acc_, bias_gyro_ + b2.bias_gyro_); | ||||
| 		} | ||||
| 
 | ||||
| 		/** between operation */ | ||||
| 		ConstantBias between(const ConstantBias& b2, | ||||
| 				boost::optional<gtsam::Matrix&> H1=boost::none, | ||||
| 				boost::optional<gtsam::Matrix&> H2=boost::none) const { | ||||
| 			if(H1) *H1 = -eye(dim()); | ||||
| 			if(H2) *H2 = eye(b2.dim()); | ||||
| 			return ConstantBias(b2.bias_acc_ - bias_acc_, b2.bias_gyro_ - bias_gyro_); | ||||
| 		} | ||||
| 
 | ||||
| 		/** invert the object and yield a new one */ | ||||
| 		inline ConstantBias inverse(boost::optional<gtsam::Matrix&> H=boost::none) const { | ||||
| 			if(H) *H = -eye(dim()); | ||||
| 
 | ||||
| 			return ConstantBias(-1.0 * bias_acc_, -1.0 * bias_gyro_); | ||||
| 		} | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 	}; // ConstantBias class
 | ||||
| 
 | ||||
| 
 | ||||
| 	} // namespace ImuBias
 | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
| 
 | ||||
| 
 | ||||
|  | @ -0,0 +1,396 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  *  @file   InertialNavFactor_GlobalVelocity.h | ||||
|  *  @author Vadim Indelman, Stephen Williams | ||||
|  *  @brief  Inertial navigation factor (velocity in the global frame) | ||||
|  *  @date   Sept 13, 2012 | ||||
|  **/ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||||
| #include <gtsam/linear/NoiseModel.h> | ||||
| #include <gtsam/geometry/Rot3.h> | ||||
| #include <gtsam/base/LieVector.h> | ||||
| #include <gtsam/base/Matrix.h> | ||||
| 
 | ||||
| // Using numerical derivative to calculate d(Pose3::Expmap)/dw
 | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| 
 | ||||
| #include <boost/optional.hpp> | ||||
| 
 | ||||
| #include <ostream> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| /*
 | ||||
|  * NOTES: | ||||
|  * ===== | ||||
|  * - The global frame (NED or ENU) is defined by the user by specifying the gravity vector in this frame. | ||||
|  * - The IMU frame is implicitly defined by the user via the rotation matrix between global and imu frames. | ||||
|  * - Camera and IMU frames are identical | ||||
|  * - The user should specify a continuous equivalent noise covariance, which can be calculated using | ||||
|  *   the static function CalcEquivalentNoiseCov based on the IMU gyro and acc measurement noise covariance | ||||
|  *   matrices and the process\modeling covariance matrix. The IneritalNavFactor converts this into a | ||||
|  *   discrete form using the supplied delta_t between sub-sequential measurements. | ||||
|  * - Earth-rate correction: | ||||
|  * 		+ Currently the user should supply R_ECEF_to_G, which is the rotation from ECEF to the global | ||||
|  * 		  frame (Local-Level system: ENU or NED, see above). | ||||
|  * 		+ R_ECEF_to_G can be calculated by approximated values of latitude and longitude of the system. | ||||
|  *		+ Currently it is assumed that a relatively small distance is traveled w.r.t. to initial pose, since R_ECEF_to_G is constant. | ||||
|  *		  Otherwise, R_ECEF_to_G should be updated each time using the current lat-lon. | ||||
|  * | ||||
|  * - Frame Notation: | ||||
|  *   Quantities are written as {Frame of Representation/Destination Frame}_{Quantity Type}_{Quatity Description/Origination Frame} | ||||
|  *   So, the rotational velocity of the sensor written in the body frame is: body_omega_sensor | ||||
|  *   And the transformation from the body frame to the world frame would be: world_P_body | ||||
|  *   This allows visual chaining. For example, converting the sensed angular velocity of the IMU | ||||
|  *   (angular velocity of the sensor in the sensor frame) into the world frame can be performed as: | ||||
|  *       world_R_body * body_R_sensor * sensor_omega_sensor = world_omega_sensor | ||||
|  * | ||||
|  * | ||||
|  * - Common Quantity Types | ||||
|  *   P : pose/3d transformation | ||||
|  *   R : rotation | ||||
|  *   omega : angular velocity | ||||
|  *   t : translation | ||||
|  *   v : velocity | ||||
|  *   a : acceleration | ||||
|  * | ||||
|  * - Common Frames | ||||
|  *   sensor : the coordinate system attached to the sensor origin | ||||
|  *   body   : the coordinate system attached to body/inertial frame. | ||||
|  *            Unless an optional frame transformation is provided, the | ||||
|  *            sensor frame and the body frame will be identical | ||||
|  *   world  : the global/world coordinate frame. This is assumed to be | ||||
|  *            a tangent plane to the earth's surface somewhere near the | ||||
|  *            vehicle | ||||
|  */ | ||||
| template<class POSE, class VELOCITY, class IMUBIAS> | ||||
| class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> { | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
| 	typedef InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> This; | ||||
| 	typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base; | ||||
| 
 | ||||
| 	Vector measurement_acc_; | ||||
| 	Vector measurement_gyro_; | ||||
| 	double dt_; | ||||
| 
 | ||||
| 	Vector world_g_; | ||||
| 	Vector world_rho_; | ||||
|   Vector world_omega_earth_; | ||||
| 
 | ||||
|   boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
 | ||||
| 
 | ||||
| public: | ||||
| 
 | ||||
| 	// shorthand for a smart pointer to a factor
 | ||||
| 	typedef typename boost::shared_ptr<InertialNavFactor_GlobalVelocity> shared_ptr; | ||||
| 
 | ||||
| 	/** default constructor - only use for serialization */ | ||||
| 	InertialNavFactor_GlobalVelocity() {} | ||||
| 
 | ||||
| 	/** Constructor */ | ||||
| 	InertialNavFactor_GlobalVelocity(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2, | ||||
|       const Vector& measurement_acc, const Vector& measurement_gyro, const double measurement_dt, const Vector world_g, const Vector world_rho, | ||||
|       const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_continuous, boost::optional<POSE> body_P_sensor = boost::none) : | ||||
|         Base(calc_descrete_noise_model(model_continuous, measurement_dt ), | ||||
|             Pose1, Vel1, IMUBias1, Pose2, Vel2), measurement_acc_(measurement_acc), measurement_gyro_(measurement_gyro), | ||||
|             dt_(measurement_dt), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), body_P_sensor_(body_P_sensor) {	} | ||||
| 
 | ||||
| 	virtual ~InertialNavFactor_GlobalVelocity() {} | ||||
| 
 | ||||
| 	/** implement functions needed for Testable */ | ||||
| 
 | ||||
| 	/** print */ | ||||
| 	virtual void print(const std::string& s = "InertialNavFactor_GlobalVelocity", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { | ||||
| 		std::cout << s << "(" | ||||
| 				<< keyFormatter(this->key1()) << "," | ||||
| 				<< keyFormatter(this->key2()) << "," | ||||
| 				<< keyFormatter(this->key3()) << "," | ||||
| 				<< keyFormatter(this->key4()) << "," | ||||
| 				<< keyFormatter(this->key5()) << "\n"; | ||||
| 		std::cout << "acc measurement: " << this->measurement_acc_.transpose() << std::endl; | ||||
| 		std::cout << "gyro measurement: " << this->measurement_gyro_.transpose() << std::endl; | ||||
| 		std::cout << "dt: " << this->dt_ << std::endl; | ||||
| 		std::cout << "gravity (in world frame): " << this->world_g_.transpose() << std::endl; | ||||
| 		std::cout << "craft rate (in world frame): " << this->world_rho_.transpose() << std::endl; | ||||
| 		std::cout << "earth's rotation (in world frame): " << this->world_omega_earth_.transpose() << std::endl; | ||||
| 		if(this->body_P_sensor_) | ||||
| 		  this->body_P_sensor_->print("  sensor pose in body frame: "); | ||||
|     this->noiseModel_->print("  noise model"); | ||||
| 	} | ||||
| 
 | ||||
| 	/** equals */ | ||||
| 	virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { | ||||
| 		const This *e =	dynamic_cast<const This*> (&expected); | ||||
| 		return e != NULL && Base::equals(*e, tol) | ||||
| 		  && (measurement_acc_ - e->measurement_acc_).norm() < tol | ||||
| 		  && (measurement_gyro_ - e->measurement_gyro_).norm() < tol | ||||
| 		  && (dt_ - e->dt_) < tol | ||||
| 		  && (world_g_ - e->world_g_).norm() < tol | ||||
| 		  && (world_rho_ - e->world_rho_).norm() < tol | ||||
| 		  && (world_omega_earth_ - e->world_omega_earth_).norm() < tol | ||||
| 		  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); | ||||
| 	} | ||||
| 
 | ||||
|   POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { | ||||
|     // Calculate the corrected measurements using the Bias object
 | ||||
|     Vector GyroCorrected(Bias1.CorrectGyro(measurement_gyro_)); | ||||
| 
 | ||||
|     const POSE& world_P1_body = Pose1; | ||||
|     const VELOCITY& world_V1_body = Vel1; | ||||
| 
 | ||||
|     // Calculate the acceleration and angular velocity of the body in the body frame (including earth-related rotations)
 | ||||
|     Vector body_omega_body; | ||||
|     if(body_P_sensor_) { | ||||
|       body_omega_body = body_P_sensor_->rotation().matrix() * GyroCorrected; | ||||
|     } else { | ||||
|       body_omega_body = GyroCorrected; | ||||
|     } | ||||
| 
 | ||||
|     // 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_; | ||||
| 
 | ||||
|     // Correct for earth-related terms
 | ||||
|     body_omega_body -= body_rho + body_omega_earth; | ||||
| 
 | ||||
|     // The velocity is in the global frame, so composing Pose1 with v*dt is incorrect
 | ||||
|     return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_omega_body*dt_), Pose1.translation() + typename POSE::Translation(world_V1_body*dt_)); | ||||
|   } | ||||
| 
 | ||||
|   VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { | ||||
|     // Calculate the corrected measurements using the Bias object
 | ||||
|     Vector AccCorrected(Bias1.CorrectAcc(measurement_acc_)); | ||||
| 
 | ||||
|     const POSE& world_P1_body = Pose1; | ||||
|     const VELOCITY& world_V1_body = Vel1; | ||||
| 
 | ||||
|     // Calculate the acceleration and angular velocity of the body in the body frame (including earth-related rotations)
 | ||||
|     Vector body_a_body, body_omega_body; | ||||
|     if(body_P_sensor_) { | ||||
|       Matrix body_R_sensor = body_P_sensor_->rotation().matrix(); | ||||
| 
 | ||||
|       Vector GyroCorrected(Bias1.CorrectGyro(measurement_gyro_)); | ||||
|       body_omega_body = body_R_sensor * GyroCorrected; | ||||
|       Matrix body_omega_body__cross = skewSymmetric(body_omega_body); | ||||
|       body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation().vector(); | ||||
|     } else { | ||||
|       body_a_body = AccCorrected; | ||||
|     } | ||||
| 
 | ||||
|     // Correct for earth-related terms
 | ||||
|     Vector world_a_body = world_P1_body.rotation().matrix() * body_a_body + world_g_ - 2*skewSymmetric(world_rho_ + world_omega_earth_)*world_V1_body; | ||||
| 
 | ||||
|     // Calculate delta in the body frame
 | ||||
|     VELOCITY VelDelta(world_a_body*dt_); | ||||
| 
 | ||||
|     // Predict
 | ||||
|     return Vel1.compose(VelDelta); | ||||
|   } | ||||
| 
 | ||||
|   void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const { | ||||
|     Pose2 = predictPose(Pose1, Vel1, Bias1); | ||||
|     Vel2 = predictVelocity(Pose1, Vel1, Bias1); | ||||
|   } | ||||
| 
 | ||||
|   POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const { | ||||
|     // Predict
 | ||||
|     POSE Pose2Pred = predictPose(Pose1, Vel1, Bias1); | ||||
| 
 | ||||
|     // Calculate error
 | ||||
|     return Pose2.between(Pose2Pred); | ||||
|   } | ||||
| 
 | ||||
|   VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2) const { | ||||
|     // Predict
 | ||||
|     VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1); | ||||
| 
 | ||||
|     // Calculate error
 | ||||
|     return Vel2.between(Vel2Pred); | ||||
|   } | ||||
| 
 | ||||
|   /** implement functions needed to derive from Factor */ | ||||
| 	Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, | ||||
| 			boost::optional<Matrix&> H1 = boost::none, | ||||
| 			boost::optional<Matrix&> H2 = boost::none, | ||||
| 			boost::optional<Matrix&> H3 = boost::none, | ||||
| 			boost::optional<Matrix&> H4 = boost::none, | ||||
| 			boost::optional<Matrix&> H5 = boost::none) const { | ||||
| 
 | ||||
|     // TODO: Write analytical derivative calculations
 | ||||
|     // Jacobian w.r.t. Pose1
 | ||||
|     if (H1){ | ||||
|       Matrix H1_Pose = gtsam::numericalDerivative11<POSE, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); | ||||
|       Matrix H1_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); | ||||
|       *H1 = stack(2, &H1_Pose, &H1_Vel); | ||||
|     } | ||||
| 
 | ||||
|     // Jacobian w.r.t. Vel1
 | ||||
|     if (H2){ | ||||
|       Matrix H2_Pose = gtsam::numericalDerivative11<POSE, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); | ||||
|       Matrix H2_Vel = gtsam::numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); | ||||
|       *H2 = stack(2, &H2_Pose, &H2_Vel); | ||||
|     } | ||||
| 
 | ||||
|     // Jacobian w.r.t. IMUBias1
 | ||||
|     if (H3){ | ||||
|       Matrix H3_Pose = gtsam::numericalDerivative11<POSE, IMUBIAS>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); | ||||
|       Matrix H3_Vel = gtsam::numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); | ||||
|       *H3 = stack(2, &H3_Pose, &H3_Vel); | ||||
|     } | ||||
| 
 | ||||
|     // Jacobian w.r.t. Pose2
 | ||||
|     if (H4){ | ||||
|       Matrix H4_Pose = gtsam::numericalDerivative11<POSE, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); | ||||
|       Matrix H4_Vel = gtsam::numericalDerivative11<VELOCITY, POSE>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); | ||||
|       *H4 = stack(2, &H4_Pose, &H4_Vel); | ||||
|     } | ||||
| 
 | ||||
|     // Jacobian w.r.t. Vel2
 | ||||
|     if (H5){ | ||||
|       Matrix H5_Pose = gtsam::numericalDerivative11<POSE, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); | ||||
|       Matrix H5_Vel = gtsam::numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); | ||||
|       *H5 = stack(2, &H5_Pose, &H5_Vel); | ||||
|     } | ||||
| 
 | ||||
| 		Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); | ||||
| 		Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); | ||||
| 
 | ||||
| 		return concatVectors(2, &ErrPoseVector, &ErrVelVector); | ||||
| 	} | ||||
| 
 | ||||
| 	static inline noiseModel::Gaussian::shared_ptr CalcEquivalentNoiseCov(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro, | ||||
| 			const noiseModel::Gaussian::shared_ptr& gaussian_process){ | ||||
| 
 | ||||
| 		Matrix cov_acc = inverse( gaussian_acc->R().transpose() * gaussian_acc->R() ); | ||||
| 		Matrix cov_gyro = inverse( gaussian_gyro->R().transpose() * gaussian_gyro->R() ); | ||||
| 		Matrix cov_process = inverse( gaussian_process->R().transpose() * gaussian_process->R() ); | ||||
| 
 | ||||
| 		cov_process.block(0,0, 3,3) += cov_gyro; | ||||
| 		cov_process.block(6,6, 3,3) += cov_acc; | ||||
| 
 | ||||
| 		return noiseModel::Gaussian::Covariance(cov_process); | ||||
| 	} | ||||
| 
 | ||||
|   static inline void Calc_g_rho_omega_earth_NED(const Vector& Pos_NED, const Vector& Vel_NED, const Vector& LatLonHeight_IC, const Vector& Pos_NED_Initial, | ||||
|       Vector& g_NED, Vector& rho_NED, Vector& omega_earth_NED) { | ||||
| 
 | ||||
|     Matrix ENU_to_NED = Matrix_(3, 3, | ||||
|         0.0,  1.0,  0.0, | ||||
|         1.0,  0.0,  0.0, | ||||
|         0.0,  0.0, -1.0); | ||||
| 
 | ||||
|     Matrix NED_to_ENU = Matrix_(3, 3, | ||||
|         0.0,  1.0,  0.0, | ||||
|         1.0,  0.0,  0.0, | ||||
|         0.0,  0.0, -1.0); | ||||
| 
 | ||||
|     // Convert incoming parameters to ENU
 | ||||
|     Vector Pos_ENU = NED_to_ENU * Pos_NED; | ||||
|     Vector Vel_ENU = NED_to_ENU * Vel_NED; | ||||
|     Vector Pos_ENU_Initial = NED_to_ENU * Pos_NED_Initial; | ||||
| 
 | ||||
|     // Call ENU version
 | ||||
|     Vector g_ENU; | ||||
|     Vector rho_ENU; | ||||
|     Vector omega_earth_ENU; | ||||
|     Calc_g_rho_omega_earth_ENU(Pos_ENU, Vel_ENU, LatLonHeight_IC, Pos_ENU_Initial, g_ENU, rho_ENU, omega_earth_ENU); | ||||
| 
 | ||||
|     // Convert output to NED
 | ||||
|     g_NED = ENU_to_NED * g_ENU; | ||||
|     rho_NED = ENU_to_NED * rho_ENU; | ||||
|     omega_earth_NED = ENU_to_NED * omega_earth_ENU; | ||||
|   } | ||||
| 
 | ||||
| 	static inline void Calc_g_rho_omega_earth_ENU(const Vector& Pos_ENU, const Vector& Vel_ENU, const Vector& LatLonHeight_IC, const Vector& Pos_ENU_Initial, | ||||
| 			Vector& g_ENU, Vector& rho_ENU, Vector& omega_earth_ENU){ | ||||
| 		double R0 = 6.378388e6; | ||||
| 		double e = 1/297; | ||||
| 		double Re( R0*( 1-e*(sin( LatLonHeight_IC(0) ))*(sin( LatLonHeight_IC(0) )) ) ); | ||||
| 
 | ||||
| 		// Calculate current lat, lon
 | ||||
| 		Vector delta_Pos_ENU(Pos_ENU - Pos_ENU_Initial); | ||||
| 		double delta_lat(delta_Pos_ENU(1)/Re); | ||||
| 		double delta_lon(delta_Pos_ENU(0)/(Re*cos(LatLonHeight_IC(0)))); | ||||
| 		double lat_new(LatLonHeight_IC(0) + delta_lat); | ||||
| 		double lon_new(LatLonHeight_IC(1) + delta_lon); | ||||
| 
 | ||||
| 		// Rotation of lon about z axis
 | ||||
| 		Rot3 C1(cos(lon_new), sin(lon_new), 0.0, | ||||
| 				-sin(lon_new), cos(lon_new), 0.0, | ||||
| 				0.0, 0.0, 1.0); | ||||
| 
 | ||||
| 		// Rotation of lat about y axis
 | ||||
| 		Rot3 C2(cos(lat_new), 0.0, sin(lat_new), | ||||
| 				0.0, 1.0, 0.0, | ||||
| 				-sin(lat_new), 0.0, cos(lat_new)); | ||||
| 
 | ||||
| 		Rot3 UEN_to_ENU(0, 1, 0, | ||||
| 				0, 0, 1, | ||||
| 				1, 0, 0); | ||||
| 
 | ||||
| 		Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 ); | ||||
| 
 | ||||
| 		Vector omega_earth_ECEF(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
| 		omega_earth_ENU = R_ECEF_to_ENU.matrix() * omega_earth_ECEF; | ||||
| 
 | ||||
| 		// Calculating g
 | ||||
| 		double height(LatLonHeight_IC(2)); | ||||
| 		double EQUA_RADIUS = 6378137.0;    		// equatorial radius of the earth; WGS-84
 | ||||
| 		double ECCENTRICITY = 0.0818191908426;  // eccentricity of the earth ellipsoid
 | ||||
| 		double e2( pow(ECCENTRICITY,2) ); | ||||
| 		double den( 1-e2*pow(sin(lat_new),2) ); | ||||
| 		double Rm( (EQUA_RADIUS*(1-e2))/( pow(den,(3/2)) ) ); | ||||
| 		double Rp( EQUA_RADIUS/( sqrt(den) ) ); | ||||
| 		double Ro( sqrt(Rp*Rm) );   				// mean earth radius of curvature
 | ||||
| 		double g0( 9.780318*( 1 + 5.3024e-3 * pow(sin(lat_new),2) - 5.9e-6 * pow(sin(2*lat_new),2) ) ); | ||||
| 		double g_calc( g0/( pow(1 + height/Ro, 2) ) ); | ||||
| 		g_ENU = Vector_(3, 0.0, 0.0, -g_calc); | ||||
| 
 | ||||
| 
 | ||||
| 		// Calculate rho
 | ||||
| 		double Ve( Vel_ENU(0) ); | ||||
| 		double Vn( Vel_ENU(1) ); | ||||
| 		double rho_E = -Vn/(Rm + height); | ||||
| 		double rho_N = Ve/(Rp + height); | ||||
| 		double rho_U = Ve*tan(lat_new)/(Rp + height); | ||||
| 		rho_ENU = Vector_(3, rho_E, rho_N, rho_U); | ||||
| 	} | ||||
| 
 | ||||
| 	static inline noiseModel::Gaussian::shared_ptr calc_descrete_noise_model(const noiseModel::Gaussian::shared_ptr& model, double delta_t){ | ||||
| 	    /* Q_d (approx)= Q * delta_t */ | ||||
| 	    /* In practice, square root of the information matrix is represented, so that:
 | ||||
| 	     *  R_d (approx)= R / sqrt(delta_t) | ||||
| 	     * */ | ||||
| 	    return noiseModel::Gaussian::SqrtInformation(model->R()/sqrt(delta_t)); | ||||
| 	} | ||||
| 
 | ||||
| private: | ||||
| 
 | ||||
| 	/** Serialization function */ | ||||
| 	friend class boost::serialization::access; | ||||
| 	template<class ARCHIVE> | ||||
| 	void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
| 		ar & boost::serialization::make_nvp("NonlinearFactor2", | ||||
| 				boost::serialization::base_object<Base>(*this)); | ||||
| 	} | ||||
| 
 | ||||
| }; // \class GaussMarkov1stOrderFactor
 | ||||
| 
 | ||||
| 
 | ||||
| } /// namespace aspn
 | ||||
|  | @ -0,0 +1,682 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file    testInertialNavFactor_GlobalVelocity.cpp | ||||
|  * @brief   Unit test for the InertialNavFactor_GlobalVelocity | ||||
|  * @author  Vadim Indelman, Stephen Williams | ||||
|  */ | ||||
| 
 | ||||
| #include <iostream> | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam_unstable/dynamics/ImuBias.h> | ||||
| #include <gtsam_unstable/dynamics/InertialNavFactor_GlobalVelocity.h> | ||||
| #include <gtsam/geometry/Pose3.h> | ||||
| #include <gtsam/nonlinear/Values.h> | ||||
| #include <gtsam/nonlinear/Key.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| #include <gtsam/base/LieVector.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| gtsam::Rot3 world_R_ECEF( | ||||
| 		0.31686,      0.51505,      0.79645, | ||||
| 		0.85173,     -0.52399,            0, | ||||
| 		0.41733,      0.67835,     -0.60471); | ||||
| 
 | ||||
| gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
| gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| gtsam::Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias>& factor) { | ||||
|   return Pose3::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).head(6)); | ||||
| } | ||||
| 
 | ||||
| gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias>& factor) { | ||||
|   return LieVector::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).tail(3)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( InertialNavFactor_GlobalVelocity, Constructor) | ||||
| { | ||||
| 	gtsam::Key Pose1(11); | ||||
| 	gtsam::Key Pose2(12); | ||||
| 	gtsam::Key Vel1(21); | ||||
| 	gtsam::Key Vel2(22); | ||||
| 	gtsam::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)); | ||||
| 
 | ||||
| 	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
 | ||||
| 	gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
| 	gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); | ||||
| 
 | ||||
| 	SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); | ||||
| 
 | ||||
| 	InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( InertialNavFactor_GlobalVelocity, Equals) | ||||
| { | ||||
|   gtsam::Key Pose1(11); | ||||
|   gtsam::Key Pose2(12); | ||||
|   gtsam::Key Vel1(21); | ||||
|   gtsam::Key Vel2(22); | ||||
|   gtsam::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)); | ||||
| 
 | ||||
| 	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
 | ||||
|   gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
|   gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); | ||||
| 
 | ||||
| 	SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); | ||||
| 
 | ||||
| 	InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); | ||||
| 	InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> 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) | ||||
| { | ||||
|   gtsam::Key PoseKey1(11); | ||||
|   gtsam::Key PoseKey2(12); | ||||
|   gtsam::Key VelKey1(21); | ||||
|   gtsam::Key VelKey2(22); | ||||
|   gtsam::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
 | ||||
|   gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
|   gtsam::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_gyro(Vector_(3, 0.0, 0.0, 0.0)); | ||||
| 
 | ||||
|   InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> 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(3, 0.50, -0.50, 0.40); | ||||
|   imuBias::ConstantBias Bias1; | ||||
|   Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); | ||||
|   LieVector expectedVel2(3, 0.51, -0.48, 0.43); | ||||
|   Pose3 actualPose2; | ||||
|   LieVector actualVel2; | ||||
|   f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); | ||||
| 
 | ||||
|   CHECK(assert_equal(expectedPose2, actualPose2, 1e-5)); | ||||
|   CHECK(assert_equal(expectedVel2, actualVel2, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) | ||||
| { | ||||
|   gtsam::Key PoseKey1(11); | ||||
|   gtsam::Key PoseKey2(12); | ||||
|   gtsam::Key VelKey1(21); | ||||
|   gtsam::Key VelKey2(22); | ||||
|   gtsam::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
 | ||||
|   gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
|   gtsam::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_gyro(Vector_(3, 0.0, 0.0, 0.0)); | ||||
| 
 | ||||
| 	InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> 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(3, 0.50, -0.50, 0.40); | ||||
| 	LieVector Vel2(3, 0.51, -0.48, 0.43); | ||||
| 	imuBias::ConstantBias Bias1; | ||||
| 
 | ||||
| 	Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); | ||||
| 	Vector ExpectedErr(zero(9)); | ||||
| 
 | ||||
| 	CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( InertialNavFactor_GlobalVelocity, ErrorRot) | ||||
| { | ||||
|   gtsam::Key PoseKey1(11); | ||||
|   gtsam::Key PoseKey2(12); | ||||
|   gtsam::Key VelKey1(21); | ||||
|   gtsam::Key VelKey2(22); | ||||
|   gtsam::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
 | ||||
|   gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
|   gtsam::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_gyro(Vector_(3, 0.1, 0.2, 0.3)); | ||||
| 
 | ||||
| 	InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> 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(3,0.0,0.0,0.0); | ||||
| 	LieVector Vel2(3,0.0,0.0,0.0); | ||||
| 	imuBias::ConstantBias Bias1; | ||||
| 
 | ||||
| 	Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); | ||||
| 	Vector ExpectedErr(zero(9)); | ||||
| 
 | ||||
| 	CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) | ||||
| { | ||||
|   gtsam::Key PoseKey1(11); | ||||
|   gtsam::Key PoseKey2(12); | ||||
|   gtsam::Key VelKey1(21); | ||||
|   gtsam::Key VelKey2(22); | ||||
|   gtsam::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
 | ||||
|   gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
|   gtsam::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_gyro(Vector_(3, 0.1, 0.2, 0.3)); | ||||
| 
 | ||||
| 	InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> 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); | ||||
| 	Pose3 Pose1(R1, t1); | ||||
| 	LieVector Vel1(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 ); | ||||
| 	imuBias::ConstantBias Bias1; | ||||
| 
 | ||||
| 	Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); | ||||
| 	Vector ExpectedErr(zero(9)); | ||||
| 
 | ||||
| 	// TODO: Expected values need to be updated for global velocity version
 | ||||
| 	CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| ///* VADIM - START ************************************************************************* */
 | ||||
| //LieVector predictionRq(const LieVector angles, const LieVector q) {
 | ||||
| //	return (Rot3().RzRyRx(angles) * q).vector();
 | ||||
| //}
 | ||||
| //
 | ||||
| //TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) {
 | ||||
| //	LieVector angles(Vector_(3, 3.001, -1.0004, 2.0005));
 | ||||
| //	Rot3 R1(Rot3().RzRyRx(angles));
 | ||||
| //	LieVector 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);
 | ||||
| //	Matrix J_hyp( -(R1*qx).matrix() );
 | ||||
| //
 | ||||
| //	gtsam::Matrix J_expected;
 | ||||
| //
 | ||||
| //	LieVector v(predictionRq(angles, q));
 | ||||
| //
 | ||||
| //	J_expected = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionRq, _1, q), angles);
 | ||||
| //
 | ||||
| //	cout<<"J_hyp"<<J_hyp<<endl;
 | ||||
| //	cout<<"J_expected"<<J_expected<<endl;
 | ||||
| //
 | ||||
| //	CHECK( gtsam::assert_equal(J_expected, J_hyp, 1e-6));
 | ||||
| //}
 | ||||
| ///* VADIM - END ************************************************************************* */
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST (InertialNavFactor_GlobalVelocity, Jacobian ) { | ||||
| 
 | ||||
|   gtsam::Key PoseKey1(11); | ||||
|   gtsam::Key PoseKey2(12); | ||||
|   gtsam::Key VelKey1(21); | ||||
|   gtsam::Key VelKey2(22); | ||||
|   gtsam::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
 | ||||
|   gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
|   gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); | ||||
| 
 | ||||
| 	SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); | ||||
| 
 | ||||
| 	Vector measurement_acc(Vector_(3, 6.501390843381716,  -6.763926150509185,  -2.300389940090343)); | ||||
| 	Vector measurement_gyro(Vector_(3, 3.14, 3.14/2, -3.14)); | ||||
| 
 | ||||
| 	InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> 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); | ||||
| 	Pose3 Pose1(R1, t1); | ||||
| 	LieVector Vel1(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(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)); | ||||
| 
 | ||||
| 	// 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())); | ||||
| 
 | ||||
| 	// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
 | ||||
| 	gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; | ||||
| 	H1_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); | ||||
| 	H2_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); | ||||
| 	H3_expectedPose = gtsam::numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); | ||||
| 	H4_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); | ||||
| 	H5_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); | ||||
| 
 | ||||
| 	// Verify they are equal for this choice of state
 | ||||
| 	CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-6)); | ||||
| 	CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-6)); | ||||
| 	CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 1e-6)); | ||||
| 	CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-6)); | ||||
| 	CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-6)); | ||||
| 
 | ||||
| 	// 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())); | ||||
| 
 | ||||
| 	// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
 | ||||
| 	gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; | ||||
| 	H1_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); | ||||
| 	H2_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); | ||||
| 	H3_expectedVel = gtsam::numericalDerivative11<LieVector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); | ||||
| 	H4_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); | ||||
| 	H5_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); | ||||
| 
 | ||||
| 	// Verify they are equal for this choice of state
 | ||||
| 	CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-6)); | ||||
| 	CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-6)); | ||||
| 	CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-6)); | ||||
| 	CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-6)); | ||||
| 	CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-6)); | ||||
| } | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) | ||||
| { | ||||
|   gtsam::Key Pose1(11); | ||||
|   gtsam::Key Pose2(12); | ||||
|   gtsam::Key Vel1(21); | ||||
|   gtsam::Key Vel2(22); | ||||
|   gtsam::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)); | ||||
| 
 | ||||
|   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
 | ||||
|   gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
|   gtsam::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
 | ||||
| 
 | ||||
| 
 | ||||
|   InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> 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) | ||||
| { | ||||
|   gtsam::Key Pose1(11); | ||||
|   gtsam::Key Pose2(12); | ||||
|   gtsam::Key Vel1(21); | ||||
|   gtsam::Key Vel2(22); | ||||
|   gtsam::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)); | ||||
| 
 | ||||
|   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
 | ||||
|   gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
|   gtsam::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
 | ||||
| 
 | ||||
| 
 | ||||
|   InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> 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<Pose3, LieVector, imuBias::ConstantBias> 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) | ||||
| { | ||||
|   gtsam::Key PoseKey1(11); | ||||
|   gtsam::Key PoseKey2(12); | ||||
|   gtsam::Key VelKey1(21); | ||||
|   gtsam::Key VelKey2(22); | ||||
|   gtsam::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
 | ||||
|   gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
|   gtsam::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
 | ||||
| 
 | ||||
| 
 | ||||
|   // First test: zero angular motion, some acceleration
 | ||||
|   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
 | ||||
| 
 | ||||
|   InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> 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(3, 0.50, -0.50, 0.40); | ||||
|   imuBias::ConstantBias Bias1; | ||||
|   Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); | ||||
|   LieVector expectedVel2(3, 0.51, -0.48, 0.43); | ||||
|   Pose3 actualPose2; | ||||
|   LieVector actualVel2; | ||||
|   f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); | ||||
| 
 | ||||
|   CHECK(assert_equal(expectedPose2, actualPose2, 1e-5)); | ||||
|   CHECK(assert_equal(expectedVel2, actualVel2, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) | ||||
| { | ||||
|   gtsam::Key PoseKey1(11); | ||||
|   gtsam::Key PoseKey2(12); | ||||
|   gtsam::Key VelKey1(21); | ||||
|   gtsam::Key VelKey2(22); | ||||
|   gtsam::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
 | ||||
|   gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
|   gtsam::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
 | ||||
| 
 | ||||
| 
 | ||||
|   // First test: zero angular motion, some acceleration
 | ||||
|   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
 | ||||
| 
 | ||||
|   InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> 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(3, 0.50, -0.50, 0.40); | ||||
|   LieVector Vel2(3, 0.51, -0.48, 0.43); | ||||
|   imuBias::ConstantBias Bias1; | ||||
| 
 | ||||
|   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); | ||||
|   Vector ExpectedErr(zero(9)); | ||||
| 
 | ||||
|   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) | ||||
| { | ||||
|   gtsam::Key PoseKey1(11); | ||||
|   gtsam::Key PoseKey2(12); | ||||
|   gtsam::Key VelKey1(21); | ||||
|   gtsam::Key VelKey2(22); | ||||
|   gtsam::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
 | ||||
|   gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
|   gtsam::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
 | ||||
| 
 | ||||
| 
 | ||||
|   // Second test: zero angular motion, some acceleration
 | ||||
|   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
 | ||||
| 
 | ||||
|   InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> 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(3,0.0,0.0,0.0); | ||||
|   LieVector Vel2(3,0.0,0.0,0.0); | ||||
|   imuBias::ConstantBias Bias1; | ||||
| 
 | ||||
|   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); | ||||
|   Vector ExpectedErr(zero(9)); | ||||
| 
 | ||||
|   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) | ||||
| { | ||||
|   gtsam::Key PoseKey1(11); | ||||
|   gtsam::Key PoseKey2(12); | ||||
|   gtsam::Key VelKey1(21); | ||||
|   gtsam::Key VelKey2(22); | ||||
|   gtsam::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
 | ||||
|   gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
|   gtsam::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
 | ||||
| 
 | ||||
| 
 | ||||
|   // 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
 | ||||
| 
 | ||||
|   InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> 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); | ||||
|   Pose3 Pose1(R1, t1); | ||||
|   LieVector Vel1(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 ); | ||||
|   imuBias::ConstantBias Bias1; | ||||
| 
 | ||||
|   Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); | ||||
|   Vector ExpectedErr(zero(9)); | ||||
| 
 | ||||
|   // TODO: Expected values need to be updated for global velocity version
 | ||||
|   CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { | ||||
| 
 | ||||
|   gtsam::Key PoseKey1(11); | ||||
|   gtsam::Key PoseKey2(12); | ||||
|   gtsam::Key VelKey1(21); | ||||
|   gtsam::Key VelKey2(22); | ||||
|   gtsam::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
 | ||||
|   gtsam::Vector ECEF_omega_earth(Vector_(3, 0.0, 0.0, 7.292115e-5)); | ||||
|   gtsam::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
 | ||||
| 
 | ||||
| 
 | ||||
|   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
 | ||||
| 
 | ||||
| 
 | ||||
|   InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> 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); | ||||
|   Pose3 Pose1(R1, t1); | ||||
|   LieVector Vel1(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(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)); | ||||
| 
 | ||||
|   // 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())); | ||||
| 
 | ||||
|   // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
 | ||||
|   gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; | ||||
|   H1_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); | ||||
|   H2_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); | ||||
|   H3_expectedPose = gtsam::numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); | ||||
|   H4_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); | ||||
|   H5_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); | ||||
| 
 | ||||
|   // Verify they are equal for this choice of state
 | ||||
|   CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-6)); | ||||
|   CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-6)); | ||||
|   CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 1e-6)); | ||||
|   CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-6)); | ||||
|   CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-6)); | ||||
| 
 | ||||
|   // 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())); | ||||
| 
 | ||||
|   // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
 | ||||
|   gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; | ||||
|   H1_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); | ||||
|   H2_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); | ||||
|   H3_expectedVel = gtsam::numericalDerivative11<LieVector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); | ||||
|   H4_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); | ||||
|   H5_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); | ||||
| 
 | ||||
|   // Verify they are equal for this choice of state
 | ||||
|   CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-6)); | ||||
|   CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-6)); | ||||
|   CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-6)); | ||||
|   CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-6)); | ||||
|   CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-6)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 	int main() { TestResult tr; return TestRegistry::runAllTests(tr);} | ||||
| /* ************************************************************************* */ | ||||
		Loading…
	
		Reference in New Issue