From 4f5b9f2074f3cac69efcd87d53d199fcc29654ff Mon Sep 17 00:00:00 2001 From: Vadim Indelman Date: Thu, 24 Jan 2013 21:01:10 +0000 Subject: [PATCH] added imu factor (global velocity) --- gtsam_unstable/dynamics/ImuBias.h | 184 +++++ .../InertialNavFactor_GlobalVelocity.h | 396 ++++++++++ .../testInertialNavFactor_GlobalVelocity.cpp | 682 ++++++++++++++++++ 3 files changed, 1262 insertions(+) create mode 100644 gtsam_unstable/dynamics/ImuBias.h create mode 100644 gtsam_unstable/dynamics/InertialNavFactor_GlobalVelocity.h create mode 100644 gtsam_unstable/dynamics/tests/testInertialNavFactor_GlobalVelocity.cpp diff --git a/gtsam_unstable/dynamics/ImuBias.h b/gtsam_unstable/dynamics/ImuBias.h new file mode 100644 index 000000000..2269cd8a4 --- /dev/null +++ b/gtsam_unstable/dynamics/ImuBias.h @@ -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 +#include +#include +#include +#include + +/* + * 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 { + 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 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 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 H1=boost::none, boost::optional 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 H1=boost::none, + boost::optional 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 H1=boost::none, + boost::optional 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 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 + + diff --git a/gtsam_unstable/dynamics/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/dynamics/InertialNavFactor_GlobalVelocity.h new file mode 100644 index 000000000..6d16b2358 --- /dev/null +++ b/gtsam_unstable/dynamics/InertialNavFactor_GlobalVelocity.h @@ -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 +#include +#include +#include +#include + +// Using numerical derivative to calculate d(Pose3::Expmap)/dw +#include + +#include + +#include + +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 InertialNavFactor_GlobalVelocity : public NoiseModelFactor5 { + +private: + + typedef InertialNavFactor_GlobalVelocity This; + typedef NoiseModelFactor5 Base; + + Vector measurement_acc_; + Vector measurement_gyro_; + double dt_; + + Vector world_g_; + Vector world_rho_; + Vector world_omega_earth_; + + boost::optional 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 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 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 (&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 H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none, + boost::optional H5 = boost::none) const { + + // TODO: Write analytical derivative calculations + // Jacobian w.r.t. Pose1 + if (H1){ + Matrix H1_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1); + Matrix H1_Vel = gtsam::numericalDerivative11(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(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = gtsam::numericalDerivative11(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(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1); + Matrix H3_Vel = gtsam::numericalDerivative11(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(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2); + Matrix H4_Vel = gtsam::numericalDerivative11(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(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Vel = gtsam::numericalDerivative11(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 + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("NonlinearFactor2", + boost::serialization::base_object(*this)); + } + +}; // \class GaussMarkov1stOrderFactor + + +} /// namespace aspn diff --git a/gtsam_unstable/dynamics/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/dynamics/tests/testInertialNavFactor_GlobalVelocity.cpp new file mode 100644 index 000000000..e3838c570 --- /dev/null +++ b/gtsam_unstable/dynamics/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include + +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& 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& 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 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 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) +{ + 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 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 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 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 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(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); + + 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(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedPose = gtsam::numericalDerivative11(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(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedVel = gtsam::numericalDerivative11(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 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 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) +{ + 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 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 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 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 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 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(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedPose = gtsam::numericalDerivative11(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(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedVel = gtsam::numericalDerivative11(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);} +/* ************************************************************************* */