Go to the documentation of this file.
43 assert(
model->dim() == 9);
50 assert(imu.size() == 6);
51 assert(
model->dim() == 9);
58 return std::static_pointer_cast<gtsam::NonlinearFactor>(
63 const This*
const f =
dynamic_cast<const This*
>(&
e);
71 std::string
a =
"FullIMUFactor: " +
s;
75 std::cout <<
"dt: " <<
dt_ << std::endl;
91 z.segment(3, 3).operator=(
gyro_);
92 z.tail(3).operator=(
x2.t());
93 if (H1) *H1 = numericalDerivative21<Vector9, PoseRTV, PoseRTV>(
95 if (H2) *H2 = numericalDerivative22<Vector9, PoseRTV, PoseRTV>(
104 return Vector6::Zero();
112 hx.head(6).operator=(
x1.imuPrediction(
x2,
dt));
113 hx.tail(3).operator=(
x1.translationIntegration(
x2,
dt));
std::shared_ptr< This > shared_ptr
Vector evaluateError(const PoseRTV &x1, const PoseRTV &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type tail(NType n)
const Vector3 & accel() const
void print(const std::string &s="", const gtsam::KeyFormatter &formatter=gtsam::DefaultKeyFormatter) const override
Array< double, 1, 3 > e(1./3., 0.5, 2.)
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
static Vector9 predict_proxy(const PoseRTV &x1, const PoseRTV &x2, double dt)
const KeyFormatter & formatter
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
FullIMUFactor(const Vector6 &imu, double dt, const Key &key1, const Key &key2, const SharedNoiseModel &model)
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
FullIMUFactor(const Vector3 &accel, const Vector3 &gyro, double dt, const Key &key1, const Key &key2, const SharedNoiseModel &model)
void print(const Matrix &A, const string &s, ostream &stream)
Some functions to compute numerical derivatives.
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Pose3 with translational velocity.
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
gtsam::NonlinearFactor::shared_ptr clone() const override
noiseModel::Base::shared_ptr SharedNoiseModel
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type head(NType n)
noiseModel::Diagonal::shared_ptr model
~FullIMUFactor() override
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Non-linear factor base classes.
bool equals(const NonlinearFactor &e, double tol=1e-9) const override
FullIMUFactor< POSE > This
virtual Vector evaluateError(const Pose3 &x1, const Pose3 &x2, OptionalMatrixType H1, OptionalMatrixType H2) const
const Vector3 & gyro() const
Matrix * OptionalMatrixType
std::uint64_t Key
Integer nonlinear key type.
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
NoiseModelFactorN< POSE, POSE > Base
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:18