34#ifdef GTSAM_TANGENT_PREINTEGRATION
35typedef TangentPreintegration PreintegrationType;
37typedef ManifoldPreintegration PreintegrationType;
70 : biasAccCovariance(I_3x3),
71 biasOmegaCovariance(I_3x3),
72 biasAccOmegaInt(I_6x6) {}
77 biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
82 static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedD(
double g = 9.81) {
87 static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedU(
double g = 9.81) {
88 return boost::shared_ptr<PreintegrationCombinedParams>(
new PreintegrationCombinedParams(Vector3(0, 0, -g)));
91 void print(
const std::string& s=
"")
const override;
92 bool equals(
const PreintegratedRotationParams& other,
double tol)
const override;
94 void setBiasAccCovariance(
const Matrix3& cov) { biasAccCovariance=cov; }
95 void setBiasOmegaCovariance(
const Matrix3& cov) { biasOmegaCovariance=cov; }
96 void setBiasAccOmegaInit(
const Matrix6& cov) { biasAccOmegaInt=cov; }
98 const Matrix3& getBiasAccCovariance()
const {
return biasAccCovariance; }
99 const Matrix3& getBiasOmegaCovariance()
const {
return biasOmegaCovariance; }
100 const Matrix6& getBiasAccOmegaInit()
const {
return biasAccOmegaInt; }
105 friend class boost::serialization::access;
106 template <
class ARCHIVE>
107 void serialize(ARCHIVE& ar,
const unsigned int ) {
108 namespace bs = ::boost::serialization;
110 ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
111 ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
112 ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
141 Eigen::Matrix<double, 15, 15> preintMeasCov_;
151 preintMeasCov_.setZero();
160 const boost::shared_ptr<Params>& p,
163 preintMeasCov_.setZero();
173 preintMeasCov_(preintMeasCov) {
185 void resetIntegration()
override;
188 Params&
p()
const {
return *boost::static_pointer_cast<Params>(this->p_); }
194 Matrix preintMeasCov()
const {
return preintMeasCov_; }
200 void print(
const std::string& s =
"Preintegrated Measurements:")
const override;
202 bool equals(
const PreintegratedCombinedMeasurements& expected,
203 double tol = 1e-9)
const;
220 void integrateMeasurement(
const Vector3& measuredAcc,
221 const Vector3& measuredOmega,
const double dt)
override;
227 friend class boost::serialization::access;
228 template <
class ARCHIVE>
229 void serialize(ARCHIVE& ar,
const unsigned int ) {
230 namespace bs = ::boost::serialization;
232 ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
259 Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
273#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
274 typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
299 gtsam::NonlinearFactor::shared_ptr clone()
const override;
305 GTSAM_EXPORT
friend std::ostream& operator<<(std::ostream& os,
306 const CombinedImuFactor&);
309 DefaultKeyFormatter)
const override;
312 bool equals(
const NonlinearFactor& expected,
double tol = 1e-9)
const override;
324 Vector evaluateError(
const Pose3& pose_i,
const Vector3& vel_i,
325 const Pose3& pose_j,
const Vector3& vel_j,
327 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
328 boost::none, boost::optional<Matrix&> H3 = boost::none,
329 boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
330 boost::none, boost::optional<Matrix&> H6 = boost::none)
const override;
334 friend class boost::serialization::access;
335 template <
class ARCHIVE>
336 void serialize(ARCHIVE& ar,
const unsigned int ) {
338 ar& boost::serialization::make_nvp(
339 "NoiseModelFactor6", boost::serialization::base_object<Base>(*
this));
340 ar& BOOST_SERIALIZATION_NVP(_PIM_);
350 :
public Testable<PreintegrationCombinedParams> {};
354 :
public Testable<PreintegratedCombinedMeasurements> {};
typedef and functions to augment Eigen's MatrixXd
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:308
Convenience functions for serializing data structures via boost.serialization.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
A 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
Parameters for pre-integration using PreintegratedCombinedMeasurements: Usage: Create just a single P...
Definition: CombinedImuFactor.h:62
Matrix6 biasAccOmegaInt
covariance of bias used as initial estimate.
Definition: CombinedImuFactor.h:65
PreintegrationCombinedParams(const Vector3 &n_gravity)
See two named constructors below for good values of n_gravity in body frame.
Definition: CombinedImuFactor.h:75
PreintegrationCombinedParams()
Default constructor makes uninitialized params struct.
Definition: CombinedImuFactor.h:69
Matrix3 biasOmegaCovariance
continuous-time "Covariance" describing gyroscope bias random walk
Definition: CombinedImuFactor.h:64
Matrix3 biasAccCovariance
continuous-time "Covariance" describing accelerometer bias random walk
Definition: CombinedImuFactor.h:63
PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
Definition: CombinedImuFactor.h:129
PreintegratedCombinedMeasurements(const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov)
Construct preintegrated directly from members: base class and preintMeasCov.
Definition: CombinedImuFactor.h:171
~PreintegratedCombinedMeasurements() override
Virtual destructor.
Definition: CombinedImuFactor.h:177
PreintegratedCombinedMeasurements()
Default constructor only for serialization and wrappers.
Definition: CombinedImuFactor.h:150
Params & p() const
const reference to params, shadows definition in base class
Definition: CombinedImuFactor.h:188
PreintegratedCombinedMeasurements(const boost::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Default constructor, initializes the class with no measurements.
Definition: CombinedImuFactor.h:159
CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle,...
Definition: CombinedImuFactor.h:259
boost::shared_ptr< CombinedImuFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition: CombinedImuFactor.h:276
CombinedImuFactor()
Default constructor - only use for serialization.
Definition: CombinedImuFactor.h:280
const PreintegratedCombinedMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition: CombinedImuFactor.h:317
IMU pre-integration on NavSatet manifold.
Definition: ManifoldPreintegration.h:33
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition: PreintegrationParams.h:26
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition: NonlinearFactor.h:400