12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Boost optional for sensor pose

release/4.3a0
dellaert 2015-08-24 15:15:57 -07:00
parent 2d251c6411
commit cf5f859679
3 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/navigation/ImuFactor.cpp
View File

@ -103,9 +103,9 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegratedImuMeasurements::integrateMeasurement( void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
const Pose3& body_P_sensor) { boost::optional<Pose3> body_P_sensor) {
// modify parameters to accommodate deprecated method:-( // modify parameters to accommodate deprecated method:-(
p_->body_P_sensor.reset(body_P_sensor); p_->body_P_sensor = body_P_sensor;
integrateMeasurement(measuredAcc, measuredOmega, deltaT); integrateMeasurement(measuredAcc, measuredOmega, deltaT);
} }

3
gtsam/navigation/ImuFactor.h
View File

@ -119,7 +119,8 @@ public:
/// @deprecated version of integrateMeasurement with body_P_sensor /// @deprecated version of integrateMeasurement with body_P_sensor
/// Use parameters instead /// Use parameters instead
void integrateMeasurement(const Vector3& measuredAcc, void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double dt, const Pose3& body_P_sensor); const Vector3& measuredOmega, double dt,
boost::optional<Pose3> body_P_sensor);
private: private:

19
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -116,8 +116,8 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
/* ************************************************************************* */ /* ************************************************************************* */
bool MonteCarlo(const PreintegratedImuMeasurements& pim, bool MonteCarlo(const PreintegratedImuMeasurements& pim,
const NavState& state, const imuBias::ConstantBias& bias, double dt, const NavState& state, const imuBias::ConstantBias& bias, double dt,
const Pose3& body_P_sensor, const Vector3& measuredAcc, boost::optional<Pose3> body_P_sensor, const Vector3& measuredAcc,
const Vector3& measuredOmega, size_t N = 10, const Vector3& measuredOmega, size_t N = 50000,
size_t M = 1) { size_t M = 1) {
// Get mean prediction from "ground truth" measurements // Get mean prediction from "ground truth" measurements
PreintegratedImuMeasurements pim1 = pim; PreintegratedImuMeasurements pim1 = pim;
@ -144,12 +144,12 @@ bool MonteCarlo(const PreintegratedImuMeasurements& pim,
samples.col(i) = xi; samples.col(i) = xi;
sum += xi; sum += xi;
} }
// Vector9 sampleMean = sum / N; Vector9 sampleMean = sum / N;
Matrix9 Q; Matrix9 Q;
Q.setZero(); Q.setZero();
for (size_t i = 0; i < N; i++) { for (size_t i = 0; i < N; i++) {
Vector9 xi = samples.col(i); Vector9 xi = samples.col(i);
// xi -= sampleMean; xi -= sampleMean;
Q += xi * xi.transpose() / (N - 1); Q += xi * xi.transpose() / (N - 1);
} }
@ -199,7 +199,9 @@ TEST(ImuFactor, StraightLine) {
// Do Monte-Carlo analysis // Do Monte-Carlo analysis
PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance, PreintegratedImuMeasurements pimMC(kZeroBiasHat, p->accelerometerCovariance,
p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise p->gyroscopeCovariance, Z_3x3, true); // MonteCarlo does not sample integration noise
EXPECT(MonteCarlo(pimMC, state1, kZeroBias, dt/10, Pose3(), measuredAcc, measuredOmega, 100000)); EXPECT(
MonteCarlo(pimMC, state1, kZeroBias, dt / 10, boost::none, measuredAcc,
measuredOmega));
// Check integrated quantities in body frame: gravity measured by IMU is integrated! // Check integrated quantities in body frame: gravity measured by IMU is integrated!
Vector3 b_deltaP(a * dt22, 0, -g * dt22); Vector3 b_deltaP(a * dt22, 0, -g * dt22);
@ -453,10 +455,9 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor // Create factor
Pose3 bodyPsensor = Pose3();
bool use2ndOrderCoriolis = true; bool use2ndOrderCoriolis = true;
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown, ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim, kGravityAlongNavZDown,
kNonZeroOmegaCoriolis, bodyPsensor, use2ndOrderCoriolis); kNonZeroOmegaCoriolis, boost::none, use2ndOrderCoriolis);
Values values; Values values;
values.insert(X(1), x1); values.insert(X(1), x1);
@ -670,7 +671,7 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
values.insert(B(1), bias); values.insert(B(1), bias);
// Make sure linearization is correct // Make sure linearization is correct
double diffDelta = 1e-7; double diffDelta = 1e-5;
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, diffDelta, 1e-3);
} }
@ -759,7 +760,7 @@ TEST(ImuFactor, PredictArbitrary) {
Pose3 x1; Pose3 x1;
Vector3 v1 = Vector3(0, 0, 0); Vector3 v1 = Vector3(0, 0, 0);
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, Pose3(), EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none,
measuredAcc, measuredOmega)); measuredAcc, measuredOmega));
for (int i = 0; i < 1000; ++i) for (int i = 0; i < 1000; ++i)