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

Isolated PreintegrationParams in separate class

release/4.3a0
Frank 2016-01-25 14:16:21 -08:00
parent fc1507acb7
commit 438da30dce
10 changed files with 148 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/navigation/CombinedImuFactor.h
View File

@ -63,14 +63,14 @@ public:
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params : PreintegrationBase::Params {
struct Params : PreintegrationParams {
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInit; ///< covariance of bias used for pre-integration
/// See two named constructors below for good values of n_gravity in body frame
Params(const Vector3& n_gravity) :
PreintegrationBase::Params(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
PreintegrationParams(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
I_3x3), biasAccOmegaInit(I_6x6) {
}

6
gtsam/navigation/ImuFactor.h
View File

@ -82,7 +82,7 @@ public:
* @param bias Current estimate of acceleration and rotation rate biases
* @param p Parameters, typically fixed in a single application
*/
PreintegratedImuMeasurements(const boost::shared_ptr<Params>& p,
PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
PreintegrationBase(p, biasHat) {
preintMeasCov_.setZero();
@ -230,7 +230,7 @@ public:
/// @deprecated typename
typedef PreintegratedImuMeasurements PreintegratedMeasurements;
/// @deprecated constructor, in the new one gravity, coriolis settings are in Params
/// @deprecated constructor, in the new one gravity, coriolis settings are in PreintegrationParams
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
@ -238,7 +238,7 @@ public:
const bool use2ndOrderCoriolis = false);
/// @deprecated use PreintegrationBase::predict,
/// in the new one gravity, coriolis settings are in Params
/// in the new one gravity, coriolis settings are in PreintegrationParams
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
PreintegratedMeasurements& pim, const Vector3& n_gravity,

28
gtsam/navigation/PreintegrationBase.cpp
View File

@ -20,38 +20,14 @@
**/
#include "PreintegrationBase.h"
#ifdef ALLOW_DEPRECATED_IN_GTSAM4
#include <boost/make_shared.hpp>
#endif
using namespace std;
namespace gtsam {
//------------------------------------------------------------------------------
void PreintegrationBase::Params::print(const string& s) const {
PreintegratedRotation::Params::print(s);
cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
<< endl;
cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]"
<< endl;
if (omegaCoriolis && use2ndOrderCoriolis)
cout << "Using 2nd-order Coriolis" << endl;
if (body_P_sensor)
body_P_sensor->print(" ");
cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
}
//------------------------------------------------------------------------------
bool PreintegrationBase::Params::equals(
const PreintegratedRotation::Params& other, double tol) const {
auto e = dynamic_cast<const PreintegrationBase::Params*>(&other);
return e != nullptr && PreintegratedRotation::Params::equals(other, tol)
&& use2ndOrderCoriolis == e->use2ndOrderCoriolis
&& equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance,
tol)
&& equal_with_abs_tol(integrationCovariance, e->integrationCovariance,
tol) && equal_with_abs_tol(n_gravity, e->n_gravity, tol);
}
//------------------------------------------------------------------------------
void PreintegrationBase::resetIntegration() {
deltaTij_ = 0.0;

65
gtsam/navigation/PreintegrationBase.h
View File

@ -21,10 +21,9 @@
#pragma once
#include <gtsam/navigation/PreintegratedRotation.h>
#include <gtsam/navigation/PreintegrationParams.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/ImuBias.h>
#include <boost/make_shared.hpp>
namespace gtsam {
@ -55,54 +54,6 @@ struct PoseVelocityBias {
*/
class PreintegrationBase {
public:
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params: PreintegratedRotation::Params {
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
Vector3 n_gravity; ///< Gravity vector in nav frame
/// The Params constructor insists on getting the navigation frame gravity vector
/// For convenience, two commonly used conventions are provided by named constructors below
Params(const Vector3& n_gravity) :
accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis(
false), n_gravity(n_gravity) {
}
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
return boost::make_shared<Params>(Vector3(0, 0, g));
}
// Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
return boost::make_shared<Params>(Vector3(0, 0, -g));
}
void print(const std::string& s) const;
bool equals(const PreintegratedRotation::Params& other, double tol) const;
protected:
/// Default constructor for serialization only: uninitialized!
Params() {}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
boost::serialization::base_object<PreintegratedRotation::Params>(*this));
ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
ar & BOOST_SERIALIZATION_NVP(n_gravity);
}
};
protected:
/// Parameters. Declared mutable only for deprecated predict method.
@ -110,7 +61,7 @@ protected:
#ifdef ALLOW_DEPRECATED_IN_GTSAM4
mutable
#endif
boost::shared_ptr<Params> p_;
boost::shared_ptr<PreintegrationParams> p_;
/// Acceleration and gyro bias used for preintegration
imuBias::ConstantBias biasHat_;
@ -146,7 +97,7 @@ public:
* @param p Parameters, typically fixed in a single application
* @param bias Current estimate of acceleration and rotation rate biases
*/
PreintegrationBase(const boost::shared_ptr<Params>& p,
PreintegrationBase(const boost::shared_ptr<PreintegrationParams>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
p_(p), biasHat_(biasHat) {
resetIntegration();
@ -155,7 +106,7 @@ public:
/**
* Constructor which takes in all members for generic construction
*/
PreintegrationBase(const boost::shared_ptr<Params>& p, const imuBias::ConstantBias& biasHat,
PreintegrationBase(const boost::shared_ptr<PreintegrationParams>& p, const imuBias::ConstantBias& biasHat,
double deltaTij, const NavState& deltaXij, const Matrix3& delPdelBiasAcc,
const Matrix3& delPdelBiasOmega, const Matrix3& delVdelBiasAcc,
const Matrix3& delVdelBiasOmega)
@ -174,19 +125,19 @@ public:
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
/// check parameters equality: checks whether shared pointer points to same Params object.
/// check parameters equality: checks whether shared pointer points to same PreintegrationParams object.
bool matchesParamsWith(const PreintegrationBase& other) const {
return p_ == other.p_;
}
/// shared pointer to params
const boost::shared_ptr<Params>& params() const {
const boost::shared_ptr<PreintegrationParams>& params() const {
return p_;
}
/// const reference to params
const Params& p() const {
return *boost::static_pointer_cast<Params>(p_);
const PreintegrationParams& p() const {
return *boost::static_pointer_cast<PreintegrationParams>(p_);
}
void set_body_P_sensor(const Pose3& body_P_sensor) {

54
gtsam/navigation/PreintegrationParams.cpp Normal file
View File

@ -0,0 +1,54 @@
/* ----------------------------------------------------------------------------
* 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 PreintegrationParams.h
* @author Luca Carlone
* @author Stephen Williams
* @author Richard Roberts
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
**/
#include "PreintegrationParams.h"
using namespace std;
namespace gtsam {
//------------------------------------------------------------------------------
void PreintegrationParams::print(const string& s) const {
PreintegratedRotation::Params::print(s);
cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
<< endl;
cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]"
<< endl;
if (omegaCoriolis && use2ndOrderCoriolis)
cout << "Using 2nd-order Coriolis" << endl;
if (body_P_sensor) body_P_sensor->print(" ");
cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
}
//------------------------------------------------------------------------------
bool PreintegrationParams::equals(const PreintegratedRotation::Params& other,
double tol) const {
auto e = dynamic_cast<const PreintegrationParams*>(&other);
return e != nullptr && PreintegratedRotation::Params::equals(other, tol) &&
use2ndOrderCoriolis == e->use2ndOrderCoriolis &&
equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance,
tol) &&
equal_with_abs_tol(integrationCovariance, e->integrationCovariance,
tol) &&
equal_with_abs_tol(n_gravity, e->n_gravity, tol);
}
} // namespace gtsam

71
gtsam/navigation/PreintegrationParams.h Normal file
View File

@ -0,0 +1,71 @@
/* ----------------------------------------------------------------------------
* 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 PreintegrationParams.h
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/navigation/PreintegratedRotation.h>
#include <boost/make_shared.hpp>
namespace gtsam {
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct PreintegrationParams: PreintegratedRotation::Params {
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
Vector3 n_gravity; ///< Gravity vector in nav frame
/// The Params constructor insists on getting the navigation frame gravity vector
/// For convenience, two commonly used conventions are provided by named constructors below
PreintegrationParams(const Vector3& n_gravity)
: accelerometerCovariance(I_3x3),
integrationCovariance(I_3x3),
use2ndOrderCoriolis(false),
n_gravity(n_gravity) {}
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
static boost::shared_ptr<PreintegrationParams> MakeSharedD(double g = 9.81) {
return boost::make_shared<PreintegrationParams>(Vector3(0, 0, g));
}
// Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
static boost::shared_ptr<PreintegrationParams> MakeSharedU(double g = 9.81) {
return boost::make_shared<PreintegrationParams>(Vector3(0, 0, -g));
}
void print(const std::string& s) const;
bool equals(const PreintegratedRotation::Params& other, double tol) const;
protected:
/// Default constructor for serialization only: uninitialized!
PreintegrationParams() {}
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
boost::serialization::base_object<PreintegratedRotation::Params>(*this));
ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
ar & BOOST_SERIALIZATION_NVP(n_gravity);
}
};
} // namespace gtsam

1
gtsam/navigation/ScenarioRunner.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/navigation/ScenarioRunner.h>
#include <gtsam/base/timing.h>
#include <boost/assign.hpp>
#include <cmath>
namespace gtsam {

2
gtsam/navigation/ScenarioRunner.h
View File

@ -28,7 +28,7 @@ namespace gtsam {
*/
class ScenarioRunner {
public:
typedef boost::shared_ptr<PreintegratedImuMeasurements::Params> SharedParams;
typedef boost::shared_ptr<PreintegrationParams> SharedParams;
ScenarioRunner(const Scenario* scenario, const SharedParams& p,
double imuSampleTime = 1.0 / 100.0,
const imuBias::ConstantBias& bias = imuBias::ConstantBias())

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

@ -62,8 +62,8 @@ static const double kGyroSigma = 0.02;
static const double kAccelerometerSigma = 0.1;
// Create default parameters with Z-down and above noise paramaters
static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
auto p = PreintegratedImuMeasurements::Params::MakeSharedD(kGravity);
static boost::shared_ptr<PreintegrationParams> defaultParams() {
auto p = PreintegrationParams::MakeSharedD(kGravity);
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
p->accelerometerCovariance = kAccelerometerSigma * kAccelerometerSigma
* I_3x3;

4
gtsam/navigation/tests/testScenarioRunner.cpp
View File

@ -32,8 +32,8 @@ static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
// Create default parameters with Z-down and above noise parameters
static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
auto p = PreintegratedImuMeasurements::Params::MakeSharedD(10);
static boost::shared_ptr<PreintegrationParams> defaultParams() {
auto p = PreintegrationParams::MakeSharedD(10);
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
p->integrationCovariance = 0.0000001 * I_3x3;