Go to the documentation of this file.
28 using namespace std::placeholders;
32 static std::shared_ptr<PreintegrationParams>
Params() {
33 auto p = PreintegrationParams::MakeSharedD(
kGravity);
36 p->integrationCovariance = 0.0001 * I_3x3;
102 f = std::bind(&ManifoldPreintegration::computeError, pim,
103 std::placeholders::_1, std::placeholders::_2,
104 std::placeholders::_3,
nullptr,
nullptr,
static int runAllTests(TestResult &result)
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative33(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
static const double kGyroSigma
static const double kAccelSigma
Array< double, 1, 3 > e(1./3., 0.5, 2.)
#define EXPECT(condition)
Matrix3 delVdelBiasAcc() const
Rot3 deltaRij() const override
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
void integrateMeasurements(const vector< ImuMeasurement > &measurements, PIM *pim)
static std::shared_ptr< PreintegrationParams > Params()
Some functions to compute numerical derivatives.
Common expressions, both linear and non-linear.
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative32(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Matrix3 delPdelBiasOmega() const
Vector3 deltaVij() const override
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative31(std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Matrix3 delRdelBiasOmega() const
Matrix3 delVdelBiasOmega() const
Matrix3 delPdelBiasAcc() const
Test harness methods for expressions.
Common testing infrastructure.
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
TEST(ManifoldPreintegration, BiasCorrectionJacobians)
Vector9 computeError(const NavState &state_i, const NavState &state_j, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1, OptionalJacobian< 9, 9 > H2, OptionalJacobian< 9, 6 > H3) const
Calculate error given navStates.
imuBias::ConstantBias Bias
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
static const Vector kZero
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Vector3 deltaPij() const override
static std::shared_ptr< PreintegratedCombinedMeasurements::Params > Params(const Matrix3 &biasAccCovariance=Matrix3::Zero(), const Matrix3 &biasOmegaCovariance=Matrix3::Zero(), const Matrix6 &biasAccOmegaInt=Matrix6::Zero())
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
gtsam
Author(s):
autogenerated on Tue Jun 25 2024 03:06:15