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

started to include optionalJacobian: compiles after merge to develop and all unit tests pass

release/4.3a0
Luca 2014-12-09 18:03:54 -05:00
parent bf8de1341b
commit d809a952df
4 changed files with 16 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/navigation/ImuFactor.cpp
View File

@ -68,7 +68,7 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){
void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor,
boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
OptionalJacobian<9, 9> F_test, OptionalJacobian<9, 9> G_test) {
Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);
@ -106,7 +106,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
// intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos
Z_3x3, R_i * deltaT, Z_3x3, // vel
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
// Propagation with no approximation:
// preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose();
}

2
gtsam/navigation/ImuFactor.h
View File

@ -115,7 +115,7 @@ public:
*/
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none,
boost::optional<Matrix&> Fout = boost::none, boost::optional<Matrix&> Gout = boost::none);
OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout = boost::none);
/// methods to access class variables
Matrix measurementCovariance() const {return measurementCovariance_;}

2
gtsam/navigation/PreintegratedRotation.h
View File

@ -75,7 +75,7 @@ public:
/// Update preintegrated measurements
void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
boost::optional<Matrix3&> H = boost::none){
OptionalJacobian<3, 3> H = boost::none){
deltaRij_ = deltaRij_.compose(incrR, H, boost::none);
deltaTij_ += deltaT;
}

14
gtsam/navigation/PreintegrationBase.h
View File

@ -116,7 +116,7 @@ public:
/// Update preintegrated measurements
void updatePreintegratedMeasurements(const Vector3& correctedAcc,
const Rot3& incrR, const double deltaT, boost::optional<Matrix&> F = boost::none) {
const Rot3& incrR, const double deltaT, OptionalJacobian<9, 9> F) {
Matrix3 dRij = deltaRij(); // expensive
Vector3 temp = dRij * correctedAcc * deltaT;
@ -127,9 +127,19 @@ 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
// }
if(F){
Matrix3 F_angles_angles;
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);