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

deleted comment and added TODO

release/4.3a0
Luca 2014-12-02 11:48:07 -05:00
parent 667673e9a9
commit b6c375db0d
2 changed files with 20 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
gtsam/navigation/CombinedImuFactor.h
View File

@ -489,27 +489,6 @@ namespace gtsam {
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
/* TODO why is this commented out. Put it on a branch but remove from develop?
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi
- Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij,
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi
skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij,
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Z_3x3,
//dBiasAcc/dPi
Z_3x3, Z_3x3,
//dBiasOmega/dPi
Z_3x3, Z_3x3;
*/
if(H1) { if(H1) {
H1->resize(15,6); H1->resize(15,6);
@ -524,25 +503,25 @@ namespace gtsam {
dfVdPi = Z_3x3; dfVdPi = Z_3x3;
} }
(*H1) << (*H1) <<
// dfP/dRi // dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_ Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
+ preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr), + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
// dfP/dPi // dfP/dPi
dfPdPi, dfPdPi,
// dfV/dRi // dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_ Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
+ preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr), + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
// dfV/dPi // dfV/dPi
dfVdPi, dfVdPi,
// dfR/dRi // dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi // dfR/dPi
Z_3x3, Z_3x3,
//dBiasAcc/dPi //dBiasAcc/dPi
Z_3x3, Z_3x3, Z_3x3, Z_3x3,
//dBiasOmega/dPi //dBiasOmega/dPi
Z_3x3, Z_3x3; Z_3x3, Z_3x3;
} }
if(H2) { if(H2) {

1
gtsam/navigation/ImuFactor.h
View File

@ -105,6 +105,7 @@ struct PoseVelocity {
PreintMeasCov_.setZero(9,9); PreintMeasCov_.setZero(9,9);
} }
// TODO: in what context is this constructor used and why do you init to zero?
PreintegratedMeasurements() : PreintegratedMeasurements() :
biasHat_(imuBias::ConstantBias()), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0), biasHat_(imuBias::ConstantBias()), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3), delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),