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

Possibly controversial (sorry @lucacarlone ) name change to make it easier to see copy/paste patterns.

release/4.3a0
dellaert 2014-12-05 12:36:14 +01:00
parent ac8e4d2536
commit 8bfe4d75fb
8 changed files with 53 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/navigation/AHRSFactor.cpp
View File

@ -148,7 +148,7 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles(
// AHRSFactor methods
//------------------------------------------------------------------------------
AHRSFactor::AHRSFactor() :
preintegratedMeasurements_(Vector3(), Matrix3::Zero()) {
_PIM_(Vector3(), Matrix3::Zero()) {
}
AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
@ -156,7 +156,7 @@ AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) :
Base(
noiseModel::Gaussian::Covariance(
preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_(
preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_(
preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
body_P_sensor) {
}
@ -172,7 +172,7 @@ void AHRSFactor::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ",";
preintegratedMeasurements_.print(" preintegrated measurements:");
_PIM_.print(" preintegrated measurements:");
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
noiseModel_->print(" noise model: ");
if (body_P_sensor_)
@ -183,7 +183,7 @@ void AHRSFactor::print(const string& s,
bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
const This *e = dynamic_cast<const This*>(&other);
return e != NULL && Base::equals(*e, tol)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
&& _PIM_.equals(e->_PIM_, tol)
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_)
|| (body_P_sensor_ && e->body_P_sensor_
@ -197,11 +197,11 @@ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j,
// Do bias correction, if (H3) will contain 3*3 derivative used below
const Vector3 theta_biascorrected = //
preintegratedMeasurements_.predict(bias, H3);
_PIM_.predict(bias, H3);
// Coriolis term
const Vector3 coriolis = //
preintegratedMeasurements_.integrateCoriolis(rot_i, omegaCoriolis_);
_PIM_.integrateCoriolis(rot_i, omegaCoriolis_);
const Vector3 theta_corrected = theta_biascorrected - coriolis;
// Prediction

6
gtsam/navigation/AHRSFactor.h
View File

@ -119,7 +119,7 @@ private:
typedef AHRSFactor This;
typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base;
PreintegratedMeasurements preintegratedMeasurements_;
PreintegratedMeasurements _PIM_;
Vector3 gravity_;
Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
@ -165,7 +165,7 @@ public:
/// Access the preintegrated measurements.
const PreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_;
return _PIM_;
}
const Vector3& omegaCoriolis() const {
@ -195,7 +195,7 @@ private:
ar
& boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}

12
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -158,7 +158,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// CombinedImuFactor methods
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor() :
ImuFactorBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {}
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {}
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
@ -167,7 +167,7 @@ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_
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),
preintegratedMeasurements_(preintegratedMeasurements) {}
_PIM_(preintegratedMeasurements) {}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
@ -185,7 +185,7 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter)
<< keyFormatter(this->key5()) << ","
<< keyFormatter(this->key6()) << ")\n";
ImuFactorBase::print("");
preintegratedMeasurements_.print(" preintegrated measurements:");
_PIM_.print(" preintegrated measurements:");
this->noiseModel_->print(" noise model: ");
}
@ -193,7 +193,7 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter)
bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
&& _PIM_.equals(e->_PIM_, tol)
&& ImuFactorBase::equals(*e, tol);
}
@ -209,7 +209,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
Matrix H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR, Hbias_i, Hbias_j; // pvR = mnemonic: position (p), velocity (v), rotation (R)
// error wrt preintegrated measurements
Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i,
Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i,
H1_pvR, H2_pvR, H3_pvR, H4_pvR, H5_pvR);
// error wrt bias evolution model (random walk)
@ -256,7 +256,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
}
// else, only compute the error vector:
// error wrt preintegrated measurements
Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i,
Vector r_pvR(9); r_pvR << ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i,
boost::none, boost::none, boost::none, boost::none, boost::none);
// error wrt bias evolution model (random walk)
Vector6 fbias = bias_j.between(bias_i).vector(); // [bias_j.acc - bias_i.acc; bias_j.gyr - bias_i.gyr]

6
gtsam/navigation/CombinedImuFactor.h
View File

@ -149,7 +149,7 @@ private:
typedef CombinedImuFactor This;
typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base;
CombinedPreintegratedMeasurements preintegratedMeasurements_;
CombinedPreintegratedMeasurements _PIM_;
public:
@ -198,7 +198,7 @@ public:
/** Access the preintegrated measurements. */
const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_; }
return _PIM_; }
/** implement functions needed to derive from Factor */
@ -228,7 +228,7 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor6",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);

10
gtsam/navigation/ImuFactor.cpp
View File

@ -139,7 +139,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// ImuFactor methods
//------------------------------------------------------------------------------
ImuFactor::ImuFactor() :
ImuFactorBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {}
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {}
//------------------------------------------------------------------------------
ImuFactor::ImuFactor(
@ -150,7 +150,7 @@ ImuFactor::ImuFactor(
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),
preintegratedMeasurements_(preintegratedMeasurements) {}
_PIM_(preintegratedMeasurements) {}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
@ -167,7 +167,7 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ")\n";
ImuFactorBase::print("");
preintegratedMeasurements_.print(" preintegrated measurements:");
_PIM_.print(" preintegrated measurements:");
this->noiseModel_->print(" noise model: ");
}
@ -175,7 +175,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)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
&& _PIM_.equals(e->_PIM_, tol)
&& ImuFactorBase::equals(*e, tol);
}
@ -186,7 +186,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
boost::optional<Matrix&> H5) const{
return ImuFactorBase::computeErrorAndJacobians(preintegratedMeasurements_, pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5);
return ImuFactorBase::computeErrorAndJacobians(_PIM_, pose_i, vel_i, pose_j, vel_j, bias_i, H1, H2, H3, H4, H5);
}
} /// namespace gtsam

