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

Merged in fix/imubias_minus_operator (pull request #120) 

fixed bug in operator - for ImuBias
release/4.3a0
Frank Dellaert 2015-03-10 22:05:26 -07:00
parent 8b63e2231d 9ae797a88c
commit e5098375e3
11 changed files with 1387 additions and 1013 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

158
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -33,19 +33,20 @@ using namespace std;
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements(
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) :
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit,
const bool use2ndOrderIntegration) :
PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
integrationErrorCovariance, use2ndOrderIntegration),
biasAccCovariance_(biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance),
biasAccOmegaInit_(biasAccOmegaInit)
{
integrationErrorCovariance, use2ndOrderIntegration), biasAccCovariance_(
biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), biasAccOmegaInit_(
biasAccOmegaInit) {
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{
void CombinedImuFactor::CombinedPreintegratedMeasurements::print(
const string& s) const {
PreintegrationBase::print(s);
cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl;
cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl;
@ -54,31 +55,35 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s
}
//------------------------------------------------------------------------------
bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{
return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_, tol)
&& equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_, tol)
&&equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol)
bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(
const CombinedPreintegratedMeasurements& expected, double tol) const {
return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_,
tol)
&& equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_,
tol)
&& equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol)
&& equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
&& PreintegrationBase::equals(expected, tol);
}
//------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){
void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration();
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega,
double deltaT, boost::optional<const Pose3&> body_P_sensor,
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor,
boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
// (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
correctedAcc, correctedOmega, body_P_sensor);
const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
@ -86,7 +91,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr,
deltaT);
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
// can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
@ -98,11 +104,11 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9);
// Single Jacobians to propagate covariance
Matrix3 H_vel_biasacc = - R_i * deltaT;
Matrix3 H_angles_biasomega =- D_Rincr_integratedOmega * deltaT;
Matrix3 H_vel_biasacc = -R_i * deltaT;
Matrix3 H_angles_biasomega = -D_Rincr_integratedOmega * deltaT;
// overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(15,15);
Matrix F(15, 15);
// for documentation:
// F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3,
// Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3,
@ -110,44 +116,46 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
// Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
F.setZero();
F.block<9,9>(0,0) = F_9x9;
F.block<6,6>(9,9) = I_6x6;
F.block<3,3>(3,9) = H_vel_biasacc;
F.block<3,3>(6,12) = H_angles_biasomega;
F.block<9, 9>(0, 0) = F_9x9;
F.block<6, 6>(9, 9) = I_6x6;
F.block<3, 3>(3, 9) = H_vel_biasacc;
F.block<3, 3>(6, 12) = H_angles_biasomega;
// first order uncertainty propagation
// Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose()
Matrix G_measCov_Gt = Matrix::Zero(15,15);
Matrix G_measCov_Gt = Matrix::Zero(15, 15);
// BLOCK DIAGONAL TERMS
G_measCov_Gt.block<3,3>(0,0) = deltaT * integrationCovariance();
G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc) *
(accelerometerCovariance() + biasAccOmegaInit_.block<3,3>(0,0) ) *
(H_vel_biasacc.transpose());
G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) * (H_angles_biasomega) *
(gyroscopeCovariance() + biasAccOmegaInit_.block<3,3>(3,3) ) *
(H_angles_biasomega.transpose());
G_measCov_Gt.block<3,3>(9,9) = (1/deltaT) * biasAccCovariance_;
G_measCov_Gt.block<3,3>(12,12) = (1/deltaT) * biasOmegaCovariance_;
G_measCov_Gt.block<3, 3>(0, 0) = deltaT * integrationCovariance();
G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc)
* (accelerometerCovariance() + biasAccOmegaInit_.block<3, 3>(0, 0))
* (H_vel_biasacc.transpose());
G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega)
* (gyroscopeCovariance() + biasAccOmegaInit_.block<3, 3>(3, 3))
* (H_angles_biasomega.transpose());
G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * biasAccCovariance_;
G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * biasOmegaCovariance_;
// OFF BLOCK DIAGONAL TERMS
Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3,3>(3,0) * H_angles_biasomega.transpose();
G_measCov_Gt.block<3,3>(3,6) = block23;
G_measCov_Gt.block<3,3>(6,3) = block23.transpose();
Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3, 3>(3, 0)
* H_angles_biasomega.transpose();
G_measCov_Gt.block<3, 3>(3, 6) = block23;
G_measCov_Gt.block<3, 3>(6, 3) = block23.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
// F_test and G_test are used for testing purposes and are not needed by the factor
if(F_test){
F_test->resize(15,15);
if (F_test) {
F_test->resize(15, 15);
(*F_test) << F;
}
if(G_test){
G_test->resize(15,21);
if (G_test) {
G_test->resize(15, 21);
// This is for testing & documentation
///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3,
Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3,
Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega,
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3,
(*G_test) << //
I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, //
Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, //
Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, //
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, //
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
}
}
@ -156,16 +164,22 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// CombinedImuFactor methods
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor() :
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6) {}
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3,
Z_3x3, Z_6x6) {
}
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j,
Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) :
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
_PIM_(preintegratedMeasurements) {}
Base(
noiseModel::Gaussian::Covariance(
preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j,
vel_j, bias_i, bias_j), ImuFactorBase(gravity, omegaCoriolis,
body_P_sensor, use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) {
}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
@ -174,13 +188,11 @@ gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
}
//------------------------------------------------------------------------------
void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "CombinedImuFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ","
void CombinedImuFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "CombinedImuFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ","
<< keyFormatter(this->key6()) << ")\n";
ImuFactorBase::print("");
_PIM_.print(" preintegrated measurements:");
@ -188,10 +200,10 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter)
}
//------------------------------------------------------------------------------
bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
&& _PIM_.equals(e->_PIM_, tol)
bool CombinedImuFactor::equals(const NonlinearFactor& expected,
double tol) const {
const This *e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
&& ImuFactorBase::equals(*e, tol);
}
@ -220,39 +232,39 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
// if we need the jacobians
if (H1) {
H1->resize(15, 6);
H1->block < 9, 6 > (0, 0) = D_r_pose_i;
H1->block<9, 6>(0, 0) = D_r_pose_i;
// adding: [dBiasAcc/dPi ; dBiasOmega/dPi]
H1->block < 6, 6 > (9, 0) = Z_6x6;
H1->block<6, 6>(9, 0) = Z_6x6;
}
if (H2) {
H2->resize(15, 3);
H2->block < 9, 3 > (0, 0) = D_r_vel_i;
H2->block<9, 3>(0, 0) = D_r_vel_i;
// adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
H2->block < 6, 3 > (9, 0) = Matrix::Zero(6, 3);
H2->block<6, 3>(9, 0) = Matrix::Zero(6, 3);
}
if (H3) {
H3->resize(15, 6);
H3->block < 9, 6 > (0, 0) = D_r_pose_j;
H3->block<9, 6>(0, 0) = D_r_pose_j;
// adding: [dBiasAcc/dPj ; dBiasOmega/dPj]
H3->block < 6, 6 > (9, 0) = Z_6x6;
H3->block<6, 6>(9, 0) = Z_6x6;
}
if (H4) {
H4->resize(15, 3);
H4->block < 9, 3 > (0, 0) = D_r_vel_j;
H4->block<9, 3>(0, 0) = D_r_vel_j;
// adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
H4->block < 6, 3 > (9, 0) = Matrix::Zero(6, 3);
H4->block<6, 3>(9, 0) = Matrix::Zero(6, 3);
}
if (H5) {
H5->resize(15, 6);
H5->block < 9, 6 > (0, 0) = D_r_bias_i;
H5->block<9, 6>(0, 0) = D_r_bias_i;
// adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i]
H5->block < 6, 6 > (9, 0) = Hbias_i;
H5->block<6, 6>(9, 0) = Hbias_i;
}
if (H6) {
H6->resize(15, 6);
H6->block < 9, 6 > (0, 0) = Matrix::Zero(9, 6);
H6->block<9, 6>(0, 0) = Matrix::Zero(9, 6);
// adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
H6->block < 6, 6 > (9, 0) = Hbias_j;
H6->block<6, 6>(9, 0) = Hbias_j;
}
// overall error

77
gtsam/navigation/CombinedImuFactor.h
View File

@ -65,7 +65,8 @@ namespace gtsam {
* the correlation between the bias uncertainty and the preintegrated
* measurements uncertainty.
*/
class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias>, public ImuFactorBase{
class CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, Pose3,
Vector3, imuBias::ConstantBias, imuBias::ConstantBias>, public ImuFactorBase {
public:
/**
@ -86,7 +87,7 @@ public:
Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration
Eigen::Matrix<double,15,15> preintMeasCov_; ///< Covariance matrix of the preintegrated measurements
Eigen::Matrix<double, 15, 15> preintMeasCov_; ///< Covariance matrix of the preintegrated measurements
///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation
///< between the preintegrated measurements and the biases
@ -105,16 +106,20 @@ public:
* @param use2ndOrderIntegration Controls the order of integration
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
*/
CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = false);
const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration =
false);
/// print
void print(const std::string& s = "Preintegrated Measurements:") const;
/// equals
bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const;
bool equals(const CombinedPreintegratedMeasurements& expected, double tol =
1e-9) const;
/// Re-initialize CombinedPreintegratedMeasurements
void resetIntegration();
@ -126,12 +131,16 @@ public:
* @param deltaT Time interval between two consecutive IMU measurements
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
*/
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none,
boost::optional<Matrix&> F_test = boost::none, boost::optional<Matrix&> G_test = boost::none);
boost::optional<Matrix&> F_test = boost::none,
boost::optional<Matrix&> G_test = boost::none);
/// methods to access class variables
Matrix preintMeasCov() const { return preintMeasCov_;}
Matrix preintMeasCov() const {
return preintMeasCov_;
}
private:
@ -147,7 +156,8 @@ public:
private:
typedef CombinedImuFactor This;
typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base;
typedef NoiseModelFactor6<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias, imuBias::ConstantBias> Base;
CombinedPreintegratedMeasurements _PIM_;
@ -177,12 +187,15 @@ public:
* @param body_P_sensor Optional pose of the sensor frame in the body frame
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
*/
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
Key bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false);
boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
virtual ~CombinedImuFactor() {}
virtual ~CombinedImuFactor() {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
@ -190,54 +203,60 @@ public:
/** implement functions needed for Testable */
/// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/// equals
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/** Access the preintegrated measurements. */
const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
return _PIM_; }
return _PIM_;
}
/** implement functions needed to derive from Factor */
/// vector of errors
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
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,
boost::optional<Matrix&> H6 = boost::none) const;
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, boost::optional<Matrix&> H6 = boost::none) const;
/** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity,
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
PoseVelocityBias PVB(PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out));
PoseVelocityBias PVB(
PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
use2ndOrderCoriolis, deltaPij_biascorrected_out,
deltaVij_biascorrected_out));
pose_j = PVB.pose;
vel_j = PVB.velocity;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor6",
ar
& boost::serialization::make_nvp("NoiseModelFactor6",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // class CombinedImuFactor
};
// class CombinedImuFactor
typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements;

83
gtsam/navigation/ImuBias.h
View File

@ -36,24 +36,24 @@ namespace gtsam {
/// All bias models live in the imuBias namespace
namespace imuBias {
class ConstantBias {
private:
class ConstantBias {
private:
Vector3 biasAcc_;
Vector3 biasGyro_;
public:
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 6;
ConstantBias():
ConstantBias() :
biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) {
}
ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro):
ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro) :
biasAcc_(biasAcc), biasGyro_(biasGyro) {
}
ConstantBias(const Vector6& v):
ConstantBias(const Vector6& v) :
biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) {
}
@ -65,13 +65,18 @@ namespace imuBias {
}
/** get accelerometer bias */
const Vector3& accelerometer() const { return biasAcc_; }
const Vector3& accelerometer() const {
return biasAcc_;
}
/** get gyroscope bias */
const Vector3& gyroscope() const { return biasGyro_; }
const Vector3& gyroscope() const {
return biasGyro_;
}
/** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
Vector3 correctAccelerometer(const Vector3& measurement, boost::optional<Matrix&> H=boost::none) const {
Vector3 correctAccelerometer(const Vector3& measurement,
boost::optional<Matrix&> H = boost::none) const {
if (H) {
H->resize(3, 6);
(*H) << -Matrix3::Identity(), Matrix3::Zero();
@ -80,7 +85,8 @@ namespace imuBias {
}
/** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
Vector3 correctGyroscope(const Vector3& measurement, boost::optional<Matrix&> H=boost::none) const {
Vector3 correctGyroscope(const Vector3& measurement,
boost::optional<Matrix&> H = boost::none) const {
if (H) {
H->resize(3, 6);
(*H) << Matrix3::Zero(), -Matrix3::Identity();
@ -118,11 +124,11 @@ namespace imuBias {
//// return measurement - bias_gyro_temp - R_G_to_I * w_earth_rate_G;
// }
/// @}
/// @name Testable
/// @{
/// @}
/// @name Testable
/// @{
/// print with optional string
/// print with optional string
void print(const std::string& s = "") const {
// explicit printing for now.
std::cout << s + ".biasAcc [" << biasAcc_.transpose() << "]" << std::endl;
@ -130,7 +136,7 @@ namespace imuBias {
}
/** equality up to tolerance */
inline bool equals(const ConstantBias& expected, double tol=1e-5) const {
inline bool equals(const ConstantBias& expected, double tol = 1e-5) const {
return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol)
&& equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol);
}
@ -140,7 +146,9 @@ namespace imuBias {
/// @{
/** identity for group operation */
static ConstantBias identity() { return ConstantBias(); }
static ConstantBias identity() {
return ConstantBias();
}
/** inverse */
inline ConstantBias operator-() const {
@ -154,23 +162,37 @@ namespace imuBias {
/** subtraction */
ConstantBias operator-(const ConstantBias& b) const {
return ConstantBias(b.biasAcc_ - biasAcc_, b.biasGyro_ - biasGyro_);
return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_);
}
/// @}
/// @name Deprecated
/// @{
ConstantBias inverse() { return -(*this);}
ConstantBias compose(const ConstantBias& q) { return (*this)+q;}
ConstantBias between(const ConstantBias& q) { return q-(*this);}
Vector6 localCoordinates(const ConstantBias& q) { return between(q).vector();}
ConstantBias retract(const Vector6& v) {return compose(ConstantBias(v));}
static Vector6 Logmap(const ConstantBias& p) {return p.vector();}
static ConstantBias Expmap(const Vector6& v) { return ConstantBias(v);}
ConstantBias inverse() {
return -(*this);
}
ConstantBias compose(const ConstantBias& q) {
return (*this) + q;
}
ConstantBias between(const ConstantBias& q) {
return q - (*this);
}
Vector6 localCoordinates(const ConstantBias& q) {
return between(q).vector();
}
ConstantBias retract(const Vector6& v) {
return compose(ConstantBias(v));
}
static Vector6 Logmap(const ConstantBias& p) {
return p.vector();
}
static ConstantBias Expmap(const Vector6& v) {
return ConstantBias(v);
}
/// @}
private:
private:
/// @name Advanced Interface
/// @{
@ -178,17 +200,17 @@ namespace imuBias {
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("imuBias::ConstantBias",*this);
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("imuBias::ConstantBias", *this);
ar & BOOST_SERIALIZATION_NVP(biasAcc_);
ar & BOOST_SERIALIZATION_NVP(biasGyro_);
}
/// @}
}; // ConstantBias class
} // namespace imuBias
};
// ConstantBias class
}// namespace imuBias
template<>
struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
@ -197,4 +219,3 @@ struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
} // namespace gtsam

