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

2
gtsam/navigation/ImuFactor.cpp
View File

@ -68,7 +68,7 @@ void ImuFactor::PreintegratedMeasurements::resetIntegration(){
void ImuFactor::PreintegratedMeasurements::integrateMeasurement( void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor, 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; Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor); correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, correctedAcc, correctedOmega, body_P_sensor);

2
gtsam/navigation/ImuFactor.h
View File

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

2
gtsam/navigation/PreintegratedRotation.h
View File

@ -75,7 +75,7 @@ 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){ OptionalJacobian<3, 3> H = boost::none){
deltaRij_ = deltaRij_.compose(incrR, H, boost::none); deltaRij_ = deltaRij_.compose(incrR, H, boost::none);
deltaTij_ += deltaT; deltaTij_ += deltaT;
} }

14
gtsam/navigation/PreintegrationBase.h
View File

@ -116,7 +116,7 @@ public:
/// Update preintegrated measurements /// Update preintegrated measurements
void updatePreintegratedMeasurements(const Vector3& correctedAcc, 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 Matrix3 dRij = deltaRij(); // expensive
Vector3 temp = dRij * correctedAcc * deltaT; Vector3 temp = dRij * correctedAcc * deltaT;
@ -127,9 +127,19 @@ public:
} }
deltaVij_ += temp; 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){ if(F){
Matrix3 F_angles_angles;
const Matrix3 R_i = deltaRij(); const Matrix3 R_i = deltaRij();
Matrix3 F_angles_angles;
updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles); updateIntegratedRotationAndDeltaT(incrR,deltaT, F_angles_angles);
Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT; Matrix3 F_vel_angles = - R_i * skewSymmetric(correctedAcc) * deltaT;
F->resize(9,9); F->resize(9,9);