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

added ImuFactorBase. In the process of moving stuff to base class

release/4.3a0
Luca 2014-12-04 11:51:31 -05:00
parent b9e96e4c6f
commit 1992cb27ba
6 changed files with 110 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -160,7 +160,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// CombinedImuFactor methods
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor() :
ImuBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {}
ImuFactorBase(), preintegratedMeasurements_(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,
@ -168,7 +168,7 @@ CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_
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),
ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
preintegratedMeasurements_(preintegratedMeasurements) {}
//------------------------------------------------------------------------------
@ -186,12 +186,9 @@ void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter)
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ","
<< keyFormatter(this->key6()) << ")\n";
ImuFactorBase::print("");
preintegratedMeasurements_.print(" preintegrated measurements:");
cout << " gravity: [ " << gravity_.transpose() << " ]" << endl;
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
this->noiseModel_->print(" noise model: ");
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
//------------------------------------------------------------------------------
@ -199,9 +196,7 @@ bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) cons
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
&& ImuFactorBase::equals(*e, tol);
}
//------------------------------------------------------------------------------
@ -393,7 +388,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_
//------------------------------------------------------------------------------
PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const PreintegrationBase& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
const double& deltaTij = preintegratedMeasurements.deltaTij_;
@ -433,8 +428,7 @@ PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3&
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
return PoseVelocityBias(pose_j, vel_j, bias_i);
return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
}
} /// namespace gtsam

5
gtsam/navigation/CombinedImuFactor.h
View File

@ -24,6 +24,7 @@
/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/navigation/ImuFactorBase.h>
#include <gtsam/base/debug.h>
namespace gtsam {
@ -70,7 +71,7 @@ struct PoseVelocityBias {
* 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty
* and the preintegrated measurements uncertainty.
*/
class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias>, public ImuBase{
class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias>, public ImuFactorBase{
public:
/** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
@ -217,7 +218,7 @@ public:
/// predicted states from IMU
static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const PreintegrationBase& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
private:

29
gtsam/navigation/ImuFactor.cpp
View File

@ -41,7 +41,7 @@ ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
preintMeasCov_.setZero(9,9);
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
@ -140,7 +140,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// ImuFactor methods
//------------------------------------------------------------------------------
ImuFactor::ImuFactor() :
ImuBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {}
ImuFactorBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {}
//------------------------------------------------------------------------------
ImuFactor::ImuFactor(
@ -150,7 +150,7 @@ ImuFactor::ImuFactor(
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),
ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
ImuFactorBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
preintegratedMeasurements_(preintegratedMeasurements) {}
//------------------------------------------------------------------------------
@ -167,12 +167,9 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
<< keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << ","
<< keyFormatter(this->key5()) << ")\n";
ImuFactorBase::print("");
preintegratedMeasurements_.print(" preintegrated measurements:");
cout << " gravity: [ " << gravity_.transpose() << " ]" << endl;
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
this->noiseModel_->print(" noise model: ");
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
//------------------------------------------------------------------------------
@ -180,9 +177,7 @@ 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)
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
&& ImuFactorBase::equals(*e, tol);
}
//------------------------------------------------------------------------------
@ -333,14 +328,14 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const
}
//------------------------------------------------------------------------------
PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis)
{
PoseVelocityBias ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i,
const PreintegrationBase& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
const double& deltaTij = preintegratedMeasurements.deltaTij_;
const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
const Rot3 Rot_i = pose_i.rotation();
const Vector3 pos_i = pose_i.translation().vector();
@ -375,7 +370,7 @@ PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected );
Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
return PoseVelocity(pose_j, vel_j);
return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
}
} /// namespace gtsam

16
gtsam/navigation/ImuFactor.h
View File

@ -24,6 +24,7 @@
/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/navigation/ImuFactorBase.h>
#include <gtsam/base/debug.h>
namespace gtsam {
@ -47,11 +48,14 @@ namespace gtsam {
/**
* Struct to hold return variables by the Predict Function
*/
struct PoseVelocity {
struct PoseVelocityBias {
Pose3 pose;
Vector3 velocity;
PoseVelocity(const Pose3& _pose, const Vector3& _velocity) :
pose(_pose), velocity(_velocity) {
imuBias::ConstantBias bias;
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
const imuBias::ConstantBias _bias) :
pose(_pose), velocity(_velocity), bias(_bias) {
}
};
@ -63,7 +67,7 @@ struct PoseVelocity {
* Note that this factor does not force "temporal consistency" of the biases (which are usually
* slowly varying quantities), see also CombinedImuFactor for more details.
*/
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias>, public ImuBase {
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias>, public ImuFactorBase {
public:
/**
@ -202,8 +206,8 @@ public:
boost::optional<Matrix&> H5 = boost::none) const;
/// predicted states from IMU
static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias, const PreintegrationBase& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
private:

74
gtsam/navigation/ImuFactorBase.h Normal file
View File

@ -0,0 +1,74 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file PreintegrationBase.h
* @author Luca Carlone
* @author Stephen Williams
* @author Richard Roberts
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
**/
#pragma once
/* GTSAM includes */
#include <gtsam/navigation/ImuBias.h>
namespace gtsam {
class ImuFactorBase {
protected:
Vector3 gravity_;
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
public:
ImuFactorBase() :
gravity_(Vector3(0.0,0.0,9.81)), omegaCoriolis_(Vector3(0.0,0.0,0.0)),
body_P_sensor_(boost::none), use2ndOrderCoriolis_(false) {}
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) {}
const Vector3& gravity() const { return gravity_; }
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
/// Needed for testable
//------------------------------------------------------------------------------
void print(const std::string& s) const {
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]" << std::endl;
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
/// Needed for testable
//------------------------------------------------------------------------------
bool equals(const ImuFactorBase& expected, double tol) const {
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_)));
}
};
} /// namespace gtsam

10
gtsam/navigation/PreintegrationBase.h
View File

@ -81,11 +81,11 @@ public:
/// Needed for testable
bool equals(const PreintegrationBase& expected, double tol) const {
return biasHat_.equals(expected.biasHat_, tol)
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
&& deltaRij_.equals(expected.deltaRij_, tol)
&& fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
&& deltaRij_.equals(expected.deltaRij_, tol)
&& fabs(deltaTij_ - expected.deltaTij_) < 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)