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

included suggestions from Frank

release/4.3a0
Luca 2014-12-08 20:20:03 -05:00
parent dc13912ce2
commit b126d98609
3 changed files with 9 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/navigation/ImuFactor.cpp
View File

@ -101,7 +101,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
} }
if(G_test){ if(G_test){
// Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
// This in only for testing // This in only for testing & documentation
G_test->resize(9,9); G_test->resize(9,9);
// intNoise accNoise omegaNoise // intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos

6
gtsam/navigation/PreintegratedRotation.h
View File

@ -76,12 +76,8 @@ public:
/// Update preintegrated measurements /// Update preintegrated measurements
void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT, void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
boost::optional<Matrix3&> H = boost::none){ boost::optional<Matrix3&> H = boost::none){
deltaRij_ = deltaRij_ * incrR; deltaRij_ = deltaRij_.compose(incrR, H, boost::none);
deltaTij_ += deltaT; deltaTij_ += deltaT;
if(H){
H->resize(3,3);
*H = incrR.transpose();
}
} }
/** /**

11
gtsam/navigation/PreintegrationBase.h
View File

@ -126,16 +126,19 @@ public:
deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
} }
deltaVij_ += temp; deltaVij_ += temp;
Matrix3 F_angles_angles;
const Matrix3 R_i = deltaRij();
updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles);
Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
if(F){ if(F){
Matrix3 F_angles_angles;
const Matrix3 R_i = deltaRij();
updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles);
Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
F->resize(9,9); F->resize(9,9);
// pos vel angle // pos vel angle
*F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos *F << I_3x3, I_3x3 * deltaT, Z_3x3, // pos
Z_3x3, I_3x3, F_vel_angles, // vel Z_3x3, I_3x3, F_vel_angles, // vel
Z_3x3, Z_3x3, F_angles_angles;// angle Z_3x3, Z_3x3, F_angles_angles;// angle
}else{
updateIntegratedRotationAndDeltaT(incrR,deltaT);
} }
} }