From 0d07e8764dfe37047ebdc70706b4a1f6c97c33a3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 30 Jan 2016 16:58:28 -0800 Subject: [PATCH] mergeWith prototype --- gtsam/navigation/ImuFactor.cpp | 24 +++++------ gtsam/navigation/PreintegrationBase.cpp | 57 +++++++++++++++++++++++++ gtsam/navigation/PreintegrationBase.h | 3 ++ 3 files changed, 72 insertions(+), 12 deletions(-) diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index c37ea532f..523809b1d 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -158,26 +158,26 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, PreintegratedImuMeasurements ImuFactor::Merge( const PreintegratedImuMeasurements& pim01, const PreintegratedImuMeasurements& pim12) { - if(!pim01.matchesParamsWith(pim12)) - throw std::domain_error("Cannot merge PreintegratedImuMeasurements with different params"); + if (!pim01.matchesParamsWith(pim12)) + throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with different params"); - if(pim01.params()->body_P_sensor) - throw std::domain_error("Cannot merge PreintegratedImuMeasurements with sensor pose yet"); + if (pim01.params()->body_P_sensor) + throw std::domain_error( + "Cannot merge PreintegratedImuMeasurements with sensor pose yet"); // the bias for the merged factor will be the bias from 01 - PreintegratedImuMeasurements pim02(pim01.params(), pim01.biasHat()); + PreintegratedImuMeasurements pim02 = pim01; - const double& t01 = pim01.deltaTij(); - const double& t12 = pim12.deltaTij(); - pim02.deltaTij_ = t01 + t12; + Matrix9 H1, H2; + pim02.mergeWith(pim12, &H1, &H2); - const Rot3 R01 = pim01.deltaRij(), R12 = pim12.deltaRij(); - pim02.preintegrated_ << Rot3::Logmap(R01 * R12), - pim01.deltaPij() + pim01.deltaVij() * t12 + R01 * pim12.deltaPij(), - pim01.deltaVij() + R01 * pim12.deltaVij(); + pim02.preintMeasCov_ = H1 * pim01.preintMeasCov_ * H1.transpose() + + H2 * pim12.preintMeasCov_ * H2.transpose(); return pim02; } + //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index c31120ffc..751dd3fa3 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -308,6 +308,63 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, return error; } +//------------------------------------------------------------------------------ +// sugar for derivative blocks +#define D_R_R(H) (H)->block<3,3>(0,0) +#define D_R_t(H) (H)->block<3,3>(0,3) +#define D_R_v(H) (H)->block<3,3>(0,6) +#define D_t_R(H) (H)->block<3,3>(3,0) +#define D_t_t(H) (H)->block<3,3>(3,3) +#define D_t_v(H) (H)->block<3,3>(3,6) +#define D_v_R(H) (H)->block<3,3>(6,0) +#define D_v_t(H) (H)->block<3,3>(6,3) +#define D_v_v(H) (H)->block<3,3>(6,6) + +//------------------------------------------------------------------------------ +void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1, + Matrix9* H2) { + if (!matchesParamsWith(pim12)) + throw std::domain_error( + "Cannot merge preintegrated measurements with different params"); + + if (params()->body_P_sensor) + throw std::domain_error( + "Cannot merge preintegrated measurements with sensor pose yet"); + + const double& t01 = deltaTij(); + const double& t12 = pim12.deltaTij(); + deltaTij_ = t01 + t12; + + Matrix3 R01_H_theta01, R12_H_theta12; + const Rot3 R01 = Rot3::Expmap(theta(), R01_H_theta01); + const Rot3 R12 = Rot3::Expmap(pim12.theta(), R12_H_theta12); + + Matrix3 R02_H_R01, R02_H_R12; + const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12); + + Matrix3 theta02_H_R02; + preintegrated_ << Rot3::Logmap(R02, theta02_H_R02), + deltaPij() + deltaVij() * t12 + R01 * pim12.deltaPij(), + deltaVij() + R01 * pim12.deltaVij(); + + H1->setZero(); + D_R_R(H1) = theta02_H_R02 * R02_H_R01 * R01_H_theta01; + D_t_t(H1) = I_3x3; + D_t_v(H1) = I_3x3 * t12; + D_v_v(H1) = I_3x3; + + H2->setZero(); + D_R_R(H2) = theta02_H_R02 * R02_H_R12 * R12_H_theta12; // I_3x3 ?? + D_t_t(H2) = R01.matrix(); // + rotated_H_theta1 ?? + D_v_v(H2) = R01.matrix(); // + rotated_H_theta1 ?? + + preintegrated_H_biasAcc_ = + (*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_; + + preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ + + (*H2) * pim12.preintegrated_H_biasOmega_; +} + //------------------------------------------------------------------------------ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i, diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index 70b12ccdb..97e1d76f4 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -211,6 +211,9 @@ public: OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 = boost::none, OptionalJacobian<9, 6> H5 = boost::none) const; + /// Merge in a different set of measurements and update bias derivatives accordingly + /// The derivatives apply to the preintegrated Vector9 + void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2); /// @} /** Dummy clone for MATLAB */