From b96a463b10ccba5b0e663d6bba099790f1c6f54b Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 9 Dec 2014 18:32:38 -0500 Subject: [PATCH] ok, fixed updatePreintegratedMeasurements with optionalJacobian --- gtsam/navigation/PreintegrationBase.h | 20 ++++---------------- 1 file changed, 4 insertions(+), 16 deletions(-) diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index e0c13f43d..74b669c04 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -127,28 +127,16 @@ public: } deltaVij_ += temp; -// const Matrix3 R_i = deltaRij(); -// OptionalJacobian<3,3> F_angles_angles; -// updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? F_angles_angles : boost::none); -// if(F){ -// Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; -// // pos vel angle -// *F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos -// Z_3x3, I_3x3, F_vel_angles, // vel -// Z_3x3, Z_3x3, F_angles_angles;// angle -// } + Matrix3 R_i; + if (F) R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij + Matrix3 F_angles_angles; + updateIntegratedRotationAndDeltaT(incrR,deltaT, F ? &F_angles_angles : 0); if(F){ - const Matrix3 R_i = deltaRij(); - Matrix3 F_angles_angles; - updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles); Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; - F->resize(9,9); // pos vel angle *F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos Z_3x3, I_3x3, F_vel_angles, // vel Z_3x3, Z_3x3, F_angles_angles;// angle - }else{ - updateIntegratedRotationAndDeltaT(incrR,deltaT); } }