12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

tabs converted to two spaces

release/4.3a0
Chris Beall 2013-10-10 17:52:57 +00:00
parent fdced7dbcf
commit e799c9ffa9
16 changed files with 1080 additions and 1080 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam.h
View File

@ -907,7 +907,7 @@ class SymbolicBayesTree {
//Constructors
SymbolicBayesTree();
SymbolicBayesTree(const gtsam::SymbolicBayesTree& other);
SymbolicBayesTree(const gtsam::SymbolicBayesTree& other);
// Testable
void print(string s);
@ -920,10 +920,10 @@ class SymbolicBayesTree {
void clear();
void deleteCachedShortcuts();
size_t numCachedSeparatorMarginals() const;
gtsam::SymbolicConditional* marginalFactor(size_t key) const;
gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const;
gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
gtsam::SymbolicConditional* marginalFactor(size_t key) const;
gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const;
gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
};
// class SymbolicBayesTreeClique {

2
gtsam/base/LieMatrix.h
View File

@ -154,7 +154,7 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
/** Logmap around identity - just returns with default cast back */
static inline Vector Logmap(const LieMatrix& p) {
return Eigen::Map<const Vector>(&p(0,0), p.dim()); }
/// @}
private:

26
gtsam/navigation/ImuBias.h
View File

@ -26,12 +26,12 @@
/*
* 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 defined 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 the user should supply R_ECEF_to_G, which is the rotation from ECEF to Local-Level system (NED or ENU as defined 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.
* - Currently, an empty constructed is not enabled so that the user is forced to specify R_ECEF_to_G.
*/
namespace gtsam {
@ -40,11 +40,11 @@ namespace gtsam {
namespace imuBias {
class ConstantBias : public DerivedValue<ConstantBias> {
private:
private:
Vector3 biasAcc_;
Vector3 biasGyro_;
public:
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 6;
@ -144,17 +144,17 @@ namespace imuBias {
/// return dimensionality of tangent space
inline size_t dim() const { return dimension; }
/** Update the LieVector with a tangent space update */
inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); }
/** Update the LieVector with a tangent space update */
inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); }
/** @return the local coordinates of another object */
inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); }
/** @return the local coordinates of another object */
inline Vector localCoordinates(const ConstantBias& b) const { return b.vector() - vector(); }
/// @}
/// @name Group
/// @{
/** identity for group operation */
/** identity for group operation */
static ConstantBias identity() { return ConstantBias(); }
/** invert the object and yield a new one */
@ -213,7 +213,7 @@ namespace imuBias {
/// @}
}; // ConstantBias class
}; // ConstantBias class
} // namespace ImuBias

26
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -136,12 +136,12 @@ TEST( CombinedImuFactor, PreintegratedMeasurements )
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero());
Matrix3::Zero(), Matrix3::Zero());
expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias,
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6));
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6));
// const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
// const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
@ -193,13 +193,13 @@ TEST( CombinedImuFactor, ErrorWithBiases )
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 );
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6 );
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -224,14 +224,14 @@ TEST( CombinedImuFactor, ErrorWithBiases )
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a, H6a;
(void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a);
Matrix H1a, H2a, H3a, H4a, H5a, H6a;
(void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a, H3a, H4a, H5a, H6a);
EXPECT(assert_equal(H1e, H1a.topRows(9)));
EXPECT(assert_equal(H2e, H2a.topRows(9)));
EXPECT(assert_equal(H3e, H3a.topRows(9)));
EXPECT(assert_equal(H4e, H4a.topRows(9)));
EXPECT(assert_equal(H5e, H5a.topRows(9)));
EXPECT(assert_equal(H1e, H1a.topRows(9)));
EXPECT(assert_equal(H2e, H2a.topRows(9)));
EXPECT(assert_equal(H3e, H3a.topRows(9)));
EXPECT(assert_equal(H4e, H4a.topRows(9)));
EXPECT(assert_equal(H5e, H5a.topRows(9)));
}
/* ************************************************************************* */

2
gtsam/navigation/tests/testImuBias.cpp
View File

@ -39,5 +39,5 @@ TEST( ImuBias, Constructor)
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

4
gtsam/slam/tests/testBetweenFactor.cpp
View File

@ -1,8 +1,8 @@
/**
* @file testBetweenFactor.cpp
* @file testBetweenFactor.cpp
* @brief
* @author Duy-Nguyen Ta
* @date Aug 2, 2013
* @date Aug 2, 2013
*/
#include <gtsam/base/numericalDerivative.h>

6
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -289,7 +289,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
}
gttoc(damp);
if (lmVerbosity >= LevenbergMarquardtParams::DAMPED)
dampedFactorGraph.print("damped");
dampedFactorGraph.print("damped");
result.lambdas++;
gttic(solve);
@ -302,7 +302,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
std::cout << "linear delta norm = " << newDelta.norm() << std::endl;
if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA)
newDelta.print("delta");
newDelta.print("delta");
// Evaluate the new error
gttic(compute_error);
@ -310,7 +310,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
gttoc(compute_error);
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
std::cout << "next error = " << error << std::endl;
std::cout << "next error = " << error << std::endl;
if(error < result.error) {
// Keep this change

6
gtsam_unstable/slam/AHRS.cpp
View File

@ -25,7 +25,7 @@ Matrix Z3 = zeros(3, 3);
/* ************************************************************************* */
AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
bool flat) :
bool flat) :
KF_(9) {
// Initial state
@ -182,8 +182,8 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
// F(:,k) = mech.x_a + dx_a - bRn*n_g;
// F(:,k) = mech.x_a + dx_a - bRn*(I+P)*n_g;
// F(:,k) = mech.x_a + dx_a - b_g - bRn*(rho x n_g); // P = [rho]_x
// Hence, the measurement z = b_g - (mech.x_a - F(:,k)) is predicted
// from the filter state (dx_a, rho) as dx_a + bRn*(n_g x rho)
// Hence, the measurement z = b_g - (mech.x_a - F(:,k)) is predicted
// from the filter state (dx_a, rho) as dx_a + bRn*(n_g x rho)
// z = b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho)
z = bRn * n_g_ - measured_b_g;
// Now the Jacobian H

46
gtsam_unstable/slam/CombinedImuFactor.h
View File

@ -118,14 +118,14 @@ namespace gtsam {
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15))
{
// COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21)
measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
// COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21)
measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3);
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(0,0,3,3), biasAccOmegaInit.block(0,3,3,3),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccOmegaInit.block(3,0,3,3), biasAccOmegaInit.block(3,3,3,3);
}
CombinedPreintegratedMeasurements() :
@ -231,10 +231,10 @@ namespace gtsam {
// overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(15,15);
F << H_pos_pos, H_pos_vel, H_pos_angles, Z_3x3, Z_3x3,
H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3,
H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega,
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
H_vel_pos, H_vel_vel, H_vel_angles, H_vel_biasacc, Z_3x3,
H_angles_pos, H_angles_vel, H_angles_angles, Z_3x3, H_angles_biasomega,
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
// first order uncertainty propagation
@ -567,18 +567,18 @@ namespace gtsam {
if(H6) {
H6->resize(15,6);
(*H6) <<
// dfP/dBias_j
Matrix3::Zero(), Matrix3::Zero(),
// dfV/dBias_j
Matrix3::Zero(), Matrix3::Zero(),
// dfR/dBias_j
Matrix3::Zero(), Matrix3::Zero(),
//dBiasAcc/dBias_j
Matrix3::Identity(), Matrix3::Zero(),
//dBiasOmega/dBias_j
Matrix3::Zero(), Matrix3::Identity();
H6->resize(15,6);
(*H6) <<
// dfP/dBias_j
Matrix3::Zero(), Matrix3::Zero(),
// dfV/dBias_j
Matrix3::Zero(), Matrix3::Zero(),
// dfR/dBias_j
Matrix3::Zero(), Matrix3::Zero(),
//dBiasAcc/dBias_j
Matrix3::Identity(), Matrix3::Zero(),
//dBiasOmega/dBias_j
Matrix3::Zero(), Matrix3::Identity();
}

764
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -39,8 +39,8 @@ namespace gtsam {
* =====
* Concept: Based on [Lupton12tro]
* - Pre-integrate IMU measurements using the static function PreIntegrateIMUObservations.
* Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began).
* All sensor-to-body transformations are performed here.
* Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began).
* All sensor-to-body transformations are performed here.
* - If required, calculate inertial solution by calling the static functions: predictPose_inertial, predictVelocity_inertial.
* - When the time is right, incorporate pre-integrated IMU data by creating an EquivInertialNavFactor_GlobalVel factor, which will
* relate between navigation variables at the two time instances (t0 and current time).
@ -54,11 +54,11 @@ namespace gtsam {
* 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.
* + 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}
@ -92,260 +92,260 @@ class EquivInertialNavFactor_GlobalVel : public NoiseModelFactor5<POSE, VELOCITY
private:
typedef EquivInertialNavFactor_GlobalVel<POSE, VELOCITY, IMUBIAS> This;
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
typedef EquivInertialNavFactor_GlobalVel<POSE, VELOCITY, IMUBIAS> This;
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
Vector delta_pos_in_t0_;
Vector delta_vel_in_t0_;
Vector3 delta_angles_;
double dt12_;
Vector delta_pos_in_t0_;
Vector delta_vel_in_t0_;
Vector3 delta_angles_;
double dt12_;
Vector world_g_;
Vector world_rho_;
Vector world_omega_earth_;
Vector world_g_;
Vector world_rho_;
Vector world_omega_earth_;
Matrix Jacobian_wrt_t0_Overall_;
Matrix Jacobian_wrt_t0_Overall_;
boost::optional<IMUBIAS> Bias_initial_; // Bias used when pre-integrating IMU measurements
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
boost::optional<IMUBIAS> Bias_initial_; // Bias used when pre-integrating IMU measurements
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<EquivInertialNavFactor_GlobalVel> shared_ptr;
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel> shared_ptr;
/** default constructor - only use for serialization */
EquivInertialNavFactor_GlobalVel() {}
/** default constructor - only use for serialization */
EquivInertialNavFactor_GlobalVel() {}
/** Constructor */
EquivInertialNavFactor_GlobalVel(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2,
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
double dt12, const Vector world_g, const Vector world_rho,
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
const Matrix& Jacobian_wrt_t0_Overall,
boost::optional<IMUBIAS> Bias_initial = boost::none, boost::optional<POSE> body_P_sensor = boost::none) :
Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2),
delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
Bias_initial_(Bias_initial), body_P_sensor_(body_P_sensor) { }
/** Constructor */
EquivInertialNavFactor_GlobalVel(const Key& Pose1, const Key& Vel1, const Key& IMUBias1, const Key& Pose2, const Key& Vel2,
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
double dt12, const Vector world_g, const Vector world_rho,
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
const Matrix& Jacobian_wrt_t0_Overall,
boost::optional<IMUBIAS> Bias_initial = boost::none, boost::optional<POSE> body_P_sensor = boost::none) :
Base(model_equivalent, Pose1, Vel1, IMUBias1, Pose2, Vel2),
delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
Bias_initial_(Bias_initial), body_P_sensor_(body_P_sensor) { }
virtual ~EquivInertialNavFactor_GlobalVel() {}
virtual ~EquivInertialNavFactor_GlobalVel() {}
/** implement functions needed for Testable */
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", 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 << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
std::cout << "delta_angles: " << this->delta_angles_ << std::endl;
std::cout << "dt12: " << this->dt12_ << 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");
}
/** print */
virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel", 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 << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
std::cout << "delta_angles: " << this->delta_angles_ << std::endl;
std::cout << "dt12: " << this->dt12_ << 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)
&& (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
&& (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
&& (delta_angles_ - e->delta_angles_).norm() < tol
&& (dt12_ - e->dt12_) < 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_)));
}
/** 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)
&& (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
&& (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
&& (delta_angles_ - e->delta_angles_).norm() < tol
&& (dt12_ - e->dt12_) < 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 {
POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
// Correct delta_pos_in_t0_ using (Bias1 - Bias_t0)
Vector delta_BiasAcc = Bias1.accelerometer();
Vector delta_BiasGyro = Bias1.gyroscope();
if (Bias_initial_){
delta_BiasAcc -= Bias_initial_->accelerometer();
delta_BiasGyro -= Bias_initial_->gyroscope();
}
// Correct delta_pos_in_t0_ using (Bias1 - Bias_t0)
Vector delta_BiasAcc = Bias1.accelerometer();
Vector delta_BiasGyro = Bias1.gyroscope();
if (Bias_initial_){
delta_BiasAcc -= Bias_initial_->accelerometer();
delta_BiasGyro -= Bias_initial_->gyroscope();
}
Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(4,9,3,3);
Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(4,12,3,3);
Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(0,12,3,3);
Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(4,9,3,3);
Matrix J_Pos_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(4,12,3,3);
Matrix J_angles_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(0,12,3,3);
/* Position term */
Vector delta_pos_in_t0_corrected = delta_pos_in_t0_ + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro;
/* Position term */
Vector delta_pos_in_t0_corrected = delta_pos_in_t0_ + J_Pos_wrt_BiasAcc*delta_BiasAcc + J_Pos_wrt_BiasGyro*delta_BiasGyro;
/* Rotation term */
Vector delta_angles_corrected = delta_angles_ + J_angles_wrt_BiasGyro*delta_BiasGyro;
// Another alternative:
// Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) );
/* Rotation term */
Vector delta_angles_corrected = delta_angles_ + J_angles_wrt_BiasGyro*delta_BiasGyro;
// Another alternative:
// Vector delta_angles_corrected = Rot3::Logmap( Rot3::Expmap(delta_angles_)*Rot3::Expmap(J_angles_wrt_BiasGyro*delta_BiasGyro) );
return predictPose_inertial(Pose1, Vel1,
delta_pos_in_t0_corrected, delta_angles_corrected,
dt12_, world_g_, world_rho_, world_omega_earth_);
}
return predictPose_inertial(Pose1, Vel1,
delta_pos_in_t0_corrected, delta_angles_corrected,
dt12_, world_g_, world_rho_, world_omega_earth_);
}
static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1,
const Vector& delta_pos_in_t0, const Vector3& delta_angles,
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){
static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1,
const Vector& delta_pos_in_t0, const Vector3& delta_angles,
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){
const POSE& world_P1_body = Pose1;
const VELOCITY& world_V1_body = Vel1;
const POSE& world_P1_body = Pose1;
const VELOCITY& world_V1_body = Vel1;
/* Position term */
Vector body_deltaPos_body = delta_pos_in_t0;
/* Position term */
Vector body_deltaPos_body = delta_pos_in_t0;
Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body;
Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body;
Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body;
Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body;
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12;
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12;
/* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct:
* the gravity should be canceled from the accelerometer measurements, bust since position
* is added with a delta velocity from a previous term, the actual delta time is more complicated.
* Need to figure out this in the future - currently because of this issue we'll get some more error
* in Z axis.
*/
/* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct:
* the gravity should be canceled from the accelerometer measurements, bust since position
* is added with a delta velocity from a previous term, the actual delta time is more complicated.
* Need to figure out this in the future - currently because of this issue we'll get some more error
* in Z axis.
*/
/* Rotation term */
Vector body_deltaAngles_body = delta_angles;
/* Rotation term */
Vector body_deltaAngles_body = delta_angles;
// 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;
// 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;
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12;
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12;
return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body));
return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body));
}
}
VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
// Correct delta_vel_in_t0_ using (Bias1 - Bias_t0)
Vector delta_BiasAcc = Bias1.accelerometer();
Vector delta_BiasGyro = Bias1.gyroscope();
if (Bias_initial_){
delta_BiasAcc -= Bias_initial_->accelerometer();
delta_BiasGyro -= Bias_initial_->gyroscope();
}
// Correct delta_vel_in_t0_ using (Bias1 - Bias_t0)
Vector delta_BiasAcc = Bias1.accelerometer();
Vector delta_BiasGyro = Bias1.gyroscope();
if (Bias_initial_){
delta_BiasAcc -= Bias_initial_->accelerometer();
delta_BiasGyro -= Bias_initial_->gyroscope();
}
Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(6,9,3,3);
Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(6,12,3,3);
Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(6,9,3,3);
Matrix J_Vel_wrt_BiasGyro = Jacobian_wrt_t0_Overall_.block(6,12,3,3);
Vector delta_vel_in_t0_corrected = delta_vel_in_t0_ + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro;
Vector delta_vel_in_t0_corrected = delta_vel_in_t0_ + J_Vel_wrt_BiasAcc*delta_BiasAcc + J_Vel_wrt_BiasGyro*delta_BiasGyro;
return predictVelocity_inertial(Pose1, Vel1,
delta_vel_in_t0_corrected,
dt12_, world_g_, world_rho_, world_omega_earth_);
}
return predictVelocity_inertial(Pose1, Vel1,
delta_vel_in_t0_corrected,
dt12_, world_g_, world_rho_, world_omega_earth_);
}
static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1,
const Vector& delta_vel_in_t0,
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) {
static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1,
const Vector& delta_vel_in_t0,
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) {
const POSE& world_P1_body = Pose1;
const VELOCITY& world_V1_body = Vel1;
const POSE& world_P1_body = Pose1;
const VELOCITY& world_V1_body = Vel1;
Vector body_deltaVel_body = delta_vel_in_t0;
Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body;
Vector body_deltaVel_body = delta_vel_in_t0;
Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body;
VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 );
VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 );
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12;
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12;
// Predict
return Vel1.compose( VelDelta );
// 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);
}
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);
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);
// Luca: difference between Pose2 and Pose2Pred
POSE DiffPose( Pose2.rotation().between(Pose2Pred.rotation()), Pose2Pred.translation() - Pose2.translation() );
// DiffPose = Pose2.between(Pose2Pred);
return DiffPose;
// Calculate error
//return Pose2.between(Pose2Pred);
}
// Luca: difference between Pose2 and Pose2Pred
POSE DiffPose( Pose2.rotation().between(Pose2Pred.rotation()), Pose2Pred.translation() - Pose2.translation() );
// DiffPose = Pose2.between(Pose2Pred);
return DiffPose;
// 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);
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);
}
// Calculate error
return Vel2.between(Vel2Pred);
}
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 {
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 = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
*H1 = stack(2, &H1_Pose, &H1_Vel);
}
// TODO: Write analytical derivative calculations
// Jacobian w.r.t. Pose1
if (H1){
Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, _1, Vel1, Bias1, Pose2, Vel2), Pose1);
Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::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 = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
*H2 = stack(2, &H2_Pose, &H2_Vel);
}
// Jacobian w.r.t. Vel1
if (H2){
Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::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 = numericalDerivative11<POSE, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1);
Matrix H3_Vel = numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1);
*H3 = stack(2, &H3_Pose, &H3_Vel);
}
// Jacobian w.r.t. IMUBias1
if (H3){
Matrix H3_Pose = numericalDerivative11<POSE, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, _1, Pose2, Vel2), Bias1);
Matrix H3_Vel = numericalDerivative11<VELOCITY, IMUBIAS>(boost::bind(&EquivInertialNavFactor_GlobalVel::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 = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2);
Matrix H4_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2);
*H4 = stack(2, &H4_Pose, &H4_Vel);
}
// Jacobian w.r.t. Pose2
if (H4){
Matrix H4_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, _1, Vel2), Pose2);
Matrix H4_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel::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 = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
Matrix H5_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
*H5 = stack(2, &H5_Pose, &H5_Vel);
}
// Jacobian w.r.t. Vel2
if (H5){
Matrix H5_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
Matrix H5_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::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)));
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);
}
return concatVectors(2, &ErrPoseVector, &ErrVelVector);
}
@ -413,137 +413,137 @@ public:
static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt,
Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(),
boost::optional<POSE> p_body_P_sensor = boost::none){
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: Earth-related terms are not accounted here but are incorporated in predict functions.
const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall, const IMUBIAS Bias_t0 = IMUBIAS(),
boost::optional<POSE> p_body_P_sensor = boost::none){
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: Earth-related terms are not accounted here but are incorporated in predict functions.
POSE body_P_sensor = POSE();
bool flag_use_body_P_sensor = false;
if (p_body_P_sensor){
body_P_sensor = *p_body_P_sensor;
flag_use_body_P_sensor = true;
}
POSE body_P_sensor = POSE();
bool flag_use_body_P_sensor = false;
if (p_body_P_sensor){
body_P_sensor = *p_body_P_sensor;
flag_use_body_P_sensor = true;
}
delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0);
delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0);
delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, Bias_t0);
delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0);
delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0);
delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, Bias_t0);
delta_t += msr_dt;
delta_t += msr_dt;
// Update EquivCov_Overall
Matrix Z_3x3 = zeros(3,3);
Matrix I_3x3 = eye(3,3);
// Update EquivCov_Overall
Matrix Z_3x3 = zeros(3,3);
Matrix I_3x3 = eye(3,3);
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
Matrix H_pos_angles = Z_3x3;
Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3);
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
Matrix H_pos_angles = Z_3x3;
Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3);
Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0);
Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
Matrix H_vel_bias = numericalDerivative11<LieVector, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
Matrix H_vel_pos = Z_3x3;
Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0);
Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
Matrix H_vel_bias = numericalDerivative11<LieVector, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
Matrix H_vel_pos = Z_3x3;
Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
Matrix H_angles_bias = numericalDerivative11<LieVector, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3;
Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
Matrix H_angles_bias = numericalDerivative11<LieVector, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3;
Matrix F_angles = collect(4, &H_angles_angles, &H_angles_pos, &H_angles_vel, &H_angles_bias);
Matrix F_pos = collect(4, &H_pos_angles, &H_pos_pos, &H_pos_vel, &H_pos_bias);
Matrix F_vel = collect(4, &H_vel_angles, &H_vel_pos, &H_vel_vel, &H_vel_bias);
Matrix F_bias_a = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3, &Z_3x3);
Matrix F_bias_g = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3);
Matrix F = stack(5, &F_angles, &F_pos, &F_vel, &F_bias_a, &F_bias_g);
Matrix F_angles = collect(4, &H_angles_angles, &H_angles_pos, &H_angles_vel, &H_angles_bias);
Matrix F_pos = collect(4, &H_pos_angles, &H_pos_pos, &H_pos_vel, &H_pos_bias);
Matrix F_vel = collect(4, &H_vel_angles, &H_vel_pos, &H_vel_vel, &H_vel_bias);
Matrix F_bias_a = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3, &Z_3x3);
Matrix F_bias_g = collect(5, &Z_3x3, &Z_3x3, &Z_3x3, &Z_3x3, &I_3x3);
Matrix F = stack(5, &F_angles, &F_pos, &F_vel, &F_bias_a, &F_bias_g);
noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt );
Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() );
noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt );
Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() );
EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d;
// Luca: force identity covariance matrix (for testing purposes)
// EquivCov_Overall = Matrix::Identity(15,15);
EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d;
// Luca: force identity covariance matrix (for testing purposes)
// EquivCov_Overall = Matrix::Identity(15,15);
// Update Jacobian_wrt_t0_Overall
Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall;
}
// Update Jacobian_wrt_t0_Overall
Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall;
}
static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt,
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){
static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt,
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here.
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here.
return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt;
}
return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt;
}
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor,
IMUBIAS Bias_t0 = IMUBIAS()){
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor,
IMUBIAS Bias_t0 = IMUBIAS()){
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object
Vector AccCorrected = Bias_t0.correctAccelerometer(msr_acc_t);
Vector body_t_a_body;
if (flag_use_body_P_sensor){
Matrix body_R_sensor = body_P_sensor.rotation().matrix();
// Calculate the corrected measurements using the Bias object
Vector AccCorrected = Bias_t0.correctAccelerometer(msr_acc_t);
Vector body_t_a_body;
if (flag_use_body_P_sensor){
Matrix body_R_sensor = body_P_sensor.rotation().matrix();
Vector GyroCorrected(Bias_t0.correctGyroscope(msr_gyro_t));
Vector GyroCorrected(Bias_t0.correctGyroscope(msr_gyro_t));
Vector body_omega_body = body_R_sensor * GyroCorrected;
Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
Vector body_omega_body = body_R_sensor * GyroCorrected;
Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector();
} else{
body_t_a_body = AccCorrected;
}
body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector();
} else{
body_t_a_body = AccCorrected;
}
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
}
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
}
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor,
IMUBIAS Bias_t0 = IMUBIAS()){
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor,
IMUBIAS Bias_t0 = IMUBIAS()){
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object
Vector GyroCorrected = Bias_t0.correctGyroscope(msr_gyro_t);
// Calculate the corrected measurements using the Bias object
Vector GyroCorrected = Bias_t0.correctGyroscope(msr_gyro_t);
Vector body_t_omega_body;
if (flag_use_body_P_sensor){
body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected;
} else {
body_t_omega_body = GyroCorrected;
}
Vector body_t_omega_body;
if (flag_use_body_P_sensor){
body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected;
} else {
body_t_omega_body = GyroCorrected;
}
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
return Rot3::Logmap(R_t_to_t0);
}
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
return Rot3::Logmap(R_t_to_t0);
}
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){
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() );
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;
cov_process.block(0,0, 3,3) += cov_gyro;
cov_process.block(6,6, 3,3) += cov_acc;
return noiseModel::Gaussian::Covariance(cov_process);
}
return noiseModel::Gaussian::Covariance(cov_process);
}
static inline void CalcEquivalentNoiseCov_DifferentParts(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
const noiseModel::Gaussian::shared_ptr& gaussian_process,
@ -554,107 +554,107 @@ public:
cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
}
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) {
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 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);
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;
// 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);
// 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;
}
// 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) )) ) );
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);
// 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 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));
// 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 UEN_to_ENU(0, 1, 0,
0, 0, 1,
1, 0, 0);
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
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;
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);
// 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);
}
// 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));
}
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));
}
/** 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));
}

678
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -39,8 +39,8 @@ namespace gtsam {
* =====
* Concept: Based on [Lupton12tro]
* - Pre-integrate IMU measurements using the static function PreIntegrateIMUObservations.
* Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began).
* All sensor-to-body transformations are performed here.
* Pre-integrated quantities are expressed in the body system of t0 - the first time instant (in which pre-integration began).
* All sensor-to-body transformations are performed here.
* - If required, calculate inertial solution by calling the static functions: predictPose_inertial, predictVelocity_inertial.
* - When the time is right, incorporate pre-integrated IMU data by creating an EquivInertialNavFactor_GlobalVel_NoBias factor, which will
* relate between navigation variables at the two time instances (t0 and current time).
@ -54,11 +54,11 @@ namespace gtsam {
* 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.
* + 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}
@ -92,222 +92,222 @@ class EquivInertialNavFactor_GlobalVel_NoBias : public NoiseModelFactor4<POSE, V
private:
typedef EquivInertialNavFactor_GlobalVel_NoBias<POSE, VELOCITY> This;
typedef NoiseModelFactor4<POSE, VELOCITY, POSE, VELOCITY> Base;
typedef EquivInertialNavFactor_GlobalVel_NoBias<POSE, VELOCITY> This;
typedef NoiseModelFactor4<POSE, VELOCITY, POSE, VELOCITY> Base;
Vector delta_pos_in_t0_;
Vector delta_vel_in_t0_;
Vector3 delta_angles_;
double dt12_;
Vector delta_pos_in_t0_;
Vector delta_vel_in_t0_;
Vector3 delta_angles_;
double dt12_;
Vector world_g_;
Vector world_rho_;
Vector world_omega_earth_;
Vector world_g_;
Vector world_rho_;
Vector world_omega_earth_;
Matrix Jacobian_wrt_t0_Overall_;
Matrix Jacobian_wrt_t0_Overall_;
boost::optional<POSE> body_P_sensor_; // The pose of the sensor in the body frame
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<EquivInertialNavFactor_GlobalVel_NoBias> shared_ptr;
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<EquivInertialNavFactor_GlobalVel_NoBias> shared_ptr;
/** default constructor - only use for serialization */
EquivInertialNavFactor_GlobalVel_NoBias() {}
/** default constructor - only use for serialization */
EquivInertialNavFactor_GlobalVel_NoBias() {}
/** Constructor */
EquivInertialNavFactor_GlobalVel_NoBias(const Key& Pose1, const Key& Vel1, const Key& Pose2, const Key& Vel2,
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
double dt12, const Vector world_g, const Vector world_rho,
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
const Matrix& Jacobian_wrt_t0_Overall,
boost::optional<POSE> body_P_sensor = boost::none) :
Base(model_equivalent, Pose1, Vel1, Pose2, Vel2),
delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
body_P_sensor_(body_P_sensor) { }
/** Constructor */
EquivInertialNavFactor_GlobalVel_NoBias(const Key& Pose1, const Key& Vel1, const Key& Pose2, const Key& Vel2,
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0, const Vector3& delta_angles,
double dt12, const Vector world_g, const Vector world_rho,
const Vector& world_omega_earth, const noiseModel::Gaussian::shared_ptr& model_equivalent,
const Matrix& Jacobian_wrt_t0_Overall,
boost::optional<POSE> body_P_sensor = boost::none) :
Base(model_equivalent, Pose1, Vel1, Pose2, Vel2),
delta_pos_in_t0_(delta_pos_in_t0), delta_vel_in_t0_(delta_vel_in_t0), delta_angles_(delta_angles),
dt12_(dt12), world_g_(world_g), world_rho_(world_rho), world_omega_earth_(world_omega_earth), Jacobian_wrt_t0_Overall_(Jacobian_wrt_t0_Overall),
body_P_sensor_(body_P_sensor) { }
virtual ~EquivInertialNavFactor_GlobalVel_NoBias() {}
virtual ~EquivInertialNavFactor_GlobalVel_NoBias() {}
/** implement functions needed for Testable */
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << "\n";
std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
std::cout << "delta_angles: " << this->delta_angles_ << std::endl;
std::cout << "dt12: " << this->dt12_ << 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");
}
/** print */
virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << "\n";
std::cout << "delta_pos_in_t0: " << this->delta_pos_in_t0_.transpose() << std::endl;
std::cout << "delta_vel_in_t0: " << this->delta_vel_in_t0_.transpose() << std::endl;
std::cout << "delta_angles: " << this->delta_angles_ << std::endl;
std::cout << "dt12: " << this->dt12_ << 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)
&& (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
&& (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
&& (delta_angles_ - e->delta_angles_).norm() < tol
&& (dt12_ - e->dt12_) < 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_)));
}
/** 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)
&& (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
&& (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
&& (delta_angles_ - e->delta_angles_).norm() < tol
&& (dt12_ - e->dt12_) < 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 {
POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1) const {
/* Position term */
Vector delta_pos_in_t0_corrected = delta_pos_in_t0_;
/* Position term */
Vector delta_pos_in_t0_corrected = delta_pos_in_t0_;
/* Rotation term */
Vector delta_angles_corrected = delta_angles_;
/* Rotation term */
Vector delta_angles_corrected = delta_angles_;
return predictPose_inertial(Pose1, Vel1,
delta_pos_in_t0_corrected, delta_angles_corrected,
dt12_, world_g_, world_rho_, world_omega_earth_);
}
return predictPose_inertial(Pose1, Vel1,
delta_pos_in_t0_corrected, delta_angles_corrected,
dt12_, world_g_, world_rho_, world_omega_earth_);
}
static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1,
const Vector& delta_pos_in_t0, const Vector3& delta_angles,
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){
static inline POSE predictPose_inertial(const POSE& Pose1, const VELOCITY& Vel1,
const Vector& delta_pos_in_t0, const Vector3& delta_angles,
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth){
const POSE& world_P1_body = Pose1;
const VELOCITY& world_V1_body = Vel1;
const POSE& world_P1_body = Pose1;
const VELOCITY& world_V1_body = Vel1;
/* Position term */
Vector body_deltaPos_body = delta_pos_in_t0;
/* Position term */
Vector body_deltaPos_body = delta_pos_in_t0;
Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body;
Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body;
Vector world_deltaPos_pls_body = world_P1_body.rotation().matrix() * body_deltaPos_body;
Vector world_deltaPos_body = world_V1_body * dt12 + 0.5*world_g*dt12*dt12 + world_deltaPos_pls_body;
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12;
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
world_deltaPos_body -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12*dt12;
/* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct:
* the gravity should be canceled from the accelerometer measurements, bust since position
* is added with a delta velocity from a previous term, the actual delta time is more complicated.
* Need to figure out this in the future - currently because of this issue we'll get some more error
* in Z axis.
*/
/* TODO: the term dt12*dt12 in 0.5*world_g*dt12*dt12 is not entirely correct:
* the gravity should be canceled from the accelerometer measurements, bust since position
* is added with a delta velocity from a previous term, the actual delta time is more complicated.
* Need to figure out this in the future - currently because of this issue we'll get some more error
* in Z axis.
*/
/* Rotation term */
Vector body_deltaAngles_body = delta_angles;
/* Rotation term */
Vector body_deltaAngles_body = delta_angles;
// 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;
// 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;
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12;
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
body_deltaAngles_body -= (body_rho + body_omega_earth)*dt12;
return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body));
return POSE(Pose1.rotation() * POSE::Rotation::Expmap(body_deltaAngles_body), Pose1.translation() + typename POSE::Translation(world_deltaPos_body));
}
}
VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1) const {
VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1) const {
Vector delta_vel_in_t0_corrected = delta_vel_in_t0_;
Vector delta_vel_in_t0_corrected = delta_vel_in_t0_;
return predictVelocity_inertial(Pose1, Vel1,
delta_vel_in_t0_corrected,
dt12_, world_g_, world_rho_, world_omega_earth_);
}
return predictVelocity_inertial(Pose1, Vel1,
delta_vel_in_t0_corrected,
dt12_, world_g_, world_rho_, world_omega_earth_);
}
static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1,
const Vector& delta_vel_in_t0,
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) {
static inline VELOCITY predictVelocity_inertial(const POSE& Pose1, const VELOCITY& Vel1,
const Vector& delta_vel_in_t0,
const double dt12, const Vector& world_g, const Vector& world_rho, const Vector& world_omega_earth) {
const POSE& world_P1_body = Pose1;
const VELOCITY& world_V1_body = Vel1;
const POSE& world_P1_body = Pose1;
const VELOCITY& world_V1_body = Vel1;
Vector body_deltaVel_body = delta_vel_in_t0;
Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body;
Vector body_deltaVel_body = delta_vel_in_t0;
Vector world_deltaVel_body = world_P1_body.rotation().matrix() * body_deltaVel_body;
VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 );
VELOCITY VelDelta( world_deltaVel_body + world_g * dt12 );
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12;
// Incorporate earth-related terms. Note - these are assumed to be constant between t1 and t2.
VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12;
// Predict
return Vel1.compose( VelDelta );
// Predict
return Vel1.compose( VelDelta );
}
}
void predict(const POSE& Pose1, const VELOCITY& Vel1, POSE& Pose2, VELOCITY& Vel2) const {
Pose2 = predictPose(Pose1, Vel1);
Vel2 = predictVelocity(Pose1, Vel1);
}
void predict(const POSE& Pose1, const VELOCITY& Vel1, POSE& Pose2, VELOCITY& Vel2) const {
Pose2 = predictPose(Pose1, Vel1);
Vel2 = predictVelocity(Pose1, Vel1);
}
POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const {
// Predict
POSE Pose2Pred = predictPose(Pose1, Vel1);
POSE evaluatePoseError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const {
// Predict
POSE Pose2Pred = predictPose(Pose1, Vel1);
// Calculate error
return Pose2.between(Pose2Pred);
}
// Calculate error
return Pose2.between(Pose2Pred);
}
VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const {
// Predict
VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1);
VELOCITY evaluateVelocityError(const POSE& Pose1, const VELOCITY& Vel1, const POSE& Pose2, const VELOCITY& Vel2) const {
// Predict
VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1);
// Calculate error
return Vel2.between(Vel2Pred);
}
// Calculate error
return Vel2.between(Vel2Pred);
}
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, 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) const {
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, 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) const {
// TODO: Write analytical derivative calculations
// Jacobian w.r.t. Pose1
if (H1){
Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1);
Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1);
*H1 = stack(2, &H1_Pose, &H1_Vel);
}
// TODO: Write analytical derivative calculations
// Jacobian w.r.t. Pose1
if (H1){
Matrix H1_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, _1, Vel1, Pose2, Vel2), Pose1);
Matrix H1_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, _1, Vel1, Pose2, Vel2), Pose1);
*H1 = stack(2, &H1_Pose, &H1_Vel);
}
// Jacobian w.r.t. Vel1
if (H2){
Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1);
Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1);
*H2 = stack(2, &H2_Pose, &H2_Vel);
}
// Jacobian w.r.t. Vel1
if (H2){
Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, _1, Pose2, Vel2), Vel1);
Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, _1, Pose2, Vel2), Vel1);
*H2 = stack(2, &H2_Pose, &H2_Vel);
}
// Jacobian w.r.t. Pose2
if (H3){
Matrix H3_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2);
Matrix H3_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2);
*H3 = stack(2, &H3_Pose, &H3_Vel);
}
// Jacobian w.r.t. Pose2
if (H3){
Matrix H3_Pose = numericalDerivative11<POSE, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, _1, Vel2), Pose2);
Matrix H3_Vel = numericalDerivative11<VELOCITY, POSE>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, _1, Vel2), Pose2);
*H3 = stack(2, &H3_Pose, &H3_Vel);
}
// Jacobian w.r.t. Vel2
if (H4){
Matrix H4_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2);
Matrix H4_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2);
*H4 = stack(2, &H4_Pose, &H4_Vel);
}
// Jacobian w.r.t. Vel2
if (H4){
Matrix H4_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluatePoseError, this, Pose1, Vel1, Pose2, _1), Vel2);
Matrix H4_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel_NoBias::evaluateVelocityError, this, Pose1, Vel1, Pose2, _1), Vel2);
*H4 = stack(2, &H4_Pose, &H4_Vel);
}
Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Pose2, Vel2)));
Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Pose2, Vel2)));
Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Pose2, Vel2)));
Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Pose2, Vel2)));
return concatVectors(2, &ErrPoseVector, &ErrVelVector);
}
return concatVectors(2, &ErrPoseVector, &ErrVelVector);
}
@ -348,126 +348,126 @@ public:
static inline void PreIntegrateIMUObservations(const Vector& msr_acc_t, const Vector& msr_gyro_t, const double msr_dt,
Vector& delta_pos_in_t0, Vector3& delta_angles, Vector& delta_vel_in_t0, double& delta_t,
const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall,
boost::optional<POSE> p_body_P_sensor = boost::none){
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: Earth-related terms are not accounted here but are incorporated in predict functions.
const noiseModel::Gaussian::shared_ptr& model_continuous_overall,
Matrix& EquivCov_Overall, Matrix& Jacobian_wrt_t0_Overall,
boost::optional<POSE> p_body_P_sensor = boost::none){
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: Earth-related terms are not accounted here but are incorporated in predict functions.
POSE body_P_sensor = POSE();
bool flag_use_body_P_sensor = false;
if (p_body_P_sensor){
body_P_sensor = *p_body_P_sensor;
flag_use_body_P_sensor = true;
}
POSE body_P_sensor = POSE();
bool flag_use_body_P_sensor = false;
if (p_body_P_sensor){
body_P_sensor = *p_body_P_sensor;
flag_use_body_P_sensor = true;
}
delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0);
delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor);
delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor);
delta_pos_in_t0 = PreIntegrateIMUObservations_delta_pos(msr_dt, delta_pos_in_t0, delta_vel_in_t0);
delta_vel_in_t0 = PreIntegrateIMUObservations_delta_vel(msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor);
delta_angles = PreIntegrateIMUObservations_delta_angles(msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor);
delta_t += msr_dt;
delta_t += msr_dt;
// Update EquivCov_Overall
Matrix Z_3x3 = zeros(3,3);
Matrix I_3x3 = eye(3,3);
// Update EquivCov_Overall
Matrix Z_3x3 = zeros(3,3);
Matrix I_3x3 = eye(3,3);
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
Matrix H_pos_angles = Z_3x3;
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
Matrix H_pos_angles = Z_3x3;
Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles);
Matrix H_vel_pos = Z_3x3;
Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor), delta_vel_in_t0);
Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor), delta_angles);
Matrix H_vel_pos = Z_3x3;
Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3;
Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor), delta_angles);
Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3;
Matrix F_angles = collect(3, &H_angles_angles, &H_angles_pos, &H_angles_vel);
Matrix F_pos = collect(3, &H_pos_angles, &H_pos_pos, &H_pos_vel);
Matrix F_vel = collect(3, &H_vel_angles, &H_vel_pos, &H_vel_vel);
Matrix F = stack(3, &F_angles, &F_pos, &F_vel);
Matrix F_angles = collect(3, &H_angles_angles, &H_angles_pos, &H_angles_vel);
Matrix F_pos = collect(3, &H_pos_angles, &H_pos_pos, &H_pos_vel);
Matrix F_vel = collect(3, &H_vel_angles, &H_vel_pos, &H_vel_vel);
Matrix F = stack(3, &F_angles, &F_pos, &F_vel);
noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt );
Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() );
noiseModel::Gaussian::shared_ptr model_discrete_curr = calc_descrete_noise_model(model_continuous_overall, msr_dt );
Matrix Q_d = inverse(model_discrete_curr->R().transpose() * model_discrete_curr->R() );
EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d;
EquivCov_Overall = F * EquivCov_Overall * F.transpose() + Q_d;
// Update Jacobian_wrt_t0_Overall
Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall;
}
// Update Jacobian_wrt_t0_Overall
Jacobian_wrt_t0_Overall = F * Jacobian_wrt_t0_Overall;
}
static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt,
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){
static inline Vector PreIntegrateIMUObservations_delta_pos(const double msr_dt,
const Vector& delta_pos_in_t0, const Vector& delta_vel_in_t0){
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here.
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: delta_vel_in_t0 is already in body frame, so no need to use the body_P_sensor transformation here.
return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt;
}
return delta_pos_in_t0 + delta_vel_in_t0 * msr_dt;
}
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){
static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
const Vector3& delta_angles, const Vector& delta_vel_in_t0, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object
Vector AccCorrected = msr_acc_t;
Vector body_t_a_body;
if (flag_use_body_P_sensor){
Matrix body_R_sensor = body_P_sensor.rotation().matrix();
// Calculate the corrected measurements using the Bias object
Vector AccCorrected = msr_acc_t;
Vector body_t_a_body;
if (flag_use_body_P_sensor){
Matrix body_R_sensor = body_P_sensor.rotation().matrix();
Vector GyroCorrected(msr_gyro_t);
Vector GyroCorrected(msr_gyro_t);
Vector body_omega_body = body_R_sensor * GyroCorrected;
Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
Vector body_omega_body = body_R_sensor * GyroCorrected;
Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector();
} else{
body_t_a_body = AccCorrected;
}
body_t_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor.translation().vector();
} else{
body_t_a_body = AccCorrected;
}
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
}
return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
}
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){
static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles, const bool flag_use_body_P_sensor, const POSE& body_P_sensor){
// Note: all delta terms refer to an IMU\sensor system at t0
// Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object
Vector GyroCorrected = msr_gyro_t;
// Calculate the corrected measurements using the Bias object
Vector GyroCorrected = msr_gyro_t;
Vector body_t_omega_body;
if (flag_use_body_P_sensor){
body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected;
} else {
body_t_omega_body = GyroCorrected;
}
Vector body_t_omega_body;
if (flag_use_body_P_sensor){
body_t_omega_body = body_P_sensor.rotation().matrix() * GyroCorrected;
} else {
body_t_omega_body = GyroCorrected;
}
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
return Rot3::Logmap(R_t_to_t0);
}
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
return Rot3::Logmap(R_t_to_t0);
}
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){
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() );
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;
cov_process.block(0,0, 3,3) += cov_gyro;
cov_process.block(6,6, 3,3) += cov_acc;
return noiseModel::Gaussian::Covariance(cov_process);
}
return noiseModel::Gaussian::Covariance(cov_process);
}
static inline void CalcEquivalentNoiseCov_DifferentParts(const noiseModel::Gaussian::shared_ptr& gaussian_acc, const noiseModel::Gaussian::shared_ptr& gaussian_gyro,
const noiseModel::Gaussian::shared_ptr& gaussian_process,
@ -478,107 +478,107 @@ public:
cov_process_without_acc_gyro = inverse( gaussian_process->R().transpose() * gaussian_process->R() );
}
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) {
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 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);
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;
// 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);
// 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;
}
// 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) )) ) );
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);
// 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 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));
// 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 UEN_to_ENU(0, 1, 0,
0, 0, 1,
1, 0, 0);
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
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;
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);
// 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);
}
// 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));
}
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));
}
/** 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));
}

256
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -44,11 +44,11 @@ namespace gtsam {
* 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.
* + 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}
@ -81,70 +81,70 @@ class InertialNavFactor_GlobalVelocity : public NoiseModelFactor5<POSE, VELOCITY
private:
typedef InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> This;
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
typedef InertialNavFactor_GlobalVelocity<POSE, VELOCITY, IMUBIAS> This;
typedef NoiseModelFactor5<POSE, VELOCITY, IMUBIAS, POSE, VELOCITY> Base;
Vector measurement_acc_;
Vector measurement_gyro_;
double dt_;
Vector measurement_acc_;
Vector measurement_gyro_;
double dt_;
Vector world_g_;
Vector world_rho_;
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;
// 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() {}
/** 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,
/** 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) { }
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() {}
virtual ~InertialNavFactor_GlobalVelocity() {}
/** implement functions needed for Testable */
/** 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: ");
/** 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_)));
}
/** 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
@ -225,12 +225,12 @@ public:
}
/** 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 {
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
@ -268,24 +268,24 @@ public:
*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)));
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);
}
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){
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() );
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;
cov_process.block(0,0, 3,3) += cov_gyro;
cov_process.block(6,6, 3,3) += cov_acc;
return noiseModel::Gaussian::Covariance(cov_process);
}
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) {
@ -317,78 +317,78 @@ public:
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) )) ) );
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);
// 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 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));
// 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 UEN_to_ENU(0, 1, 0,
0, 0, 1,
1, 0, 0);
Rot3 R_ECEF_to_ENU( UEN_to_ENU * C2 * C1 );
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;
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);
// 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);
}
// 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()/std::sqrt(delta_t));
}
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()/std::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));
}
/** 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

4
gtsam_unstable/slam/tests/testAHRS.cpp
View File

@ -92,8 +92,8 @@ TEST (AHRS, init) {
*/
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

