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

clean up

release/4.3a0
Russell Buchanan 2021-01-19 10:49:42 +00:00
parent e54ef580f7
commit bb662f0cb4
1 changed files with 3 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
tests/testImuPreintegration.cpp
View File

@ -49,8 +49,6 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
double rate = 400.0; // Hz double rate = 400.0; // Hz
string inFileString = findExampleDataFile("quadraped_imu_data.csv"); string inFileString = findExampleDataFile("quadraped_imu_data.csv");
ofstream outputFile;
outputFile.open("imu_preint_output.csv", ios::out);
ifstream inputFile(inFileString); ifstream inputFile(inFileString);
string line; string line;
while (getline(inputFile, line)) { while (getline(inputFile, line)) {
@ -95,27 +93,13 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
NavState initialNavState(priorPose, priorVelocity); NavState initialNavState(priorPose, priorVelocity);
// Bias estimated by my Algorithm // Assume zero bias for simulated data
priorImuBias = imuBias::ConstantBias( priorImuBias = imuBias::ConstantBias(Eigen::Vector3d(0,0,0),
Eigen::Vector3d(-0.0314648, 0.0219921, 6.95945e-05), Eigen::Vector3d(0,0,0));
Eigen::Vector3d(4.88581e-08, -1.04971e-09, -0.000122868));
// zero bias
// priorImuBias = imuBias::ConstantBias(Eigen::Vector3d(0,0,0),
// Eigen::Vector3d(0,0,0));
imuPreintegrated = PreintegratedCombinedMeasurements( imuPreintegrated = PreintegratedCombinedMeasurements(
imuPreintegratedParams, priorImuBias); imuPreintegratedParams, priorImuBias);
// Put header row in output csv
outputFile << "X Position,"
<< "Y Position,"
<< "Z Position,"
<< "X Velocity,"
<< "Y Velocity,"
<< "Z Velocity,"
<< "\n";
// start at 1 to skip header // start at 1 to skip header
for (size_t n = 1; n < imuMeasurements.size(); n++) { for (size_t n = 1; n < imuMeasurements.size(); n++) {
// integrate // integrate
@ -127,17 +111,8 @@ TEST(TestImuPreintegration, LoadedSimulationData) {
velocity = propState.velocity(); velocity = propState.velocity();
// cout << "IMU Position " << position.transpose() << endl; // cout << "IMU Position " << position.transpose() << endl;
// cout << "IMU Velocity " << velocity.transpose() << endl; // cout << "IMU Velocity " << velocity.transpose() << endl;
// Write to csv
outputFile << to_string(position.x()) << "," << to_string(position.y())
<< "," << to_string(position.z()) << ","
<< to_string(velocity.x()) << "," << to_string(velocity.y())
<< "," << to_string(velocity.z()) << ","
<< "\n";
} }
outputFile.close();
Vector3 rotation = propState.pose().rotation().rpy(); Vector3 rotation = propState.pose().rotation().rpy();
// Dont have ground truth for x and y position yet // Dont have ground truth for x and y position yet