32#ifdef GTSAM_TANGENT_PREINTEGRATION
33typedef TangentPreintegration PreintegrationType;
35typedef ManifoldPreintegration PreintegrationType;
86 preintMeasCov_.setZero();
97 preintMeasCov_.setZero();
107 preintMeasCov_(preintMeasCov) {
115 void print(
const std::string& s =
"Preintegrated Measurements:")
const override;
121 void resetIntegration()
override;
133 void integrateMeasurement(
const Vector3& measuredAcc,
134 const Vector3& measuredOmega,
const double dt)
override;
137 void integrateMeasurements(
const Matrix& measuredAccs,
const Matrix& measuredOmegas,
143#ifdef GTSAM_TANGENT_PREINTEGRATION
150 friend class boost::serialization::access;
151 template<
class ARCHIVE>
152 void serialize(ARCHIVE & ar,
const unsigned int ) {
153 namespace bs = ::boost::serialization;
155 ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
172 imuBias::ConstantBias> {
184#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
185 typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
210 gtsam::NonlinearFactor::shared_ptr clone()
const override;
214 GTSAM_EXPORT
friend std::ostream& operator<<(std::ostream& os,
const ImuFactor&);
216 DefaultKeyFormatter)
const override;
217 bool equals(
const NonlinearFactor& expected,
double tol = 1e-9)
const override;
229 Vector evaluateError(
const Pose3& pose_i,
const Vector3& vel_i,
230 const Pose3& pose_j,
const Vector3& vel_j,
232 boost::none, boost::optional<Matrix&> H2 = boost::none,
233 boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
234 boost::none, boost::optional<Matrix&> H5 = boost::none)
const override;
236#ifdef GTSAM_TANGENT_PREINTEGRATION
243 static shared_ptr Merge(
const shared_ptr& f01,
const shared_ptr& f12);
248 friend class boost::serialization::access;
249 template<
class ARCHIVE>
250 void serialize(ARCHIVE & ar,
const unsigned int ) {
252 ar & boost::serialization::make_nvp(
"NoiseModelFactor5",
253 boost::serialization::base_object<Base>(*
this));
254 ar & BOOST_SERIALIZATION_NVP(_PIM_);
289 gtsam::NonlinearFactor::shared_ptr clone()
const override;
293 GTSAM_EXPORT
friend std::ostream& operator<<(std::ostream& os,
const ImuFactor2&);
295 DefaultKeyFormatter)
const override;
296 bool equals(
const NonlinearFactor& expected,
double tol = 1e-9)
const override;
310 boost::optional<Matrix&> H1 = boost::none,
311 boost::optional<Matrix&> H2 = boost::none,
312 boost::optional<Matrix&> H3 = boost::none)
const override;
317 friend class boost::serialization::access;
318 template<
class ARCHIVE>
319 void serialize(ARCHIVE & ar,
const unsigned int ) {
321 ar & boost::serialization::make_nvp(
"NoiseModelFactor3",
322 boost::serialization::base_object<Base>(*
this));
323 ar & BOOST_SERIALIZATION_NVP(_PIM_);
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
Template to create a binary predicate.
Definition: Testable.h:111
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
PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
Definition: ImuFactor.h:72
~PreintegratedImuMeasurements() override
Virtual destructor.
Definition: ImuFactor.h:111
PreintegratedImuMeasurements(const boost::shared_ptr< PreintegrationParams > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Constructor, initializes the class with no measurements.
Definition: ImuFactor.h:94
PreintegratedImuMeasurements(const PreintegrationType &base, const Matrix9 &preintMeasCov)
Construct preintegrated directly from members: base class and preintMeasCov.
Definition: ImuFactor.h:105
PreintegratedImuMeasurements()
Default constructor for serialization and wrappers.
Definition: ImuFactor.h:85
Matrix preintMeasCov() const
Return pre-integrated measurement covariance.
Definition: ImuFactor.h:141
Matrix9 preintMeasCov_
COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY].
Definition: ImuFactor.h:79
ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous t...
Definition: ImuFactor.h:172
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition: ImuFactor.h:222
ImuFactor()
Default constructor - only use for serialization.
Definition: ImuFactor.h:191
boost::shared_ptr< ImuFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition: ImuFactor.h:187
ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
Definition: ImuFactor.h:263
ImuFactor2()
Default constructor - only use for serialization.
Definition: ImuFactor.h:274
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition: ImuFactor.h:301
IMU pre-integration on NavSatet manifold.
Definition: ManifoldPreintegration.h:33
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition: NavState.h:34
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition: NonlinearFactor.h:400