22
gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
View File

@ -31,11 +31,11 @@ using namespace gtsam;
/* ************************************************************************* */
TEST( EquivInertialNavFactor_GlobalVel, Constructor)
{
Key poseKey1(11);
Key poseKey2(12);
Key velKey1(21);
Key velKey2(22);
Key biasKey1(31);
Key poseKey1(11);
Key poseKey2(12);
Key velKey1(21);
Key velKey2(22);
Key biasKey1(31);
// IMU accumulation variables
Vector delta_pos_in_t0 = Vector_(3, 0.0, 0.0, 0.0);
@ -46,16 +46,16 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor)
Matrix Jacobian_wrt_t0_Overall = eye(15);
imuBias::ConstantBias bias1 = imuBias::ConstantBias();
// Earth Terms (gravity, etc)
// Earth Terms (gravity, etc)
Vector3 g(0.0, 0.0, -9.80);
Vector3 rho(0.0, 0.0, 0.0);
Vector3 omega_earth(0.0, 0.0, 0.0);
// IMU Noise Model
SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9));
// IMU Noise Model
SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9));
// Constructor
EquivInertialNavFactor_GlobalVel<Pose3, LieVector, imuBias::ConstantBias> factor(
// Constructor
EquivInertialNavFactor_GlobalVel<Pose3, LieVector, imuBias::ConstantBias> factor(
poseKey1, velKey1, biasKey1, poseKey2, velKey2,
delta_pos_in_t0, delta_vel_in_t0, delta_angles, delta_t,
g, rho, omega_earth, imu_model, Jacobian_wrt_t0_Overall, bias1);
@ -63,5 +63,5 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor)
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