81
gtsam/navigation/ImuFactor.cpp
View File

@ -33,12 +33,11 @@ using namespace std;
//------------------------------------------------------------------------------
ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const bool use2ndOrderIntegration) :
PreintegrationBase(bias,
measuredAccCovariance, measuredOmegaCovariance,
integrationErrorCovariance, use2ndOrderIntegration)
{
PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
integrationErrorCovariance, use2ndOrderIntegration) {
preintMeasCov_.setZero();
}
@ -49,13 +48,14 @@ void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
}
//------------------------------------------------------------------------------
bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const {
bool ImuFactor::PreintegratedMeasurements::equals(
const PreintegratedMeasurements& expected, double tol) const {
return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
&& PreintegrationBase::equals(expected, tol);
}
//------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::resetIntegration(){
void ImuFactor::PreintegratedMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration();
preintMeasCov_.setZero();
}
@ -63,18 +63,20 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){
//------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor,
OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
boost::optional<const Pose3&> body_P_sensor, OptionalJacobian<9, 9> F_test,
OptionalJacobian<9, 9> G_test) {
Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
correctedAcc, correctedOmega, body_P_sensor);
const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
// Update Jacobians
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr,
deltaT);
// Update preintegrated measurements (also get Jacobian)
const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test
@ -90,25 +92,28 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done blockwise,
// as G and measurementCovariance are blockdiagonal matrices
preintMeasCov_ = F * preintMeasCov_ * F.transpose();
preintMeasCov_.block<3,3>(0,0) += integrationCovariance() * deltaT;
preintMeasCov_.block<3,3>(3,3) += R_i * accelerometerCovariance() * R_i.transpose() * deltaT;
preintMeasCov_.block<3,3>(6,6) += D_Rincr_integratedOmega * gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT;
preintMeasCov_.block<3, 3>(0, 0) += integrationCovariance() * deltaT;
preintMeasCov_.block<3, 3>(3, 3) += R_i * accelerometerCovariance()
* R_i.transpose() * deltaT;
preintMeasCov_.block<3, 3>(6, 6) += D_Rincr_integratedOmega
* gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT;
Matrix3 F_pos_noiseacc;
if(use2ndOrderIntegration()){
if (use2ndOrderIntegration()) {
F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT;
preintMeasCov_.block<3,3>(0,0) += (1/deltaT) * F_pos_noiseacc * accelerometerCovariance() * F_pos_noiseacc.transpose();
preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc
* accelerometerCovariance() * F_pos_noiseacc.transpose();
Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT
preintMeasCov_.block<3,3>(0,3) += temp;
preintMeasCov_.block<3,3>(3,0) += temp.transpose();
preintMeasCov_.block<3, 3>(0, 3) += temp;
preintMeasCov_.block<3, 3>(3, 0) += temp.transpose();
}
// F_test and G_test are given as output for testing purposes and are not needed by the factor
if(F_test){ // This in only for testing
if (F_test) { // This in only for testing
(*F_test) << F;
}
if(G_test){ // This in only for testing & documentation, while the actual computation is done block-wise
if(!use2ndOrderIntegration())
if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise
if (!use2ndOrderIntegration())
F_pos_noiseacc = Z_3x3;
// intNoise accNoise omegaNoise
@ -122,19 +127,20 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// ImuFactor methods
//------------------------------------------------------------------------------
ImuFactor::ImuFactor() :
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {}
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {
}
//------------------------------------------------------------------------------
ImuFactor::ImuFactor(
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor,
const bool use2ndOrderCoriolis) :
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov_),
pose_i, vel_i, pose_j, vel_j, bias),
ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
_PIM_(preintegratedMeasurements) {}
boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) :
Base(
noiseModel::Gaussian::Covariance(
preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j,
vel_j, bias), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor,
use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) {
}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
@ -144,12 +150,10 @@ gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
//------------------------------------------------------------------------------
void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "ImuFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ","
<< keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ")\n";
cout << s << "ImuFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << "," << keyFormatter(this->key5())
<< ")\n";
ImuFactorBase::print("");
_PIM_.print(" preintegrated measurements:");
this->noiseModel_->print(" noise model: ");
@ -157,9 +161,8 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
//------------------------------------------------------------------------------
bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
&& _PIM_.equals(e->_PIM_, tol)
const This *e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
&& ImuFactorBase::equals(*e, tol);
}

76
gtsam/navigation/ImuFactor.h
View File

@ -58,7 +58,8 @@ namespace gtsam {
* (which are usually slowly varying quantities), which is up to the caller.
* See also CombinedImuFactor for a class that does this for you.
*/
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias>, public ImuFactorBase {
class ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias>, public ImuFactorBase {
public:
/**
@ -75,7 +76,7 @@ public:
protected:
Eigen::Matrix<double,9,9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
Eigen::Matrix<double, 9, 9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
///< (first-order propagation from *measurementCovariance*).
public:
@ -90,14 +91,17 @@ public:
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
*/
PreintegratedMeasurements(const imuBias::ConstantBias& bias,
const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration = false);
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const bool use2ndOrderIntegration = false);
/// print
void print(const std::string& s = "Preintegrated Measurements:") const;
/// equals
bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const;
bool equals(const PreintegratedMeasurements& expected,
double tol = 1e-9) const;
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
@ -110,12 +114,16 @@ public:
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
* @param Fout, Gout Jacobians used internally (only needed for testing)
*/
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none,
OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none);
OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout =
boost::none);
/// methods to access class variables
Matrix preintMeasCov() const { return preintMeasCov_;}
Matrix preintMeasCov() const {
return preintMeasCov_;
}
private:
@ -128,14 +136,15 @@ public:
}
};
private:
private:
typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base;
typedef NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> Base;
PreintegratedMeasurements _PIM_;
public:
public:
/** Shorthand for a smart pointer to a factor */
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
@ -163,9 +172,11 @@ public:
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false);
boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
virtual ~ImuFactor() {}
virtual ~ImuFactor() {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
@ -173,52 +184,59 @@ public:
/** implement functions needed for Testable */
/// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const;
/// equals
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
/** Access the preintegrated measurements. */
const PreintegratedMeasurements& preintegratedMeasurements() const {
return _PIM_; }
return _PIM_;
}
/** implement functions needed to derive from Factor */
/// vector of errors
Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias,
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 Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias, 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;
/** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const PreintegratedMeasurements& PIM, const Vector3& gravity,
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
const PreintegratedMeasurements& PIM, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
PoseVelocityBias PVB(PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected_out, deltaVij_biascorrected_out));
PoseVelocityBias PVB(
PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
use2ndOrderCoriolis, deltaPij_biascorrected_out,
deltaVij_biascorrected_out));
pose_j = PVB.pose;
vel_j = PVB.velocity;
}
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor5",
ar
& boost::serialization::make_nvp("NoiseModelFactor5",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // class ImuFactor
};
// class ImuFactor
typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;

34
gtsam/navigation/ImuFactorBase.h
View File

@ -40,8 +40,9 @@ public:
/** Default constructor - only use for serialization */
ImuFactorBase() :
gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)),
body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {}
gravity_(Vector3(0.0, 0.0, 9.81)), omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), body_P_sensor_(
boost::none), use2ndOrderCoriolis_(false) {
}
/**
* Default constructor, stores basic quantities required by the Imu factors
@ -51,21 +52,29 @@ public:
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
*/
ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) :
gravity_(gravity), omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {}
boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false) :
gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {
}
/// Methods to access class variables
const Vector3& gravity() const { return gravity_; }
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
const Vector3& gravity() const {
return gravity_;
}
const Vector3& omegaCoriolis() const {
return omegaCoriolis_;
}
/// Needed for testable
//------------------------------------------------------------------------------
void print(const std::string& s) const {
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" << std::endl;
if(this->body_P_sensor_)
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]"
<< std::endl;
std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]"
<< std::endl;
if (this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
@ -75,8 +84,9 @@ public:
return equal_with_abs_tol(gravity_, expected.gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol)
&& (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_)
&& ((!body_P_sensor_ && !expected.body_P_sensor_) ||
(body_P_sensor_ && expected.body_P_sensor_ && body_P_sensor_->equals(*expected.body_P_sensor_)));
&& ((!body_P_sensor_ && !expected.body_P_sensor_)
|| (body_P_sensor_ && expected.body_P_sensor_
&& body_P_sensor_->equals(*expected.body_P_sensor_)));
}
};

50
gtsam/navigation/PreintegratedRotation.h
View File

@ -45,15 +45,26 @@ public:
* Default constructor, initializes the variables in the base class
*/
PreintegratedRotation(const Matrix3& measuredOmegaCovariance) :
deltaRij_(Rot3()), deltaTij_(0.0),
delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(measuredOmegaCovariance) {}
deltaRij_(Rot3()), deltaTij_(0.0), delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(
measuredOmegaCovariance) {
}
/// methods to access class variables
Matrix3 deltaRij() const {return deltaRij_.matrix();} // expensive
Vector3 thetaRij(OptionalJacobian<3,3> H = boost::none) const {return Rot3::Logmap(deltaRij_, H);} // super-expensive
const double& deltaTij() const{return deltaTij_;}
const Matrix3& delRdelBiasOmega() const{ return delRdelBiasOmega_;}
const Matrix3& gyroscopeCovariance() const { return gyroscopeCovariance_;}
Matrix3 deltaRij() const {
return deltaRij_.matrix();
} // expensive
Vector3 thetaRij(OptionalJacobian<3, 3> H = boost::none) const {
return Rot3::Logmap(deltaRij_, H);
} // super-expensive
const double& deltaTij() const {
return deltaTij_;
}
const Matrix3& delRdelBiasOmega() const {
return delRdelBiasOmega_;
}
const Matrix3& gyroscopeCovariance() const {
return gyroscopeCovariance_;
}
/// Needed for testable
void print(const std::string& s) const {
@ -61,19 +72,22 @@ public:
std::cout << "deltaTij [" << deltaTij_ << "]" << std::endl;
deltaRij_.print(" deltaRij ");
std::cout << "delRdelBiasOmega [" << delRdelBiasOmega_ << "]" << std::endl;
std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]" << std::endl;
std::cout << "gyroscopeCovariance [" << gyroscopeCovariance_ << "]"
<< std::endl;
}
/// Needed for testable
bool equals(const PreintegratedRotation& expected, double tol) const {
return deltaRij_.equals(expected.deltaRij_, tol)
&& fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol)
&& equal_with_abs_tol(gyroscopeCovariance_, expected.gyroscopeCovariance_, tol);
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_,
tol)
&& equal_with_abs_tol(gyroscopeCovariance_,
expected.gyroscopeCovariance_, tol);
}
/// Re-initialize PreintegratedMeasurements
void resetIntegration(){
void resetIntegration() {
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delRdelBiasOmega_ = Z_3x3;
@ -81,7 +95,7 @@ public:
/// Update preintegrated measurements
void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
OptionalJacobian<3, 3> H = boost::none){
OptionalJacobian<3, 3> H = boost::none) {
deltaRij_ = deltaRij_.compose(incrR, H, boost::none);
deltaTij_ += deltaT;
}
@ -90,15 +104,16 @@ public:
* Update Jacobians to be used during preintegration
* TODO: explain arguments
*/
void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
double deltaT) {
void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega,
const Rot3& incrR, double deltaT) {
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_Rincr_integratedOmega * deltaT;
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- D_Rincr_integratedOmega * deltaT;
}
/// Return a bias corrected version of the integrated rotation - expensive
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const {
return deltaRij_*Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr);
return deltaRij_ * Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr);
}
/// Get so<3> version of bias corrected rotation, with optional Jacobian
@ -111,7 +126,8 @@ public:
if (H) {
Matrix3 Jrinv_theta_bc;
// This was done via an expmap, now we go *back* to so<3>
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected, Jrinv_theta_bc);
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected,
Jrinv_theta_bc);
const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;

285
gtsam/navigation/PreintegrationBase.h
View File

@ -46,7 +46,7 @@ struct PoseVelocityBias {
* It includes the definitions of the preintegrated variables and the methods
* to access, print, and compare them.
*/
class PreintegrationBase : public PreintegratedRotation {
class PreintegrationBase: public PreintegratedRotation {
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
bool use2ndOrderIntegration_; ///< Controls the order of integration
@ -72,56 +72,84 @@ public:
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
*/
PreintegrationBase(const imuBias::ConstantBias& bias,
const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance,
const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration) :
PreintegratedRotation(measuredOmegaCovariance),
biasHat_(bias), use2ndOrderIntegration_(use2ndOrderIntegration),
deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
accelerometerCovariance_(measuredAccCovariance),
integrationCovariance_(integrationErrorCovariance) {}
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3&integrationErrorCovariance,
const bool use2ndOrderIntegration) :
PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias), use2ndOrderIntegration_(
use2ndOrderIntegration), deltaPij_(Vector3::Zero()), deltaVij_(
Vector3::Zero()), delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), delVdelBiasAcc_(
Z_3x3), delVdelBiasOmega_(Z_3x3), accelerometerCovariance_(
measuredAccCovariance), integrationCovariance_(
integrationErrorCovariance) {
}
/// methods to access class variables
bool use2ndOrderIntegration() const {return use2ndOrderIntegration_;}
const Vector3& deltaPij() const {return deltaPij_;}
const Vector3& deltaVij() const {return deltaVij_;}
const imuBias::ConstantBias& biasHat() const { return biasHat_;}
Vector6 biasHatVector() const { return biasHat_.vector();} // expensive
const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;}
const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;}
const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;}
const Matrix3& delVdelBiasOmega() const { return delVdelBiasOmega_;}
bool use2ndOrderIntegration() const {
return use2ndOrderIntegration_;
}
const Vector3& deltaPij() const {
return deltaPij_;
}
const Vector3& deltaVij() const {
return deltaVij_;
}
const imuBias::ConstantBias& biasHat() const {
return biasHat_;
}
Vector6 biasHatVector() const {
return biasHat_.vector();
} // expensive
const Matrix3& delPdelBiasAcc() const {
return delPdelBiasAcc_;
}
const Matrix3& delPdelBiasOmega() const {
return delPdelBiasOmega_;
}
const Matrix3& delVdelBiasAcc() const {
return delVdelBiasAcc_;
}
const Matrix3& delVdelBiasOmega() const {
return delVdelBiasOmega_;
}
const Matrix3& accelerometerCovariance() const { return accelerometerCovariance_;}
const Matrix3& integrationCovariance() const { return integrationCovariance_;}
const Matrix3& accelerometerCovariance() const {
return accelerometerCovariance_;
}
const Matrix3& integrationCovariance() const {
return integrationCovariance_;
}
/// Needed for testable
void print(const std::string& s) const {
PreintegratedRotation::print(s);
std::cout << " accelerometerCovariance [ " << accelerometerCovariance_ << " ]" << std::endl;
std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]" << std::endl;
std::cout << " accelerometerCovariance [ " << accelerometerCovariance_
<< " ]" << std::endl;
std::cout << " integrationCovariance [ " << integrationCovariance_ << " ]"
<< std::endl;
std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
biasHat_.print(" biasHat");
}
/// Needed for testable
bool equals(const PreintegrationBase& expected, double tol) const {
return PreintegratedRotation::equals(expected, tol)
&& biasHat_.equals(expected.biasHat_, tol)
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
&& equal_with_abs_tol(accelerometerCovariance_, expected.accelerometerCovariance_, tol)
&& equal_with_abs_tol(integrationCovariance_, expected.integrationCovariance_, tol);
bool equals(const PreintegrationBase& other, double tol) const {
return PreintegratedRotation::equals(other, tol)
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(deltaPij_, other.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, other.deltaVij_, tol)
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol)
&& equal_with_abs_tol(accelerometerCovariance_,
other.accelerometerCovariance_, tol)
&& equal_with_abs_tol(integrationCovariance_,
other.integrationCovariance_, tol);
}
/// Re-initialize PreintegratedMeasurements
void resetIntegration(){
void resetIntegration() {
PreintegratedRotation::resetIntegration();
deltaPij_ = Vector3::Zero();
deltaVij_ = Vector3::Zero();
@ -137,47 +165,52 @@ public:
Matrix3 dRij = deltaRij(); // expensive
Vector3 temp = dRij * correctedAcc * deltaT;
if(!use2ndOrderIntegration_){
if (!use2ndOrderIntegration_) {
deltaPij_ += deltaVij_ * deltaT;
}else{
} else {
deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
}
deltaVij_ += temp;
Matrix3 R_i, F_angles_angles;
if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0);
if (F)
R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0);
if(F){
Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
if (F) {
Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT;
Matrix3 F_pos_angles;
if(use2ndOrderIntegration_)
if (use2ndOrderIntegration_)
F_pos_angles = 0.5 * F_vel_angles * deltaT;
else
F_pos_angles = Z_3x3;
// pos vel angle
*F << I_3x3, I_3x3 * deltaT, F_pos_angles, // pos
*F << //
I_3x3, I_3x3 * deltaT, F_pos_angles, // pos
Z_3x3, I_3x3, F_vel_angles, // vel
Z_3x3, Z_3x3, F_angles_angles;// angle
Z_3x3, Z_3x3, F_angles_angles; // angle
}
}
/// Update Jacobians to be used during preintegration
void updatePreintegratedJacobians(const Vector3& correctedAcc,
const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR, double deltaT){
const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
double deltaT) {
Matrix3 dRij = deltaRij(); // expensive
Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega();
Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT
* delRdelBiasOmega();
if (!use2ndOrderIntegration_) {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
} else {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT;
delPdelBiasOmega_ += deltaT*(delVdelBiasOmega_ + temp * 0.5);
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT
- 0.5 * dRij * deltaT * deltaT;
delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5);
}
delVdelBiasAcc_ += -dRij * deltaT;
delVdelBiasOmega_ += temp;
update_delRdelBiasOmega(D_Rincr_integratedOmega,incrR,deltaT);
update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
}
void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
@ -188,11 +221,13 @@ public:
// Then compensate for sensor-body displacement: we express the quantities
// (originally in the IMU frame) into the body frame
if(body_P_sensor){
if (body_P_sensor) {
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
correctedAcc = body_R_sensor * correctedAcc
- body_omega_body__cross * body_omega_body__cross
* body_P_sensor->translation().vector();
// linear acceleration vector in the body frame
}
}
@ -205,47 +240,51 @@ public:
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const {
const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
const imuBias::ConstantBias biasIncr = bias_i - biasHat();
const Vector3& biasAccIncr = biasIncr.accelerometer();
const Vector3& biasOmegaIncr = biasIncr.gyroscope();
const Rot3& Rot_i = pose_i.rotation();
const Vector3& pos_i = pose_i.translation().vector();
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr;
if(deltaPij_biascorrected_out)// if desired, store this
Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr
+ delPdelBiasOmega() * biasOmegaIncr;
if (deltaPij_biascorrected_out) // if desired, store this
*deltaPij_biascorrected_out = deltaPij_biascorrected;
Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected
+ vel_i * deltaTij()
- omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij()*deltaTij();
- omegaCoriolis.cross(vel_i) * deltaTij() * deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij() * deltaTij();
Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr;
if(deltaVij_biascorrected_out)// if desired, store this
Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr
+ delVdelBiasOmega() * biasOmegaIncr;
if (deltaVij_biascorrected_out) // if desired, store this
*deltaVij_biascorrected_out = deltaVij_biascorrected;
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected
Vector3 vel_j = Vector3(
vel_i + Rot_i.matrix() * deltaVij_biascorrected
- 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term
+ gravity * deltaTij());
if(use2ndOrderCoriolis){
pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij(); // 2nd order coriolis term for position
vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity
if (use2ndOrderCoriolis) {
pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i))
* deltaTij() * deltaTij(); // 2nd order coriolis term for position
vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity
}
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
// deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
Vector3 correctedOmega = biascorrectedOmega -
Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
const Rot3 correctedDeltaRij =
Rot3::Expmap( correctedOmega );
const Rot3 Rot_j = Rot_i.compose( correctedDeltaRij );
Vector3 correctedOmega = biascorrectedOmega
- Rot_i.inverse().matrix() * omegaCoriolis * deltaTij(); // Coriolis term
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
const Rot3 Rot_j = Rot_i.compose(correctedDeltaRij);
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
Pose3 pose_j = Pose3(Rot_j, Point3(pos_j));
return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
}
@ -255,14 +294,12 @@ public:
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis,
OptionalJacobian<9, 6> H1 = boost::none,
OptionalJacobian<9, 3> H2 = boost::none,
OptionalJacobian<9, 6> H3 = boost::none,
OptionalJacobian<9, 3> H4 = boost::none,
OptionalJacobian<9, 6> H5 = boost::none) const {
OptionalJacobian<9, 6> H1 = boost::none, OptionalJacobian<9, 3> H2 =
boost::none, OptionalJacobian<9, 6> H3 = boost::none,
OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 =
boost::none) const {
// We need the mismatch w.r.t. the biases used for preintegration
// const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary
const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
// we give some shorter name to rotations and translations
@ -274,56 +311,62 @@ public:
/* ---------------------------------------------------------------------------------------------------- */
Vector3 deltaPij_biascorrected, deltaVij_biascorrected;
PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected);
omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected,
deltaVij_biascorrected);
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() );
const Vector3 fp = Ri.transpose()
* (pos_j - predictedState_j.pose.translation().vector());
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity );
const Vector3 fv = Ri.transpose() * (vel_j - predictedState_j.velocity);
// fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j)
// Jacobian computation
/* ---------------------------------------------------------------------------------------------------- */
// Get Get so<3> version of bias corrected rotation
// If H5 is asked for, we will need the Jacobian, which we store in H5
// H5 will then be corrected below to take into account the Coriolis effect
Matrix3 D_cThetaRij_biasOmegaIncr;
Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0);
Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr,
H5 ? &D_cThetaRij_biasOmegaIncr : 0);
// Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis);
const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
Vector3 correctedOmega = biascorrectedOmega - coriolis;
Rot3 correctedDeltaRij, fRrot;
// Calculate Jacobians, matrices below needed only for some Jacobians...
Vector3 fR;
Rot3 correctedDeltaRij, fRrot;
Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot,
Ritranspose_omegaCoriolisHat;
// Accessory matrix, used to build the jacobians
Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot;
if (H1 || H2)
Ritranspose_omegaCoriolisHat = Ri.transpose()
* skewSymmetric(omegaCoriolis);
// This is done to save computation: we ask for the jacobians only when they are needed
if(H1 || H2 || H3 || H4 || H5){
correctedDeltaRij = Rot3::Expmap( correctedOmega, D_cDeltaRij_cOmega);
if (H1 || H2 || H3 || H4 || H5) {
correctedDeltaRij = Rot3::Expmap(correctedOmega, D_cDeltaRij_cOmega);
// Residual rotation error
fRrot = correctedDeltaRij.between(Ri.between(Rj));
fR = Rot3::Logmap(fRrot, D_fR_fRrot);
D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis);
}else{
correctedDeltaRij = Rot3::Expmap( correctedOmega);
} else {
correctedDeltaRij = Rot3::Expmap(correctedOmega);
// Residual rotation error
fRrot = correctedDeltaRij.between(Ri.between(Rj));
fR = Rot3::Logmap(fRrot);
}
if(H1) {
H1->resize(9,6);
if (H1) {
H1->resize(9, 6);
Matrix3 dfPdPi = -I_3x3;
Matrix3 dfVdPi = Z_3x3;
if(use2ndOrderCoriolis){
if (use2ndOrderCoriolis) {
// this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix()
Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose());
dfPdPi += 0.5 * temp * deltaTij()*deltaTij();
Matrix3 temp = Ritranspose_omegaCoriolisHat
* (-Ritranspose_omegaCoriolisHat.transpose());
dfPdPi += 0.5 * temp * deltaTij() * deltaTij();
dfVdPi += temp * deltaTij();
}
(*H1) <<
@ -336,34 +379,34 @@ public:
// dfV/dPi
dfVdPi,
// dfR/dRi
D_fR_fRrot * (- Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis),
D_fR_fRrot
* (-Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis),
// dfR/dPi
Z_3x3;
}
if(H2) {
H2->resize(9,3);
if (H2) {
H2->resize(9, 3);
(*H2) <<
// dfP/dVi
- Ri.transpose() * deltaTij()
-Ri.transpose() * deltaTij()
+ Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi
- Ri.transpose()
+ 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term
-Ri.transpose() + 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term
// dfR/dVi
Z_3x3;
}
if(H3) {
H3->resize(9,6);
if (H3) {
H3->resize(9, 6);
(*H3) <<
// dfP/dPosej
Z_3x3, Ri.transpose() * Rj.matrix(),
// dfV/dPosej
Matrix::Zero(3,6),
Matrix::Zero(3, 6),
// dfR/dPosej
D_fR_fRrot, Z_3x3;
}
if(H4) {
H4->resize(9,3);
if (H4) {
H4->resize(9, 3);
(*H4) <<
// dfP/dVj
Z_3x3,
@ -372,19 +415,20 @@ public:
// dfR/dVj
Z_3x3;
}
if(H5) {
if (H5) {
// H5 by this point already contains 3*3 biascorrectedThetaRij derivative
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr;
H5->resize(9,6);
H5->resize(9, 6);
(*H5) <<
// dfP/dBias
- delPdelBiasAcc(), - delPdelBiasOmega(),
-delPdelBiasAcc(), -delPdelBiasOmega(),
// dfV/dBias
- delVdelBiasAcc(), - delVdelBiasOmega(),
-delVdelBiasAcc(), -delVdelBiasOmega(),
// dfR/dBias
Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega);
Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega);
}
Vector9 r; r << fp, fv, fR;
Vector9 r;
r << fp, fv, fR;
return r;
}
@ -416,16 +460,23 @@ protected:
public:
ImuBase() :
gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)),
body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {}
gravity_(Vector3(0.0, 0.0, 9.81)), omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), body_P_sensor_(
boost::none), use2ndOrderCoriolis_(false) {
}
ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false) :
gravity_(gravity), omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {}
boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false) :
gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {
}
const Vector3& gravity() const { return gravity_; }
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
const Vector3& gravity() const {
return gravity_;
}
const Vector3& omegaCoriolis() const {
return omegaCoriolis_;
}
};

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

