32 #include <boost/bind.hpp> 39 static boost::shared_ptr<PreintegratedCombinedMeasurements::Params>
Params() {
40 auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(
kGravity);
43 p->integrationCovariance = 0.0001 * I_3x3;
86 p->omegaCoriolis =
Vector3(0,0.1,0.1);
92 measuredOmega << 0, 0,
M_PI / 10.0 + 0.3;
109 noiseModel::Gaussian::Covariance(combined_pim.
preintMeasCov());
114 Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias,
119 Matrix H1e, H2e, H3e, H4e, H5e;
120 (void) imuFactor.
evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
123 Matrix H1a, H2a, H3a, H4a, H5a, H6a;
124 (void) combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a,
135 #ifdef GTSAM_TANGENT_PREINTEGRATION 143 return pim.preintegrated();
151 pim.preintegrated_H_biasAcc()));
153 pim.preintegrated_H_biasOmega(), 1
e-3));
166 const double deltaT = 0.01;
170 for (
int i = 0;
i < 100; ++
i)
181 const Vector3 expectedVelocity(0, 1, 0);
193 const double deltaT = 0.01;
194 const double tol = 1
e-4;
195 for (
int i = 0;
i < 100; ++
i)
void integrateMeasurement(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override
Provides additional testing facilities for common data structures.
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
static int runAllTests(TestResult &result)
A non-templated config holding any types of Manifold-group elements.
Bias bias2(biasAcc2, biasGyro2)
#define DOUBLES_EQUAL(expected, actual, threshold)
void integrateMeasurement(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt) override
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from world to rotated frame
void integrateMeasurements(const vector< ImuMeasurement > &measurements, PIM *pim)
Pose2_ Expmap(const Vector3_ &xi)
TEST(CombinedImuFactor, PreintegratedMeasurements)
Some functions to compute numerical derivatives.
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
static const Vector3 kGravityAlongNavZDown(0, 0, kGravity)
Common testing infrastructure.
Matrix< SCALARB, Dynamic, Dynamic > B
NavState predict(const NavState &state_i, const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 9 > H1=boost::none, OptionalJacobian< 9, 6 > H2=boost::none) const
Predict state at time j.
Vector3 deltaPij() const override
Vector evaluateError(const Pose3 &pose_i, const Vector3 &vel_i, const Pose3 &pose_j, const Vector3 &vel_j, const imuBias::ConstantBias &bias_i, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none, boost::optional< Matrix & > H5=boost::none) const override
vector of errors
Vector3 deltaVij() const override
static const double kAccelSigma
#define EXPECT(condition)
static const double deltaT
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static boost::shared_ptr< PreintegratedCombinedMeasurements::Params > Params()
static const Vector3 measuredAcc
Matrix preintMeasCov() const
const Velocity3 & velocity(OptionalJacobian< 3, 9 > H=boost::none) const
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
static const Vector3 measuredOmega(w, 0, 0)
imuBias::ConstantBias Bias
Rot3 deltaRij() const override
boost::shared_ptr< Gaussian > shared_ptr
static const double kGyroSigma
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation