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

added ImuBase class

release/4.3a0
Luca 2014-12-03 19:35:21 -05:00
parent 7dd359ce5d
commit b9e96e4c6f
5 changed files with 34 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -160,7 +160,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
// CombinedImuFactor methods
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor() :
preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {}
ImuBase(), 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,12 +168,8 @@ 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),
preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity),
omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor),
use2ndOrderCoriolis_(use2ndOrderCoriolis){
}
ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
preintegratedMeasurements_(preintegratedMeasurements) {}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {

11
gtsam/navigation/CombinedImuFactor.h
View File

@ -70,7 +70,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> {
class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias>, public ImuBase{
public:
/** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
@ -152,11 +152,6 @@ private:
typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base;
CombinedPreintegratedMeasurements preintegratedMeasurements_;
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:
@ -207,10 +202,6 @@ public:
const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_; }
const Vector3& gravity() const { return gravity_; }
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
/** implement functions needed to derive from Factor */
/// vector of errors

10
gtsam/navigation/ImuFactor.cpp
View File

@ -140,7 +140,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// ImuFactor methods
//------------------------------------------------------------------------------
ImuFactor::ImuFactor() :
preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){}
ImuBase(), preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {}
//------------------------------------------------------------------------------
ImuFactor::ImuFactor(
@ -150,12 +150,8 @@ 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),
preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity),
omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor),
use2ndOrderCoriolis_(use2ndOrderCoriolis){
}
ImuBase(gravity, omegaCoriolis, body_P_sensor, use2ndOrderCoriolis),
preintegratedMeasurements_(preintegratedMeasurements) {}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {

11
gtsam/navigation/ImuFactor.h
View File

@ -63,7 +63,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> {
class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias>, public ImuBase {
public:
/**
@ -141,11 +141,6 @@ public:
typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base;
PreintegratedMeasurements preintegratedMeasurements_;
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:
@ -195,10 +190,6 @@ public:
const PreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_; }
const Vector3& gravity() const { return gravity_; }
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
/** implement functions needed to derive from Factor */
/// vector of errors

27
gtsam/navigation/PreintegrationBase.h
View File

@ -66,7 +66,7 @@ public:
deltaRij_(Rot3()), deltaTij_(0.0),
delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
delRdelBiasOmega_(Z_3x3) { }
delRdelBiasOmega_(Z_3x3) {}
/// Needed for testable
void print(const std::string& s) const {
@ -199,4 +199,29 @@ private:
}
};
class ImuBase {
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:
ImuBase() :
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) {}
const Vector3& gravity() const { return gravity_; }
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
};
} /// namespace gtsam