6
gtsam/navigation/ImuFactor.h
View File

@ -136,7 +136,7 @@ public:
typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base;
PreintegratedMeasurements preintegratedMeasurements_;
PreintegratedMeasurements _PIM_;
public:
@ -184,7 +184,7 @@ public:
/** Access the preintegrated measurements. */
const PreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_; }
return _PIM_; }
/** implement functions needed to derive from Factor */
@ -212,7 +212,7 @@ public:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor5",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);

50
gtsam/navigation/ImuFactorBase.h
View File

@ -95,14 +95,14 @@ public:
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
//------------------------------------------------------------------------------
Vector computeErrorAndJacobians(const PreintegrationBase& preintegratedMeasurements_, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
Vector computeErrorAndJacobians(const PreintegrationBase& _PIM, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3, boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const{
const double& deltaTij = preintegratedMeasurements_.deltaTij();
const double& deltaTij = _PIM.deltaTij();
// We need the mistmatch w.r.t. the biases used for preintegration
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.biasHat().gyroscope();
// we give some shorter name to rotations and translations
const Rot3& Rot_i = pose_i.rotation();
@ -115,7 +115,7 @@ public:
// 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
Vector3 theta_biascorrected =
preintegratedMeasurements_.biascorrectedThetaRij(biasOmegaIncr, H5);
_PIM.biascorrectedThetaRij(biasOmegaIncr, H5);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -
Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
@ -144,13 +144,13 @@ public:
}
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
Rot_i.matrix() * skewSymmetric(_PIM.deltaPij()
+ _PIM.delPdelBiasOmega() * biasOmegaIncr + _PIM.delPdelBiasAcc() * biasAccIncr),
// dfP/dPi
dfPdPi,
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
Rot_i.matrix() * skewSymmetric(_PIM.deltaVij()
+ _PIM.delVdelBiasOmega() * biasOmegaIncr + _PIM.delVdelBiasAcc() * biasAccIncr),
// dfV/dPi
dfVdPi,
// dfR/dRi
@ -196,11 +196,11 @@ public:
H5->resize(9,6);
(*H5) <<
// dfP/dBias
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
- Rot_i.matrix() * _PIM.delPdelBiasAcc(),
- Rot_i.matrix() * _PIM.delPdelBiasOmega(),
// dfV/dBias
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
- Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
- Rot_i.matrix() * _PIM.delVdelBiasAcc(),
- Rot_i.matrix() * _PIM.delVdelBiasOmega(),
// dfR/dBias
Matrix::Zero(3,3),
Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
@ -208,7 +208,7 @@ public:
// Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
PoseVelocityBias predictedState_j = ImuFactorBase::predict(pose_i, vel_i, bias_i, preintegratedMeasurements_,
PoseVelocityBias predictedState_j = ImuFactorBase::predict(pose_i, vel_i, bias_i, _PIM,
gravity_, omegaCoriolis_, use2ndOrderCoriolis_);
const Vector3 fp = pos_j - predictedState_j.pose.translation().vector();
@ -226,28 +226,28 @@ public:
//------------------------------------------------------------------------------
static PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i,
const PreintegrationBase& preintegratedMeasurements,
const PreintegrationBase& _PIM,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
const double& deltaTij = preintegratedMeasurements.deltaTij();
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
const double& deltaTij = _PIM.deltaTij();
const Vector3 biasAccIncr = bias_i.accelerometer() - _PIM.biasHat().accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - _PIM.biasHat().gyroscope();
const Rot3& Rot_i = pose_i.rotation();
const Vector3& pos_i = pose_i.translation().vector();
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
+ preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
Vector3 pos_j = pos_i + Rot_i.matrix() * (_PIM.deltaPij()
+ _PIM.delPdelBiasAcc() * biasAccIncr
+ _PIM.delPdelBiasOmega() * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
+ preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (_PIM.deltaVij()
+ _PIM.delVdelBiasAcc() * biasAccIncr
+ _PIM.delVdelBiasOmega() * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
@ -256,7 +256,7 @@ public:
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
}
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.biascorrectedDeltaRij(biasOmegaIncr);
const Rot3 deltaRij_biascorrected = _PIM.biascorrectedDeltaRij(biasOmegaIncr);
// TODO Frank says comment below does not reflect what was in code
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)

8
gtsam/navigation/PreintegrationBase.h
View File

@ -34,11 +34,6 @@ namespace gtsam {
*/
class PreintegrationBase : public PreintegratedRotation {
friend class ImuFactorBase;
friend class ImuFactor;
friend class CombinedImuFactor;
protected:
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
bool use2ndOrderIntegration_; ///< Controls the order of integration
@ -67,7 +62,8 @@ public:
/// methods to access class variables
const Vector3& deltaPij() const {return deltaPij_;}
const Vector3& deltaVij() const {return deltaVij_;}
Vector biasHat() const { return biasHat_.vector();} // TODO expensive
const imuBias::ConstantBias& biasHat() const { return biasHat_;}
Vector biasHatVector() const { return biasHat_.vector();} // expensive
const Matrix3& delPdelBiasAcc() const { return delPdelBiasAcc_;}
const Matrix3& delPdelBiasOmega() const { return delPdelBiasOmega_;}
const Matrix3& delVdelBiasAcc() const { return delVdelBiasAcc_;}