-
Notifications
You must be signed in to change notification settings - Fork 28
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add `PreintegratedImuFactor`, a variant of `gtsam::CombinedImuFactor` that works on our combined states. It is templated to work on either `PoseVel` or `PoseVelBias`. Add missing wave_gtsam include dir.
- Loading branch information
Showing
6 changed files
with
467 additions
and
2 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,57 @@ | ||
| #ifndef WAVE_PREINT_IMU_FACTOR_HPP | ||
| #define WAVE_PREINT_IMU_FACTOR_HPP | ||
|
|
||
| #include <gtsam/nonlinear/NonlinearFactor.h> | ||
| #include <gtsam/base/Matrix.h> | ||
| #include "wave/gtsam/pose_vel_bias.hpp" | ||
| #include "wave/gtsam/pose_vel.hpp" | ||
| #include <gtsam/navigation/CombinedImuFactor.h> | ||
| #include <gtsam/navigation/NavState.h> | ||
|
|
||
| namespace wave { | ||
|
|
||
| /** | ||
| * 4-ary factor connecting two combined pose/vel/bias states and two 6d IMU bias | ||
| * estimates. Provides the effect of gtsam::CombinedImuFactor (a 6-ary factor). | ||
| * | ||
| * @tparam StateType either PoseVel or PoseVelBias | ||
| */ | ||
| template <typename StateType> | ||
| class PreintegratedImuFactor | ||
| : public gtsam::NoiseModelFactor4<StateType, | ||
| StateType, | ||
| gtsam::imuBias::ConstantBias, | ||
| gtsam::imuBias::ConstantBias> { | ||
| private: | ||
| gtsam::PreintegratedCombinedMeasurements pim; | ||
| using Base = gtsam::NoiseModelFactor4<StateType, | ||
| StateType, | ||
| gtsam::imuBias::ConstantBias, | ||
| gtsam::imuBias::ConstantBias>; | ||
|
|
||
| public: | ||
| PreintegratedImuFactor(gtsam::Key S1, | ||
| gtsam::Key S2, | ||
| gtsam::Key B1, | ||
| gtsam::Key B2, | ||
| const gtsam::PreintegratedCombinedMeasurements &pim) | ||
| : Base{gtsam::noiseModel::Gaussian::Covariance(pim.preintMeasCov()), | ||
| S1, | ||
| S2, | ||
| B1, | ||
| B2}, | ||
| pim{pim} {} | ||
|
|
||
| gtsam::Vector evaluateError( | ||
| const StateType &state_i, | ||
| const StateType &state_j, | ||
| const gtsam::imuBias::ConstantBias &bias_i, | ||
| const gtsam::imuBias::ConstantBias &bias_j, | ||
| boost::optional<gtsam::Matrix &> H1 = boost::none, | ||
| boost::optional<gtsam::Matrix &> H2 = boost::none, | ||
| boost::optional<gtsam::Matrix &> H3 = boost::none, | ||
| boost::optional<gtsam::Matrix &> H4 = boost::none) const; | ||
| }; | ||
| } // namespace wave | ||
|
|
||
| #endif // WAVE_PREINT_IMU_FACTOR_HPP |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,208 @@ | ||
| #include "wave/gtsam/preint_imu_factor.hpp" | ||
|
|
||
| namespace wave { | ||
|
|
||
| // Implementation of evaluateError for PoseVelBias states | ||
| // This function is adapted from GTSAM's IMU factor code | ||
| template <> | ||
| gtsam::Vector PreintegratedImuFactor<PoseVelBias>::evaluateError( | ||
| const PoseVelBias &state_i, | ||
| const PoseVelBias &state_j, | ||
| const gtsam::imuBias::ConstantBias &bias_i, | ||
| const gtsam::imuBias::ConstantBias &bias_j, | ||
| boost::optional<gtsam::Matrix &> H1, | ||
| boost::optional<gtsam::Matrix &> H2, | ||
| boost::optional<gtsam::Matrix &> H3, | ||
| boost::optional<gtsam::Matrix &> H4) const { | ||
| // Split up the PoseVelBias combined states into pose and vel. | ||
| // (ignore gps bias) | ||
| // Then use code adapted from gtsam::CombinedImuFactor. | ||
| const auto &pose_i = state_i.pose; | ||
| const auto &pose_j = state_j.pose; | ||
|
|
||
| // Note we use only linear velocity here | ||
| const auto &vel_i = state_i.vel.tail<3>(); | ||
| const auto &vel_j = state_j.vel.tail<3>(); | ||
|
|
||
| // Calculate error wrt bias evolution model (random walk) | ||
| gtsam::Matrix6 Hbias_i, Hbias_j; | ||
| gtsam::Vector6 fbias = | ||
| gtsam::traits<gtsam::imuBias::ConstantBias>::Between( | ||
| bias_j, bias_i, H4 ? &Hbias_j : 0, H3 ? &Hbias_i : 0) | ||
| .vector(); | ||
|
|
||
| gtsam::Matrix96 D_r_pose_i, D_r_pose_j, D_r_bias_i; | ||
| gtsam::Matrix93 D_r_vel_i, D_r_vel_j; | ||
|
|
||
| // Calculate error wrt preintegrated measurements | ||
| gtsam::Vector9 r_Rpv = | ||
| this->pim.computeErrorAndJacobians(pose_i, | ||
| vel_i, | ||
| pose_j, | ||
| vel_j, | ||
| bias_i, | ||
| H1 ? &D_r_pose_i : 0, | ||
| H2 ? &D_r_vel_i : 0, | ||
| H1 ? &D_r_pose_j : 0, | ||
| H2 ? &D_r_vel_j : 0, | ||
| H1 ? &D_r_bias_i : 0); | ||
|
|
||
| if (H1) { | ||
| H1->resize(15, 15); | ||
|
|
||
| // Jacobian wrt pose (Pi) | ||
| H1->block<9, 6>(0, PoseVelBias::pose_offset).noalias() = D_r_pose_i; | ||
| // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] | ||
| H1->block<6, 6>(9, PoseVelBias::pose_offset).setZero(); | ||
|
|
||
| // Jacobian wrt linear velocity | ||
| H1->block<9, 3>(0, PoseVelBias::vel_offset + 3).noalias() = D_r_vel_i; | ||
| // Jacobian of bias wrt linear velocity is zero | ||
| H1->block<6, 3>(9, PoseVelBias::vel_offset + 3).setZero(); | ||
| // Jacobian of all wrt angular velocity is zero | ||
| H1->block<15, 3>(0, PoseVelBias::vel_offset).setZero(); | ||
|
|
||
| // Jacobian of all wrt gps bias is zero | ||
| H1->block<15, 3>(0, PoseVelBias::bias_offset).setZero(); | ||
| } | ||
|
|
||
| if (H2) { | ||
| H2->resize(15, 15); | ||
|
|
||
| // Jacobian wrt pose (Pj) | ||
| H2->block<9, 6>(0, PoseVelBias::pose_offset).noalias() = D_r_pose_j; | ||
| // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] | ||
| H2->block<6, 6>(9, PoseVelBias::pose_offset).setZero(); | ||
|
|
||
| // Jacobian wrt linear velocity | ||
| H2->block<9, 3>(0, PoseVelBias::vel_offset + 3).noalias() = D_r_vel_j; | ||
| // Jacobian of bias wrt linear velocity is zero | ||
| H2->block<6, 3>(9, PoseVelBias::vel_offset + 3).setZero(); | ||
| // Jacobian of all wrt angular velocity is zero | ||
| H2->block<15, 3>(0, PoseVelBias::vel_offset).setZero(); | ||
|
|
||
| // Jacobian of all wrt gps bias is zero | ||
| H2->block<15, 3>(0, PoseVelBias::bias_offset).setZero(); | ||
| } | ||
|
|
||
| if (H3) { | ||
| H3->resize(15, 6); | ||
| // Jacobian wrt imu bias | ||
| H3->block<9, 6>(0, 0) = D_r_bias_i; | ||
| // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] | ||
| H3->block<6, 6>(9, 0) = Hbias_i; | ||
| } | ||
|
|
||
| if (H4) { | ||
| H4->resize(15, 6); | ||
| // Jacobian wrt imu bias_j is zero | ||
| H4->block<9, 6>(0, 0).setZero(); | ||
| // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] | ||
| H4->block<6, 6>(9, 0) = Hbias_j; | ||
| } | ||
|
|
||
| // Return overall error | ||
| gtsam::Vector r(15); | ||
| r << r_Rpv, fbias; // vector of size 15 | ||
| return r; | ||
| } | ||
|
|
||
| // Implementation of evaluateError for PoseVel states | ||
| // This function is adapted from GTSAM's IMU factor code | ||
| template <> | ||
| gtsam::Vector PreintegratedImuFactor<PoseVel>::evaluateError( | ||
| const PoseVel &state_i, | ||
| const PoseVel &state_j, | ||
| const gtsam::imuBias::ConstantBias &bias_i, | ||
| const gtsam::imuBias::ConstantBias &bias_j, | ||
| boost::optional<gtsam::Matrix &> H1, | ||
| boost::optional<gtsam::Matrix &> H2, | ||
| boost::optional<gtsam::Matrix &> H3, | ||
| boost::optional<gtsam::Matrix &> H4) const { | ||
| // Split up the PoseVel combined states into pose and vel. | ||
| // Then use code adapted from gtsam::CombinedImuFactor. | ||
| const auto &pose_i = state_i.pose; | ||
| const auto &pose_j = state_j.pose; | ||
|
|
||
| // Note we use only linear velocity here | ||
| const auto &vel_i = state_i.vel.tail<3>(); | ||
| const auto &vel_j = state_j.vel.tail<3>(); | ||
|
|
||
| // Calculate error wrt bias evolution model (random walk) | ||
| gtsam::Matrix6 Hbias_i, Hbias_j; | ||
| gtsam::Vector6 fbias = | ||
| gtsam::traits<gtsam::imuBias::ConstantBias>::Between( | ||
| bias_j, bias_i, H4 ? &Hbias_j : 0, H3 ? &Hbias_i : 0) | ||
| .vector(); | ||
|
|
||
| gtsam::Matrix96 D_r_pose_i, D_r_pose_j, D_r_bias_i; | ||
| gtsam::Matrix93 D_r_vel_i, D_r_vel_j; | ||
|
|
||
| // Calculate error wrt preintegrated measurements | ||
| gtsam::Vector9 r_Rpv = | ||
| this->pim.computeErrorAndJacobians(pose_i, | ||
| vel_i, | ||
| pose_j, | ||
| vel_j, | ||
| bias_i, | ||
| H1 ? &D_r_pose_i : 0, | ||
| H2 ? &D_r_vel_i : 0, | ||
| H1 ? &D_r_pose_j : 0, | ||
| H2 ? &D_r_vel_j : 0, | ||
| H1 ? &D_r_bias_i : 0); | ||
|
|
||
| if (H1) { | ||
| H1->resize(15, 12); | ||
|
|
||
| // Jacobian wrt pose (Pi) | ||
| H1->block<9, 6>(0, PoseVel::pose_offset).noalias() = D_r_pose_i; | ||
| // adding: [dBiasAcc/dPi ; dBiasOmega/dPi] | ||
| H1->block<6, 6>(9, PoseVel::pose_offset).setZero(); | ||
|
|
||
| // Jacobian wrt linear velocity | ||
| H1->block<9, 3>(0, PoseVel::vel_offset + 3).noalias() = D_r_vel_i; | ||
| // Jacobian of bias wrt linear velocity is zero | ||
| H1->block<6, 3>(9, PoseVel::vel_offset + 3).setZero(); | ||
| // Jacobian of all wrt angular velocity is zero | ||
| H1->block<15, 3>(0, PoseVel::vel_offset).setZero(); | ||
| } | ||
|
|
||
| if (H2) { | ||
| H2->resize(15, 12); | ||
|
|
||
| // Jacobian wrt pose (Pj) | ||
| H2->block<9, 6>(0, PoseVel::pose_offset).noalias() = D_r_pose_j; | ||
| // adding: [dBiasAcc/dPj ; dBiasOmega/dPj] | ||
| H2->block<6, 6>(9, PoseVel::pose_offset).setZero(); | ||
|
|
||
| // Jacobian wrt linear velocity | ||
| H2->block<9, 3>(0, PoseVel::vel_offset + 3).noalias() = D_r_vel_j; | ||
| // Jacobian of bias wrt linear velocity is zero | ||
| H2->block<6, 3>(9, PoseVel::vel_offset + 3).setZero(); | ||
| // Jacobian of all wrt angular velocity is zero | ||
| H2->block<15, 3>(0, PoseVel::vel_offset).setZero(); | ||
| } | ||
|
|
||
| if (H3) { | ||
| H3->resize(15, 6); | ||
| // Jacobian wrt imu bias | ||
| H3->block<9, 6>(0, 0) = D_r_bias_i; | ||
| // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] | ||
| H3->block<6, 6>(9, 0) = Hbias_i; | ||
| } | ||
|
|
||
| if (H4) { | ||
| H4->resize(15, 6); | ||
| // Jacobian wrt imu bias_j is zero | ||
| H4->block<9, 6>(0, 0).setZero(); | ||
| // adding: [dBiasAcc/dBias_i ; dBiasOmega/dBias_i] | ||
| H4->block<6, 6>(9, 0) = Hbias_j; | ||
| } | ||
|
|
||
| // Return overall error | ||
| gtsam::Vector r(15); | ||
| r << r_Rpv, fbias; // vector of size 15 | ||
| return r; | ||
| } | ||
|
|
||
| } // namespace wave |
Oops, something went wrong.