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

94
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,9 +55,12 @@ 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)
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);
@ -70,15 +74,16 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){
//------------------------------------------------------------------------------
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
@ -121,16 +127,17 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// 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>(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();
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;
@ -144,10 +151,11 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
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 {
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)
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
&& ImuFactorBase::equals(*e, tol);
}

73
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:
/**
@ -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,7 +203,8 @@ 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;
@ -198,46 +212,51 @@ public:
/** 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;

55
gtsam/navigation/ImuBias.h
View File

@ -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();
@ -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,20 +162,34 @@ 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:
@ -178,8 +200,7 @@ namespace imuBias {
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version)
{
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_);
@ -187,7 +208,8 @@ namespace imuBias {
/// @}
}; // ConstantBias class
};
// ConstantBias class
}// namespace imuBias
template<>
@ -197,4 +219,3 @@ struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
} // namespace gtsam

63
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,7 +48,8 @@ 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);
}
@ -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
@ -91,13 +93,16 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// 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>(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()) {
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();
@ -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: ");
@ -158,8 +162,7 @@ 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)
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
&& ImuFactorBase::equals(*e, tol);
}

66
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:
/**
@ -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:
@ -131,7 +139,8 @@ public:
private:
typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base;
typedef NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> Base;
PreintegratedMeasurements _PIM_;
@ -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,7 +184,8 @@ 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;
@ -181,26 +193,30 @@ public:
/** 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;
}
@ -211,14 +227,16 @@ public:
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;

32
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,20 +52,28 @@ 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;
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_)));
}
};

44
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,15 +72,18 @@ 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
@ -90,10 +104,11 @@ 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
@ -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_;

201
gtsam/navigation/PreintegrationBase.h
View File

@ -72,52 +72,80 @@ 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
@ -145,7 +173,8 @@ public:
deltaVij_ += temp;
Matrix3 R_i, F_angles_angles;
if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
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) {
@ -157,7 +186,8 @@ public:
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
}
@ -165,14 +195,17 @@ public:
/// 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;
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT
- 0.5 * dRij * deltaT * deltaT;
delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5);
}
delVdelBiasAcc_ += -dRij * deltaT;
@ -192,7 +225,9 @@ public:
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,15 +240,17 @@ 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;
Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr
+ delPdelBiasOmega() * biasOmegaIncr;
if (deltaPij_biascorrected_out) // if desired, store this
*deltaPij_biascorrected_out = deltaPij_biascorrected;
@ -222,16 +259,19 @@ public:
- 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;
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
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
}
@ -239,10 +279,9 @@ public:
// 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 );
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));
@ -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,34 +311,39 @@ 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);
// 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) {
@ -322,7 +364,8 @@ public:
Matrix3 dfVdPi = Z_3x3;
if (use2ndOrderCoriolis) {
// this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix()
Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose());
Matrix3 temp = Ritranspose_omegaCoriolisHat
* (-Ritranspose_omegaCoriolisHat.transpose());
dfPdPi += 0.5 * temp * deltaTij() * deltaTij();
dfVdPi += temp * deltaTij();
}
@ -336,7 +379,8 @@ 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;
}
@ -347,8 +391,7 @@ public:
-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;
}
@ -384,7 +427,8 @@ public:
// dfR/dBias
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_;
}
};

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

@ -44,10 +44,10 @@ 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();
@ -59,21 +59,24 @@ Vector updatePreintegratedMeasurementsTest(
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,
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,12 +85,11 @@ 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();
@ -99,37 +101,31 @@ CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasur
}
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
@ -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));
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);
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,8 +230,7 @@ TEST( CombinedImuFactor, ErrorWithBiases )
}
/* ************************************************************************* */
TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
{
TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) {
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
@ -232,73 +245,96 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
measuredAccs.push_back(Vector3(0.1, 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(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(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)
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);
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)));
}
/* ************************************************************************* */
@ -306,11 +342,16 @@ TEST(CombinedImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
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)
@ -322,14 +363,14 @@ TEST(CombinedImuFactor, PredictRotation) {
// 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);
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();
@ -343,15 +384,16 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
measuredAccs.push_back(Vector3(0.1, 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
@ -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,42 +416,49 @@ 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);
Fexpected << df_dpos, df_dvel, df_dangle, df_dbias;
@ -423,22 +472,26 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
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
@ -446,13 +499,16 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
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<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);
@ -461,13 +517,16 @@ TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation )
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);
}
/* ************************************************************************* */

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

