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

Fixed two bugs

release/4.3a0
Frank Dellaert 2015-12-28 17:19:33 -08:00
parent 52397bb4a4
commit 9eed146612
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/navigation/ScenarioRunner.cpp
View File

@ -88,7 +88,7 @@ PreintegratedMeasurements2::integrateCorrected(const Vector3& correctedAcc,
// estimate current estimate from posterior // estimate current estimate from posterior
// TODO(frank): maybe we should store this - or only recover theta = inv(R)*d // TODO(frank): maybe we should store this - or only recover theta = inv(R)*d
Vector9 zeta = currentEstimate(); Vector9 zeta = currentEstimate();
Vector3 theta_k = zeta.tail<3>(); Vector3 theta_k = zeta.head<3>();
cout << "zeta: " << zeta.transpose() << endl; cout << "zeta: " << zeta.transpose() << endl;
Rot3 Rk = Rot3::Expmap(theta_k); Rot3 Rk = Rot3::Expmap(theta_k);
Matrix3 Rkt = Rk.transpose(); Matrix3 Rkt = Rk.transpose();
@ -170,7 +170,7 @@ NavState PreintegratedMeasurements2::predict(
cout << "tij: " << deltaTij_ << endl; cout << "tij: " << deltaTij_ << endl;
cout << "gt: " << gt.transpose() << endl; cout << "gt: " << gt.transpose() << endl;
cout << "gt^2/2: " << 0.5 * deltaTij_* gt.transpose() << endl; cout << "gt^2/2: " << 0.5 * deltaTij_* gt.transpose() << endl;
return state_i.expmap(zeta); return state_i.retract(zeta);
} }
//////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////