34 #ifdef GTSAM_TANGENT_PREINTEGRATION 69 : biasAccCovariance(I_3x3),
70 biasOmegaCovariance(I_3x3),
71 biasAccOmegaInt(I_6x6) {}
76 biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
81 static boost::shared_ptr<PreintegrationCombinedParams>
MakeSharedD(
double g = 9.81) {
86 static boost::shared_ptr<PreintegrationCombinedParams>
MakeSharedU(
double g = 9.81) {
90 void print(
const std::string&
s=
"")
const override;
104 friend class boost::serialization::access;
105 template <
class ARCHIVE>
107 namespace bs = ::boost::serialization;
109 ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
110 ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
111 ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
159 const boost::shared_ptr<Params>&
p,
161 : PreintegrationType(p, biasHat) {
171 : PreintegrationType(base),
172 preintMeasCov_(preintMeasCov) {
184 void resetIntegration()
override;
187 Params&
p()
const {
return *boost::static_pointer_cast<Params>(this->p_); }
199 void print(
const std::string&
s =
"Preintegrated Measurements:")
const override;
202 double tol = 1
e-9)
const;
223 friend class boost::serialization::access;
224 template <
class ARCHIVE>
226 namespace bs = ::boost::serialization;
227 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
228 ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
255 Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
269 #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 270 typedef typename boost::shared_ptr<CombinedImuFactor>
shared_ptr;
299 GTSAM_EXPORT
friend std::ostream&
operator<<(std::ostream&
os,
319 Vector evaluateError(
const Pose3& pose_i,
const Vector3& vel_i,
321 const Pose3& pose_j,
const Vector3& vel_j,
323 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
324 boost::none, boost::optional<Matrix&> H3 = boost::none,
325 boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
326 boost::none, boost::optional<Matrix&> H6 = boost::none)
const override;
330 friend class boost::serialization::access;
331 template <
class ARCHIVE>
334 ar& BOOST_SERIALIZATION_NVP(_PIM_);
344 :
public Testable<PreintegrationCombinedParams> {};
348 :
public Testable<PreintegratedCombinedMeasurements> {};
void print(const Matrix &A, const string &s, ostream &stream)
PreintegrationCombinedParams Params
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
static boost::shared_ptr< PreintegrationCombinedParams > MakeSharedU(double g=9.81)
Matrix6 biasAccOmegaInt
covariance of bias used for pre-integration
const Matrix6 & getBiasAccOmegaInt() const
PreintegratedCombinedMeasurements()
Default constructor only for serialization and wrappers.
void setBiasAccCovariance(const Matrix3 &cov)
const Matrix3 & getBiasOmegaCovariance() const
static const KeyFormatter DefaultKeyFormatter
NoiseModelFactor6< Pose3, Vector3, Pose3, Vector3, imuBias::ConstantBias, imuBias::ConstantBias > Base
void g(const string &key, int i)
void setBiasAccOmegaInt(const Matrix6 &cov)
const Matrix3 & getBiasAccCovariance() const
void setBiasOmegaCovariance(const Matrix3 &cov)
ManifoldPreintegration PreintegrationType
~CombinedImuFactor() override
~PreintegratedCombinedMeasurements() override
Virtual destructor.
boost::shared_ptr< This > shared_ptr
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
boost::shared_ptr< CombinedImuFactor > shared_ptr
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const Vector3 measuredAcc
Matrix preintMeasCov() const
PreintegrationCombinedParams(const Vector3 &n_gravity)
See two named constructors below for good values of n_gravity in body frame.
PreintegrationCombinedParams()
void serialize(ARCHIVE &ar, const unsigned int)
Matrix3 biasOmegaCovariance
continuous-time "Covariance" describing gyroscope bias random walk
PreintegratedCombinedMeasurements _PIM_
static const Vector3 measuredOmega(w, 0, 0)
boost::shared_ptr< Factor > shared_ptr
A shared_ptr to this class.
Non-linear factor base classes.
ofstream os("timeSchurFactors.csv")
Eigen::Matrix< double, 15, 15 > preintMeasCov_
BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams)
Add Boost serialization export key (declaration) for derived class.
void serialize(ARCHIVE &ar, const unsigned int)
PreintegratedCombinedMeasurements(const boost::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Params & p() const
const reference to params, shadows definition in base class
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
void serialize(ARCHIVE &ar, const unsigned int)
Annotation indicating that a class derives from another given type.
PreintegratedCombinedMeasurements(const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov)
static boost::shared_ptr< PreintegrationCombinedParams > MakeSharedD(double g=9.81)
const PreintegratedCombinedMeasurements & preintegratedMeasurements() const
std::uint64_t Key
Integer nonlinear key type.
Matrix3 biasAccCovariance
continuous-time "Covariance" describing accelerometer bias random walk