@ -44,36 +44,39 @@ namespace {
// Auxiliary functions to test Jacobians F and G used for
// covariance propagation during preintegration
/* ************************************************************************* */
Vector updatePreintegratedMeasurementsTest(
const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old,
const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT,
Vector updatePreintegratedMeasurementsTest(const Vector3 deltaPij_old,
const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration) {
Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT;
Vector3 deltaPij_new;
if(!use2ndOrderIntegration){
if (!use2ndOrderIntegration) {
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
}else{
} else {
deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
}
Vector3 deltaVij_new = deltaVij_old + temp;
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap((correctedOmega-bias_old.gyroscope()) * deltaT);
Rot3 deltaRij_new = deltaRij_old
* Rot3::Expmap((correctedOmega - bias_old.gyroscope()) * deltaT);
Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more
imuBias::ConstantBias bias_new(bias_old);
Vector result(15); result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector();
Vector result(15);
result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector();
return result;
}
Rot3 updatePreintegratedMeasurementsRot(
const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old,
const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration){
Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old,
const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration) {
Vector result = updatePreintegratedMeasurementsTest(deltaPij_old, deltaVij_old, deltaRij_old,
bias_old, correctedAcc, correctedOmega, deltaT, use2ndOrderIntegration);
Vector result = updatePreintegratedMeasurementsTest(deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega,
deltaT, use2ndOrderIntegration);
return Rot3::Expmap(result.segment<3>(6));
}
@ -82,60 +85,53 @@ Rot3 updatePreintegratedMeasurementsRot(
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
/* ************************************************************************* */
CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs){
CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, Matrix3::Identity(),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6,6), false);
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
CombinedImuFactor::CombinedPreintegratedMeasurements result(bias,
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6, 6), false);
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
list<double>::const_iterator itDeltaT = deltaTs.begin();
for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
for (; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
}
return result;
}
Vector3 evaluatePreintegratedMeasurementsPosition(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs){
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaPij();
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs).deltaPij();
}
Vector3 evaluatePreintegratedMeasurementsVelocity(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs){
return evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaVij();
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
return evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs).deltaVij();
}
Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs){
return Rot3(evaluatePreintegratedMeasurements(bias,
measuredAccs, measuredOmegas, deltaTs).deltaRij());
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
return Rot3(
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs).deltaRij());
}
}
/* ************************************************************************* */
TEST( CombinedImuFactor, PreintegratedMeasurements )
{
TEST( CombinedImuFactor, PreintegratedMeasurements ) {
// Linearization point
imuBias::ConstantBias bias(Vector3(0,0,0), Vector3(0,0,0)); ///< Current estimate of acceleration and angular rate biases
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
// Measurements
Vector3 measuredAcc(0.1, 0.0, 0.0);
Vector3 measuredOmega(M_PI/100.0, 0.0, 0.0);
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
double deltaT = 0.5;
double tol = 1e-6;
@ -145,58 +141,75 @@ TEST( CombinedImuFactor, PreintegratedMeasurements )
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));
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()), tol));
EXPECT(assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()), tol));
EXPECT(assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()), tol));
EXPECT(
assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()),
tol));
EXPECT(
assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()),
tol));
EXPECT(
assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()),
tol));
DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
}
/* ************************************************************************* */
TEST( CombinedImuFactor, ErrorWithBiases )
{
TEST( CombinedImuFactor, ErrorWithBiases ) {
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
Vector3 v1(0.5, 0.0, 0.0);
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
Point3(5.5, 1.0, -50.0));
Vector3 v2(0.5, 0.0, 0.0);
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector()
+ Vector3(0.2, 0.0, 0.0);
double deltaT = 1.0;
double tol = 1e-6;
Matrix I6x6(6,6);
I6x6 = Matrix::Identity(6,6);
Matrix I6x6(6, 6);
I6x6 = Matrix::Identity(6, 6);
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
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());
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 );
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6);
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega,
deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity,
omegaCoriolis);
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
noiseModel::Gaussian::shared_ptr Combinedmodel =
noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
Combined_pre_int_data, gravity, omegaCoriolis);
Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias);
Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2);
Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias,
bias2);
EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
@ -206,7 +219,8 @@ 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);
(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)));
@ -216,101 +230,128 @@ TEST( CombinedImuFactor, ErrorWithBiases )
}
/* ************************************************************************* */
TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
{
TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) {
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
// Measurements
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
for(int i=1;i<100;i++)
{
for (int i = 1; i < 100; i++) {
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
measuredOmegas.push_back(
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(0.01);
}
// Actual preintegrated values
CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs);
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs);
// Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelPdelBias = numericalDerivative11<Vector,
imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs,
measuredOmegas, deltaTs), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelVdelBias = numericalDerivative11<Vector,
imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs,
measuredOmegas, deltaTs), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
Matrix expectedDelRdelBias = numericalDerivative11<Rot3,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelRdelBias =
numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
measuredAccs, measuredOmegas, deltaTs), bias);
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
EXPECT(assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
EXPECT(
assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
EXPECT(
assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
EXPECT(
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}
/* ************************************************************************* */
TEST(CombinedImuFactor, PredictPositionAndVelocity){
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
TEST(CombinedImuFactor, PredictPositionAndVelocity) {
imuBias::ConstantBias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0)); // Biases (acc, rot)
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3;
Vector3 measuredAcc; measuredAcc << 0,1,-9.81;
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega;
measuredOmega << 0, 0.1, 0; //M_PI/10.0+0.3;
Vector3 measuredAcc;
measuredAcc << 0, 1.1, -9.81;
double deltaT = 0.001;
Matrix I6x6(6,6);
I6x6 = Matrix::Identity(6,6);
Matrix I6x6(6, 6);
I6x6 = Matrix::Identity(6, 6);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true );
bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true);
for (int i = 0; i<1000; ++i) Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
for (int i = 0; i < 1000; ++i)
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega,
deltaT);
// Create factor
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
noiseModel::Gaussian::shared_ptr Combinedmodel =
noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
Combined_pre_int_data, gravity, omegaCoriolis);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1,
bias, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity; expectedVelocity<<0,1,0;
Vector3 expectedVelocity;
expectedVelocity << 0, 1, 0;
EXPECT(assert_equal(expectedPose, poseVelocityBias.pose));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity)));
EXPECT(
assert_equal(Vector(expectedVelocity), Vector(poseVelocityBias.velocity)));
}
/* ************************************************************************* */
TEST(CombinedImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
Matrix I6x6(6,6);
Matrix I6x6(6, 6);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true );
Vector3 measuredAcc; measuredAcc << 0, 0, -9.81;
Vector3 gravity; gravity<<0,0,9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0,0,0;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0;
bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true);
Vector3 measuredAcc;
measuredAcc << 0, 0, -9.81;
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0;
double deltaT = 0.001;
double tol = 1e-4;
for (int i = 0; i < 1000; ++i)
@ -320,16 +361,16 @@ TEST(CombinedImuFactor, PredictRotation) {
Combined_pre_int_data, gravity, omegaCoriolis);
// Predict
Pose3 x(Rot3().ypr(0,0, 0), Point3(0,0,0)), x2;
Vector3 v(0,0,0), v2;
CombinedImuFactor::Predict(x, v, x2, v2, bias, Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI/10, 0,0), Point3(0,0,0));
Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2;
Vector3 v(0, 0, 0), v2;
CombinedImuFactor::Predict(x, v, x2, v2, bias,
Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
EXPECT(assert_equal(expectedPose, x2, tol));
}
/* ************************************************************************* */
TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
{
TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
// Linearization point
imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3();
@ -338,25 +379,26 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI/100.0, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
for(int i=1;i<100;i++)
{
for (int i = 1; i < 100; i++) {
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(Vector3(M_PI/100.0, M_PI/300.0, 2*M_PI/100.0));
measuredOmegas.push_back(
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(0.01);
}
// Actual preintegrated values
CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas, deltaTs);
evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas,
deltaTs);
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI/100.0, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0);
const double newDeltaT = 0.01;
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
@ -365,8 +407,8 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
Matrix oldPreintCovariance = preintegrated.preintMeasCov();
Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT,
body_P_sensor, Factual, Gactual);
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
newDeltaT, body_P_sensor, Factual, Gactual);
bool use2ndOrderIntegration = false;
@ -374,44 +416,51 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
// COMPUTE NUMERICAL DERIVATIVES FOR F
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected F wrt positions (15,3)
Matrix df_dpos =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
_1, deltaVij_old, deltaRij_old, bias_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
Matrix df_dpos = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, _1, deltaVij_old,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaPij_old);
// rotation part has to be done properly, on manifold
df_dpos.block<3,3>(6,0) = numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot,
_1, deltaVij_old, deltaRij_old, bias_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
df_dpos.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, _1, deltaVij_old,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaPij_old);
// Compute expected F wrt velocities (15,3)
Matrix df_dvel =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, _1, deltaRij_old, bias_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
Matrix df_dvel = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, _1,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaVij_old);
// rotation part has to be done properly, on manifold
df_dvel.block<3,3>(6,0) = numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot,
deltaPij_old, _1, deltaRij_old, bias_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
df_dvel.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, _1,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaVij_old);
// Compute expected F wrt angles (15,3)
Matrix df_dangle = numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, deltaVij_old, _1, bias_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
Matrix df_dangle = numericalDerivative11<Vector, Rot3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), deltaRij_old);
// rotation part has to be done properly, on manifold
df_dangle.block<3,3>(6,0) = numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedMeasurementsRot,
deltaPij_old, deltaVij_old, _1, bias_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
df_dangle.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), deltaRij_old);
// Compute expected F wrt biases (15,6)
Matrix df_dbias = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, deltaVij_old, deltaRij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
Matrix df_dbias = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
// rotation part has to be done properly, on manifold
df_dbias.block<3,6>(6,0) = numericalDerivative11<Rot3, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsRot,
deltaPij_old, deltaVij_old, deltaRij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
df_dbias.block<3, 6>(6, 0) =
numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
Matrix Fexpected(15,15);
Matrix Fexpected(15, 15);
Fexpected << df_dpos, df_dvel, df_dangle, df_dbias;
EXPECT(assert_equal(Fexpected, Factual));
@ -419,55 +468,65 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
// COMPUTE NUMERICAL DERIVATIVES FOR G
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected G wrt integration noise
Matrix df_dintNoise(15,3);
Matrix df_dintNoise(15, 3);
df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3;
// Compute expected G wrt acc noise (15,3)
Matrix df_daccNoise = numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, deltaVij_old, deltaRij_old, bias_old,
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
Matrix df_daccNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), newMeasuredAcc);
// rotation part has to be done properly, on manifold
df_daccNoise.block<3,3>(6,0) = numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot,
deltaPij_old, deltaVij_old, deltaRij_old, bias_old,
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), newMeasuredAcc);
// Compute expected G wrt gyro noise (15,3)
Matrix df_domegaNoise = numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, deltaVij_old, deltaRij_old, bias_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
Matrix df_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT,
use2ndOrderIntegration), newMeasuredOmega);
// rotation part has to be done properly, on manifold
df_domegaNoise.block<3,3>(6,0) = numericalDerivative11< Rot3, Vector3>(boost::bind(&updatePreintegratedMeasurementsRot,
deltaPij_old, deltaVij_old, deltaRij_old, bias_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT,
use2ndOrderIntegration), newMeasuredOmega);
// Compute expected G wrt bias random walk noise (15,6)
Matrix df_rwBias(15,6); // random walk on the bias does not appear in the first 9 entries
Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries
df_rwBias.setZero();
df_rwBias.block<6,6>(9,0) = eye(6);
df_rwBias.block<6, 6>(9, 0) = eye(6);
// Compute expected G wrt gyro noise (15,6)
Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsTest,
deltaPij_old, deltaVij_old, deltaRij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
// rotation part has to be done properly, on manifold
df_dinitBias.block<3,6>(6,0) = numericalDerivative11<Rot3, imuBias::ConstantBias>(boost::bind(&updatePreintegratedMeasurementsRot,
deltaPij_old, deltaVij_old, deltaRij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), bias_old);
df_dinitBias.block<6,6>(9,0) = Matrix::Zero(6,6); // only has to influence first 9 rows
df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11<Rot3,
imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
df_dinitBias.block<6, 6>(9, 0) = Matrix::Zero(6, 6); // only has to influence first 9 rows
Matrix Gexpected(15,21);
Matrix Gexpected(15, 21);
Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias;
EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() +
(1/newDeltaT) * Gactual * Gactual.transpose();
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance
* Factual.transpose() + (1 / newDeltaT) * Gactual * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

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

@ -21,23 +21,111 @@
using namespace std;
using namespace gtsam;
Vector biasAcc1(Vector3(0.2, -0.1, 0));
Vector biasGyro1(Vector3(0.1, -0.3, -0.2));
imuBias::ConstantBias bias1(biasAcc1, biasGyro1);
Vector biasAcc2(Vector3(0.1, 0.2, 0.04));
Vector biasGyro2(Vector3(-0.002, 0.005, 0.03));
imuBias::ConstantBias bias2(biasAcc2, biasGyro2);
/* ************************************************************************* */
TEST( ImuBias, Constructor)
{
Vector bias_acc(Vector3(0.1,0.2,0.4));
Vector bias_gyro(Vector3(-0.2, 0.5, 0.03));
TEST( ImuBias, Constructor) {
// Default Constructor
gtsam::imuBias::ConstantBias bias1;
imuBias::ConstantBias bias1;
// Acc + Gyro Constructor
gtsam::imuBias::ConstantBias bias2(bias_acc, bias_gyro);
imuBias::ConstantBias bias2(biasAcc2, biasGyro2);
// Copy Constructor
gtsam::imuBias::ConstantBias bias3(bias2);
imuBias::ConstantBias bias3(bias2);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
TEST( ImuBias, inverse) {
imuBias::ConstantBias biasActual = bias1.inverse();
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(-biasAcc1,
-biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, compose) {
imuBias::ConstantBias biasActual = bias2.compose(bias1);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
biasAcc1 + biasAcc2, biasGyro1 + biasGyro2);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, between) {
// p.between(q) == q - p
imuBias::ConstantBias biasActual = bias2.between(bias1);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
biasAcc1 - biasAcc2, biasGyro1 - biasGyro2);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, localCoordinates) {
Vector deltaActual = Vector(bias2.localCoordinates(bias1));
Vector deltaExpected = (imuBias::ConstantBias(biasAcc1 - biasAcc2,
biasGyro1 - biasGyro2)).vector();
EXPECT(assert_equal(deltaExpected, deltaActual));
}
/* ************************************************************************* */
TEST( ImuBias, retract) {
Vector6 delta;
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
imuBias::ConstantBias biasActual = bias2.retract(delta);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(
biasAcc2 + delta.block<3, 1>(0, 0), biasGyro2 + delta.block<3, 1>(3, 0));
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, Logmap) {
Vector deltaActual = bias2.Logmap(bias1);
Vector deltaExpected = bias1.vector();
EXPECT(assert_equal(deltaExpected, deltaActual));
}
/* ************************************************************************* */
TEST( ImuBias, Expmap) {
Vector6 delta;
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
imuBias::ConstantBias biasActual = bias2.Expmap(delta);
imuBias::ConstantBias biasExpected = imuBias::ConstantBias(delta);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, operatorSub) {
imuBias::ConstantBias biasActual = -bias1;
imuBias::ConstantBias biasExpected(-biasAcc1, -biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, operatorAdd) {
imuBias::ConstantBias biasActual = bias2 + bias1;
imuBias::ConstantBias biasExpected(biasAcc2 + biasAcc1,
biasGyro2 + biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST( ImuBias, operatorSubB) {
imuBias::ConstantBias biasActual = bias2 - bias1;
imuBias::ConstantBias biasExpected(biasAcc2 - biasAcc1,
biasGyro2 - biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

681
gtsam/navigation/tests/testImuFactor.cpp
View File

File diff suppressed because it is too large Load Diff