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

New method computeError, and more derivative checking (though, expression factors already checked out)

release/4.3a0
Frank 2016-01-27 16:02:40 -08:00
parent 653a41bc5a
commit 7b60c50297
5 changed files with 81 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
gtsam/navigation/PreintegrationBase.cpp
View File

@ -302,6 +302,32 @@ NavState PreintegrationBase::predict(const NavState& state_i,
return state_j;
}
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::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 {
// Predict state at time j
Matrix9 D_predict_state_i;
Matrix96 D_predict_bias_i;
NavState predictedState_j = predict(
state_i, bias_i, H1 ? &D_predict_state_i : 0, H3 ? &D_predict_bias_i : 0);
// Calculate error
Matrix9 D_error_state_j, D_error_predict;
Vector9 error =
state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0,
H1 || H3 ? &D_error_predict : 0);
if (H1) *H1 << D_error_predict* D_predict_state_i;
if (H2) *H2 << D_error_state_j;
if (H3) *H3 << D_error_predict* D_predict_bias_i;
return error;
}
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
@ -314,26 +340,20 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
NavState state_i(pose_i, vel_i);
NavState state_j(pose_j, vel_j);
/// Predict state at time j
Matrix99 D_predict_state_i;
Matrix96 D_predict_bias_i;
NavState predictedState_j = predict(state_i, bias_i,
H1 || H2 ? &D_predict_state_i : 0, H5 ? &D_predict_bias_i : 0);
Matrix9 D_error_state_j, D_error_predict;
Vector9 error = state_j.localCoordinates(predictedState_j,
H3 || H4 ? &D_error_state_j : 0, H1 || H2 || H5 ? &D_error_predict : 0);
// Predict state at time j
Matrix9 D_error_state_i, D_error_state_j;
Vector9 error = computeError(state_i, state_j, bias_i,
H1 || H2 ? &D_error_state_i : 0, H3 || H4 ? &D_error_state_j : 0, H5);
// Separate out derivatives in terms of 5 arguments
// Note that doing so requires special treatment of velocities, as when treated as
// separate variables the retract applied will not be the semi-direct product in NavState
// Instead, the velocities in nav are updated using a straight addition
// This is difference is accounted for by the R().transpose calls below
if (H1) *H1 << D_error_predict* D_predict_state_i.leftCols<6>();
if (H2) *H2 << D_error_predict* D_predict_state_i.rightCols<3>() * state_i.R().transpose();
if (H1) *H1 << D_error_state_i.leftCols<6>();
if (H2) *H2 << D_error_state_i.rightCols<3>() * state_i.R().transpose();
if (H3) *H3 << D_error_state_j.leftCols<6>();
if (H4) *H4 << D_error_state_j.rightCols<3>() * state_j.R().transpose();
if (H5) *H5 << D_error_predict* D_predict_bias_i;
return error;
}
@ -355,5 +375,4 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
#endif
//------------------------------------------------------------------------------
}
/// namespace gtsam
} // namespace gtsam

6
gtsam/navigation/PreintegrationBase.h
View File

@ -264,6 +264,12 @@ public:
OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 6> H2 = boost::none) const;
/// Calculate error given navStates
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;
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,

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

@ -71,7 +71,7 @@ static boost::shared_ptr<PreintegrationParams> defaultParams() {
return p;
}
// Auxiliary functions to test preintegrated Jacobians
// Auxiliary functions to test pre-integrated Jacobians
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
/* ************************************************************************* */
PreintegratedImuMeasurements evaluatePreintegratedMeasurements(
@ -151,14 +151,14 @@ TEST(ImuFactor, PreintegratedMeasurements) {
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
double deltaT = 0.5;
// Expected preintegrated values
// Expected pre-integrated values
Vector3 expectedDeltaP1;
expectedDeltaP1 << 0.5 * 0.1 * 0.5 * 0.5, 0, 0;
Vector3 expectedDeltaV1(0.05, 0.0, 0.0);
Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT1(0.5);
// Actual preintegrated values
// Actual pre-integrated values
PreintegratedImuMeasurements actual1(defaultParams());
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -167,6 +167,24 @@ TEST(ImuFactor, PreintegratedMeasurements) {
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij())));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-9);
// Check derivatives of computeError
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
NavState x1, x2 = actual1.predict(x1, bias);
{
Matrix9 aH1, aH2;
Matrix96 aH3;
actual1.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f =
boost::bind(&PreintegrationBase::computeError, actual1, _1, _2, _3,
boost::none, boost::none, boost::none);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
}
// Integrate again
Vector3 expectedDeltaP2;
expectedDeltaP2 << 0.025 + expectedDeltaP1(0) + 0.5 * 0.1 * 0.5 * 0.5, 0, 0;
@ -175,7 +193,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT2(1);
// Actual preintegrated values
// Actual pre-integrated values
PreintegratedImuMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -184,7 +202,6 @@ TEST(ImuFactor, PreintegratedMeasurements) {
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij())));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-9);
}
/* ************************************************************************* */
// Common linearization point and measurements for tests
namespace common {
@ -482,7 +499,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
deltaTs.push_back(0.01);
}
// Actual preintegrated values
// Actual pre-integrated values
PreintegratedImuMeasurements preintegrated =
evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas,
deltaTs);

18
gtsam/navigation/tests/testPreintegrationBase.cpp
View File

@ -73,6 +73,24 @@ TEST(PreintegrationBase, UpdateEstimate2) {
EXPECT(assert_equal(numericalDerivative33(f, zeta, acc, omega), aH3, 1e-9));
}
/* ************************************************************************* */
TEST(PreintegrationBase, computeError) {
PreintegrationBase pim(defaultParams());
NavState x1, x2;
imuBias::ConstantBias bias;
Matrix9 aH1, aH2;
Matrix96 aH3;
pim.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f =
boost::bind(&PreintegrationBase::computeError, pim, _1, _2, _3,
boost::none, boost::none, boost::none);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;

1
python/handwritten/navigation/ImuFactor.cpp
View File

@ -68,6 +68,7 @@ void exportImuFactor() {
const imuBias::ConstantBias&>())
.def(repr(self))
.def("predict", &PreintegratedImuMeasurements::predict)
.def("computeError", &PreintegratedImuMeasurements::computeError)
.def("resetIntegration", &PreintegratedImuMeasurements::resetIntegration)
.def("integrateMeasurement",
&PreintegratedImuMeasurements::integrateMeasurement)