Go to the documentation of this file.
53 return std::static_pointer_cast<gtsam::NonlinearFactor>(
58 const This*
const f =
dynamic_cast<const This*
>(&
e);
66 std::string
a =
"IMUFactor: " +
s;
70 std::cout <<
"dt: " <<
dt_ << std::endl;
76 Vector6
z()
const {
return (Vector6() <<
accel_,
gyro_).finished(); }
84 const Vector6 meas =
z();
85 if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
87 if (H2) *H2 = numericalDerivative22<Vector6, PoseRTV, PoseRTV>(
96 return Vector6::Zero();
102 double dt,
const Vector6& meas) {
103 Vector6 hx =
x1.imuPrediction(
x2,
dt);
std::shared_ptr< This > shared_ptr
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type tail(NType n)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
const Vector3 & gyro() const
bool equals(const NonlinearFactor &e, double tol=1e-9) const override
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrixTypeT< ValueTypes >... H) const=0
const KeyFormatter & formatter
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
void print(const Matrix &A, const string &s, ostream &stream)
Some functions to compute numerical derivatives.
static Vector6 predict_proxy(const PoseRTV &x1, const PoseRTV &x2, double dt, const Vector6 &meas)
Vector evaluateError(const PoseRTV &x1, const PoseRTV &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
virtual Vector evaluateError(const Pose3 &x1, const Pose3 &x2, OptionalMatrixType H1, OptionalMatrixType H2) const
IMUFactor(const Vector3 &accel, const Vector3 &gyro, double dt, const Key &key1, const Key &key2, const SharedNoiseModel &model)
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Pose3 with translational velocity.
IMUFactor(const Vector6 &imu_vector, double dt, const Key &key1, const Key &key2, const SharedNoiseModel &model)
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
noiseModel::Base::shared_ptr SharedNoiseModel
NoiseModelFactorN< POSE, POSE > Base
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type head(NType n)
void print(const std::string &s="", const gtsam::KeyFormatter &formatter=gtsam::DefaultKeyFormatter) const override
noiseModel::Diagonal::shared_ptr model
gtsam::NonlinearFactor::shared_ptr clone() const override
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Non-linear factor base classes.
Matrix * OptionalMatrixType
std::uint64_t Key
Integer nonlinear key type.
const Vector3 & accel() const
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:41