302
gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -30,9 +30,9 @@ 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);
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);
@ -49,24 +49,24 @@ gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, Constructor)
{
gtsam::Key Pose1(11);
gtsam::Key Pose2(12);
gtsam::Key Vel1(21);
gtsam::Key Vel2(22);
gtsam::Key Bias1(31);
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));
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);
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));
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> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
}
/* ************************************************************************* */
@ -78,20 +78,20 @@ TEST( InertialNavFactor_GlobalVelocity, Equals)
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));
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);
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));
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));
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));
}
/* ************************************************************************* */
@ -140,31 +140,31 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
double measurement_dt(0.1);
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));
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));
// 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);
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;
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));
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
Vector ExpectedErr(zero(9));
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
}
/* ************************************************************************* */
@ -176,30 +176,30 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
double measurement_dt(0.1);
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));
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));
// 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);
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;
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));
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
Vector ExpectedErr(zero(9));
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
}
/* ************************************************************************* */
@ -211,67 +211,67 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
double measurement_dt(0.1);
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));
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));
// 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);
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;
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));
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));
// 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();
// 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() );
// 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;
// gtsam::Matrix J_expected;
//
// LieVector v(predictionRq(angles, q));
// LieVector v(predictionRq(angles, q));
//
// J_expected = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionRq, _1, q), angles);
// J_expected = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionRq, _1, q), angles);
//
// cout<<"J_hyp"<<J_hyp<<endl;
// cout<<"J_expected"<<J_expected<<endl;
// cout<<"J_hyp"<<J_hyp<<endl;
// cout<<"J_expected"<<J_expected<<endl;
//
// CHECK( gtsam::assert_equal(J_expected, J_hyp, 1e-6));
// CHECK( gtsam::assert_equal(J_expected, J_hyp, 1e-6));
//}
///* VADIM - END ************************************************************************* */
@ -284,82 +284,82 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
double measurement_dt(0.01);
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));
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));
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);
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;
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;
Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual));
Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual));
// Checking for Pose part in the jacobians
// ******
Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols()));
Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols()));
Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols()));
Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols()));
Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols()));
// 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);
// 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-5));
CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3));
CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5));
// Verify they are equal for this choice of state
CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3));
CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5));
// 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()));
// 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);
// 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-5));
CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
// Verify they are equal for this choice of state
CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
}
@ -679,5 +679,5 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

6
gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp
View File

@ -28,9 +28,9 @@ 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);
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);