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

Removed debug code

release/4.3a0
Frank 2015-12-22 11:39:20 -08:00
parent d3534b2d2b
commit f1fa66e9c1
1 changed files with 0 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/tests/testScenarioRunner.cpp
View File

@ -48,19 +48,9 @@ class ScenarioRunner {
const double deltaT = scenario_.imuSampleTime(); const double deltaT = scenario_.imuSampleTime();
const size_t nrSteps = T / deltaT; const size_t nrSteps = T / deltaT;
double t = 0; double t = 0;
Vector3 v0 = scenario_.velocity(0);
Vector3 v = Vector3::Zero();
Vector3 p = Vector3::Zero();
for (size_t k = 0; k < nrSteps; k++, t += deltaT) { for (size_t k = 0; k < nrSteps; k++, t += deltaT) {
std::cout << t << ", " << deltaT << ": ";
p += deltaT * v;
v += deltaT * scenario_.acceleration(t);
const Vector3 measuredAcc = scenario_.accelerationInBody(t); const Vector3 measuredAcc = scenario_.accelerationInBody(t);
result.integrateMeasurement(measuredAcc, measuredOmega, deltaT); result.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
std::cout << " P:" << result.deltaPij().transpose();
std::cout << " p:" << p.transpose();
std::cout << " p0:" << (p + v0 * t).transpose();
std::cout << std::endl;
} }
return result; return result;