@ -39,25 +39,26 @@ using symbol_shorthand::B;
namespace {
// Auxiliary functions to test evaluate error in ImuFactor
/* ************************************************************************* */
Vector callEvaluateError(const ImuFactor& factor,
const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
Vector callEvaluateError(const ImuFactor& factor, const Pose3& pose_i,
const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias) {
return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias);
}
Rot3 evaluateRotationError(const ImuFactor& factor,
const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias) {
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
return Rot3::Expmap(
factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3));
}
// Auxiliary functions to test Jacobians F and G used for
// covariance propagation during preintegration
/* ************************************************************************* */
Vector updatePreintegratedPosVel(
const Vector3 deltaPij_old, const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const Vector3& correctedAcc, const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration_) {
Vector updatePreintegratedPosVel(const Vector3 deltaPij_old,
const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const Vector3& correctedAcc, const Vector3& correctedOmega,
const double deltaT, const bool use2ndOrderIntegration_) {
Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * correctedAcc * deltaT;
@ -69,7 +70,8 @@ Vector updatePreintegratedPosVel(
}
Vector3 deltaVij_new = deltaVij_old + temp;
Vector result(6); result << deltaPij_new, deltaVij_new;
Vector result(6);
result << deltaPij_new, deltaVij_new;
return result;
}
@ -86,13 +88,12 @@ double accNoiseVar = 0.01;
double omegaNoiseVar = 0.03;
double intNoiseVar = 0.0001;
ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs,
const bool use2ndOrderIntegration = false) {
ImuFactor::PreintegratedMeasurements result(bias, accNoiseVar * Matrix3::Identity(),
omegaNoiseVar *Matrix3::Identity(), intNoiseVar * Matrix3::Identity(),use2ndOrderIntegration);
ImuFactor::PreintegratedMeasurements result(bias,
accNoiseVar * Matrix3::Identity(), omegaNoiseVar * Matrix3::Identity(),
intNoiseVar * Matrix3::Identity(), use2ndOrderIntegration);
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@ -104,34 +105,29 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
}
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());
}
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
const double deltaT) {
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
}
@ -142,8 +138,7 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){
}
/* ************************************************************************* */
TEST( ImuFactor, PreintegratedMeasurements )
{
TEST( ImuFactor, PreintegratedMeasurements ) {
// Linearization point
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
@ -153,24 +148,30 @@ TEST( ImuFactor, PreintegratedMeasurements )
double deltaT = 0.5;
// Expected preintegrated values
Vector3 expectedDeltaP1; expectedDeltaP1 << 0.5*0.1*0.5*0.5, 0, 0;
Vector3 expectedDeltaP1;
expectedDeltaP1 << 0.5 * 0.1 * 0.5 * 0.5, 0, 0;
Vector3 expectedDeltaV1(0.05, 0.0, 0.0);
Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT1(0.5);
bool use2ndOrderIntegration = true;
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6));
EXPECT(
assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij()), 1e-6));
EXPECT(
assert_equal(Vector(expectedDeltaV1), Vector(actual1.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
// Integrate again
Vector3 expectedDeltaP2; expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5*0.1*0.5*0.5, 0, 0;
Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0) + expectedDeltaR1.matrix() * measuredAcc * 0.5;
Vector3 expectedDeltaP2;
expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0;
Vector3 expectedDeltaV2 = Vector3(0.05, 0.0, 0.0)
+ expectedDeltaR1.matrix() * measuredAcc * 0.5;
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT2(1);
@ -178,39 +179,48 @@ TEST( ImuFactor, PreintegratedMeasurements )
ImuFactor::PreintegratedMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6));
EXPECT(assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6));
EXPECT(
assert_equal(Vector(expectedDeltaP2), Vector(actual2.deltaPij()), 1e-6));
EXPECT(
assert_equal(Vector(expectedDeltaV2), Vector(actual2.deltaVij()), 1e-6));
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
}
/* ************************************************************************* */
TEST( ImuFactor, ErrorAndJacobians )
{
TEST( ImuFactor, ErrorAndJacobians ) {
// Linearization point
imuBias::ConstantBias bias; // Bias
Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
Point3(5.0, 1.0, -50.0));
Vector3 v1(Vector3(0.5, 0.0, 0.0));
Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
Pose3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0),
Point3(5.5, 1.0, -50.0));
Vector3 v2(Vector3(0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0;
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega;
measuredOmega << M_PI / 100, 0, 0;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector();
double deltaT = 1.0;
bool use2ndOrderIntegration = true;
ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
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);
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
// Expected error
Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
Vector errorExpected(9);
errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Actual Jacobians
@ -253,27 +263,34 @@ TEST( ImuFactor, ErrorAndJacobians )
}
/* ************************************************************************* */
TEST( ImuFactor, ErrorAndJacobianWithBiases )
{
TEST( ImuFactor, ErrorAndJacobianWithBiases ) {
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0));
Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 10.0),
Point3(5.0, 1.0, -50.0));
Vector3 v1(Vector3(0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
Point3(5.5, 1.0, -50.0));
Vector3 v2(Vector3(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;
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
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);
SETDEBUG("ImuFactor evaluateError", false);
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
@ -316,29 +333,36 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases )
}
/* ************************************************************************* */
TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis )
{
TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis ) {
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0));
Pose3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 10.0),
Point3(5.0, 1.0, -50.0));
Vector3 v1(Vector3(0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 10.0 + M_PI / 10.0)),
Point3(5.5, 1.0, -50.0));
Vector3 v2(Vector3(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;
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
Pose3 bodyPsensor = Pose3();
bool use2ndOrderCoriolis = true;
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity,
omegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
SETDEBUG("ImuFactor evaluateError", false);
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
@ -381,20 +405,23 @@ TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis )
}
/* ************************************************************************* */
TEST( ImuFactor, PartialDerivative_wrt_Bias )
{
TEST( ImuFactor, PartialDerivative_wrt_Bias ) {
// Linearization point
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
Vector3 biasOmega;
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
// Measurements
Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
Vector3 measuredOmega;
measuredOmega << 0.1, 0, 0;
double deltaT = 0.5;
// Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind(
&evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega));
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotation, measuredOmega, _1, deltaT),
Vector3(biasOmega));
const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
const Matrix3 Jr = Rot3::ExpmapDerivative(
(measuredOmega - biasOmega) * deltaT);
Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
@ -403,17 +430,18 @@ TEST( ImuFactor, PartialDerivative_wrt_Bias )
}
/* ************************************************************************* */
TEST( ImuFactor, PartialDerivativeLogmap )
{
TEST( ImuFactor, PartialDerivativeLogmap ) {
// Linearization point
Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias
Vector3 thetahat;
thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias
// Measurements
Vector3 deltatheta; deltatheta << 0, 0, 0;
Vector3 deltatheta;
deltatheta << 0, 0, 0;
// Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<Vector,Vector3>(boost::bind(
&evaluateLogRotation, thetahat, _1), Vector3(deltatheta));
Matrix expectedDelFdeltheta = numericalDerivative11<Vector, Vector3>(
boost::bind(&evaluateLogRotation, thetahat, _1), Vector3(deltatheta));
Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat);
@ -422,28 +450,33 @@ TEST( ImuFactor, PartialDerivativeLogmap )
}
/* ************************************************************************* */
TEST( ImuFactor, fistOrderExponential )
{
TEST( ImuFactor, fistOrderExponential ) {
// Linearization point
Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
Vector3 biasOmega;
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
// Measurements
Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
Vector3 measuredOmega;
measuredOmega << 0.1, 0, 0;
double deltaT = 1.0;
// change w.r.t. linearization point
double alpha = 0.0;
Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha;
Vector3 deltabiasOmega;
deltabiasOmega << alpha, alpha, alpha;
const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
const Matrix3 Jr = Rot3::ExpmapDerivative(
(measuredOmega - biasOmega) * deltaT);
Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
const Matrix expectedRot = Rot3::Expmap(
(measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
const Matrix3 actualRot =
hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
const Matrix3 hatRot =
Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
const Matrix3 actualRot = hatRot
* Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
//hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
// This is a first order expansion so the equality is only an approximation
@ -451,8 +484,7 @@ TEST( ImuFactor, fistOrderExponential )
}
/* ************************************************************************* */
TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
{
TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) {
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
@ -467,45 +499,55 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
measuredAccs.push_back(Vector3(0.1, 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
ImuFactor::PreintegratedMeasurements 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(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(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}
/* ************************************************************************* */
TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
{
TEST( ImuFactor, JacobianPreintegratedCovariancePropagation ) {
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
@ -519,16 +561,17 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
measuredAccs.push_back(Vector3(0.1, 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);
}
bool use2ndOrderIntegration = false;
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration);
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs, use2ndOrderIntegration);
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
@ -542,40 +585,42 @@ TEST( ImuFactor, 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);
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected f_pos_vel wrt positions
Matrix dfpv_dpos =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
_1, deltaVij_old, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaPij_old);
// Compute expected f_pos_vel wrt velocities
Matrix dfpv_dvel =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, _1, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaVij_old);
// Compute expected f_pos_vel wrt angles
Matrix dfpv_dangle =
numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaRij_old);
Matrix FexpectedTop6(6,9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
Matrix FexpectedTop6(6, 9);
FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
// Compute expected f_rot wrt angles
Matrix dfr_dangle =
numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedRot,
_1, newMeasuredOmega, newDeltaT), deltaRij_old);
Matrix dfr_dangle = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT),
deltaRij_old);
Matrix FexpectedBottom3(3, 9);
FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3;
Matrix Fexpected(9, 9);
Fexpected << FexpectedTop6, FexpectedBottom3;
EXPECT(assert_equal(Fexpected, Factual));
@ -587,46 +632,46 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation )
dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3;
// Compute jacobian wrt acc noise
Matrix dgpv_daccNoise =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, deltaRij_old,
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, _1, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), newMeasuredAcc);
// Compute expected F wrt gyro noise
Matrix dgpv_domegaNoise =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, deltaRij_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration),
newMeasuredOmega);
Matrix GexpectedTop6(6, 9);
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
// Compute expected f_rot wrt gyro noise
Matrix dgr_dangle =
numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedRot,
deltaRij_old, _1, newDeltaT), newMeasuredOmega);
Matrix dgr_dangle = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT),
newMeasuredOmega);
Matrix GexpectedBottom3(3, 9);
GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3;
Matrix Gexpected(9, 9);
Gexpected << GexpectedTop6, GexpectedBottom3;
EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation
Matrix9 measurementCovariance;
measurementCovariance << intNoiseVar*I_3x3, Z_3x3, Z_3x3,
Z_3x3, accNoiseVar*I_3x3, Z_3x3,
Z_3x3, Z_3x3, omegaNoiseVar*I_3x3;
measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar
* I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3;
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() +
(1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance
* Factual.transpose()
+ (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
}
/* ************************************************************************* */
TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt )
{
TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt ) {
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3(); // (Rot3::Expmap(Vector3(0,0.1,0.1)), Point3(1, 0, 1));
@ -640,16 +685,17 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt )
measuredAccs.push_back(Vector3(0.1, 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);
}
bool use2ndOrderIntegration = true;
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, use2ndOrderIntegration);
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs, use2ndOrderIntegration);
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
@ -663,40 +709,42 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt )
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);
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected f_pos_vel wrt positions
Matrix dfpv_dpos =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
_1, deltaVij_old, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaPij_old);
Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaPij_old);
// Compute expected f_pos_vel wrt velocities
Matrix dfpv_dvel =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, _1, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaVij_old);
Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaVij_old);
// Compute expected f_pos_vel wrt angles
Matrix dfpv_dangle =
numericalDerivative11<Vector, Rot3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), deltaRij_old);
Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT, use2ndOrderIntegration),
deltaRij_old);
Matrix FexpectedTop6(6,9); FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
Matrix FexpectedTop6(6, 9);
FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
// Compute expected f_rot wrt angles
Matrix dfr_dangle =
numericalDerivative11<Rot3, Rot3>(boost::bind(&updatePreintegratedRot,
_1, newMeasuredOmega, newDeltaT), deltaRij_old);
Matrix dfr_dangle = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT),
deltaRij_old);
Matrix FexpectedBottom3(3, 9);
FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
Matrix Fexpected(9,9); Fexpected << FexpectedTop6, FexpectedBottom3;
Matrix Fexpected(9, 9);
Fexpected << FexpectedTop6, FexpectedBottom3;
EXPECT(assert_equal(Fexpected, Factual));
@ -708,38 +756,39 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt )
dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3;
// Compute jacobian wrt acc noise
Matrix dgpv_daccNoise =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, deltaRij_old,
_1, newMeasuredOmega, newDeltaT, use2ndOrderIntegration), newMeasuredAcc);
Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, _1, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), newMeasuredAcc);
// Compute expected F wrt gyro noise
Matrix dgpv_domegaNoise =
numericalDerivative11<Vector, Vector3>(boost::bind(&updatePreintegratedPosVel,
deltaPij_old, deltaVij_old, deltaRij_old,
newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration), newMeasuredOmega);
Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, newMeasuredAcc, _1, newDeltaT, use2ndOrderIntegration),
newMeasuredOmega);
Matrix GexpectedTop6(6, 9);
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
// Compute expected f_rot wrt gyro noise
Matrix dgr_dangle =
numericalDerivative11<Rot3, Vector3>(boost::bind(&updatePreintegratedRot,
deltaRij_old, _1, newDeltaT), newMeasuredOmega);
Matrix dgr_dangle = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT),
newMeasuredOmega);
Matrix GexpectedBottom3(3, 9);
GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
Matrix Gexpected(9,9); Gexpected << GexpectedTop6, GexpectedBottom3;
Matrix Gexpected(9, 9);
Gexpected << GexpectedTop6, GexpectedBottom3;
EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation
Matrix9 measurementCovariance;
measurementCovariance << intNoiseVar*I_3x3, Z_3x3, Z_3x3,
Z_3x3, accNoiseVar*I_3x3, Z_3x3,
Z_3x3, Z_3x3, omegaNoiseVar*I_3x3;
measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar
* I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3;
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance * Factual.transpose() +
(1/newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance
* Factual.transpose()
+ (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
@ -801,31 +850,38 @@ TEST( ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt )
//}
/* ************************************************************************* */
TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
{
TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
Vector3 v1(Vector3(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(Vector3(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;
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0));
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
Point3(1, 0, 0));
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
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);
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Pose3>(
@ -863,29 +919,38 @@ TEST(ImuFactor, PredictPositionAndVelocity){
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 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, 0; //M_PI/10.0+0.3;
Vector3 measuredAcc;
measuredAcc << 0, 1, -9.81;
double deltaT = 0.001;
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)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
for (int i = 0; i < 1000; ++i)
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);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocity = pre_int_data.predict(x1, v1, bias, gravity, omegaCoriolis);
PoseVelocityBias poseVelocity = 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, poseVelocity.pose));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(poseVelocity.velocity)));
}
@ -895,34 +960,46 @@ TEST(ImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10;//M_PI/10.0+0.3;
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; //M_PI/10.0+0.3;
Vector3 measuredAcc;
measuredAcc << 0, 0, -9.81;
double deltaT = 0.001;
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)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
ImuFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), true);
for (int i = 0; i<1000; ++i) pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
for (int i = 0; i < 1000; ++i)
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);
// Predict
Pose3 x1, x2;
Vector3 v1 = Vector3(0, 0.0, 0.0);
Vector3 v2;
ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(), gravity, omegaCoriolis);
ImuFactor::Predict(x1, v1, x2, v2, bias, factor.preintegratedMeasurements(),
gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
Vector3 expectedVelocity; expectedVelocity<<0,0,0;
Vector3 expectedVelocity;
expectedVelocity << 0, 0, 0;
EXPECT(assert_equal(expectedPose, x2));
EXPECT(assert_equal(Vector(expectedVelocity), Vector(v2)));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */