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

MakeParams

release/4.3a0
dellaert 2015-07-19 21:23:16 -07:00
parent d4d99c390d
commit 13cca7be00
2 changed files with 23 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/navigation/ImuFactor.cpp
View File

@ -31,6 +31,20 @@ using namespace std;
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
// Inner class PreintegratedMeasurements // Inner class PreintegratedMeasurements
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
boost::shared_ptr<PreintegratedImuMeasurements::Params> PreintegratedImuMeasurements::MakeParams(
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration,
bool use2ndOrderCoriolis) {
boost::shared_ptr<Params> p = boost::make_shared<Params>();
p->accelerometerCovariance = measuredAccCovariance;
p->gyroscopeCovariance = measuredOmegaCovariance;
p->integrationCovariance = integrationErrorCovariance;
p->use2ndOrderIntegration = use2ndOrderIntegration;
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
return p;
}
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::print(const string& s) const { void PreintegratedImuMeasurements::print(const string& s) const {
PreintegrationBase::print(s); PreintegrationBase::print(s);
} }
@ -116,9 +130,7 @@ void PreintegratedImuMeasurements::integrateMeasurement(
PreintegratedImuMeasurements::PreintegratedImuMeasurements( PreintegratedImuMeasurements::PreintegratedImuMeasurements(
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance, const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) {
const bool use2ndOrderIntegration)
{
biasHat_ = biasHat; biasHat_ = biasHat;
boost::shared_ptr<Params> p = boost::make_shared<Params>(); boost::shared_ptr<Params> p = boost::make_shared<Params>();
p->gyroscopeCovariance = measuredOmegaCovariance; p->gyroscopeCovariance = measuredOmegaCovariance;

9
gtsam/navigation/ImuFactor.h
View File

@ -50,6 +50,13 @@ protected:
public: public:
/// Construct parameters
static boost::shared_ptr<Params> MakeParams(
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration =
false, bool use2ndOrderCoriolis = false);
/** /**
* Constructor, initializes the class with no measurements * Constructor, initializes the class with no measurements
* @param bias Current estimate of acceleration and rotation rate biases * @param bias Current estimate of acceleration and rotation rate biases
@ -91,7 +98,7 @@ public:
const Matrix3& measuredAccCovariance, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance, const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const Matrix3& integrationErrorCovariance,
const bool use2ndOrderIntegration = false); bool use2ndOrderIntegration = false);
private: private: