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

refactored code for testing ImuPreintegration with impact

release/4.3a0
Varun Agrawal 2020-12-14 14:32:24 -05:00
parent 8b9f917f43
commit 7f975d194a
1 changed files with 68 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

133
tests/testImuPreintegration.cpp
View File

@ -1,46 +1,42 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/** /**
* @file testImuPreintegration.cpp * @file testImuPreintegration.cpp
* @brief Unit tests for IMU Preintegration * @brief Unit tests for IMU Preintegration
* @author Russell Buchanan * @author Russell Buchanan
**/ **/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <tests/ImuMeasurement.h> #include <tests/ImuMeasurement.h>
#include <fstream> #include <fstream>
#include <iostream> #include <iostream>
#include <CppUnitLite/TestHarness.h> using namespace std;
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/navigation/CombinedImuFactor.h>
namespace drs {
Measurement::Measurement() : dt(0), time(0), type("UNDEFINED") {}
Measurement::Measurement(std::string _type) : dt(0), time(0), type(_type) {}
ImuMeasurement::ImuMeasurement() : I_a_WI{0, 0, 0}, I_w_WI{0, 0, 0} { type = "ImuMeasurement"; }
std::ostream& operator<<(std::ostream& stream, const ImuMeasurement& meas) {
stream << "IMU Measurement at time = " << meas.time << " : \n"
<< "dt : " << meas.dt << "\n"
<< "I_a_WI: " << meas.I_a_WI.transpose() << "\n"
<< "I_w_WI: " << meas.I_w_WI.transpose() << "\n";
return stream;
}
} // namespace drs
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
/// \brief Uses the GTSAM library to perform IMU preintegration on an acceleration input. /**
/// * \brief Uses the GTSAM library to perform IMU preintegration on an
TEST(GtsamLibraryTests, LoadedSimulationData) { * acceleration input.
*/
TEST(TestImuPreintegration, LoadedSimulationData) {
Eigen::Vector3d finalPos; Eigen::Vector3d finalPos;
std::vector<drs::ImuMeasurement> imuMeasurements; vector<ImuMeasurement> imuMeasurements;
double accNoiseSigma = 0.001249; double accNoiseSigma = 0.001249;
double accBiasRwSigma = 0.000106; double accBiasRwSigma = 0.000106;
@ -52,33 +48,36 @@ TEST(GtsamLibraryTests, LoadedSimulationData) {
double gravity = 9.81; double gravity = 9.81;
double rate = 400.0; // Hz double rate = 400.0; // Hz
/// @todo Update directory to correct location string inFileString = findExampleDataFile("quadraped_imu_data.csv");
std::string inFileString = "/home/russell/imu_data.csv"; ofstream outputFile;
std::ofstream outputFile; outputFile.open("imu_preint_output.csv", ios::out);
outputFile.open("/home/russell/gtsam_output.csv", std::ofstream::out); ifstream inputFile(inFileString);
std::ifstream inputFile(inFileString); string line;
std::string line; while (getline(inputFile, line)) {
while (std::getline(inputFile, line)) { stringstream ss(line);
std::stringstream ss(line); string str;
std::string str; vector<double> results;
std::vector<double> results;
while (getline(ss, str, ',')) { while (getline(ss, str, ',')) {
results.push_back(std::atof(str.c_str())); results.push_back(atof(str.c_str()));
} }
drs::ImuMeasurement measurement; ImuMeasurement measurement;
measurement.dt = static_cast<uint64_t>(1e9 * (1 / rate)); measurement.dt = static_cast<size_t>(1e9 * (1 / rate));
measurement.time = results[2]; measurement.time = results[2];
measurement.I_a_WI = {results[29], results[30], results[31]}; measurement.I_a_WI = {results[29], results[30], results[31]};
measurement.I_w_WI = {results[17], results[18], results[19]}; measurement.I_w_WI = {results[17], results[18], results[19]};
imuMeasurements.push_back(measurement); imuMeasurements.push_back(measurement);
// std::cout << "IMU measurement " << measurement << std::endl; // cout << "IMU measurement " << measurement << endl;
} }
// Assume a Z-up navigation (assuming we are performing optimization in the IMU frame). // Assume a Z-up navigation (assuming we are performing optimization in the
boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> imuPreintegratedParams = // IMU frame).
gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedU(gravity); boost::shared_ptr<PreintegratedCombinedMeasurements::Params>
imuPreintegratedParams->accelerometerCovariance = I_3x3 * pow(accNoiseSigma, 2); imuPreintegratedParams =
PreintegratedCombinedMeasurements::Params::MakeSharedU(
gravity);
imuPreintegratedParams->accelerometerCovariance =
I_3x3 * pow(accNoiseSigma, 2);
imuPreintegratedParams->biasAccCovariance = I_3x3 * pow(accBiasRwSigma, 2); imuPreintegratedParams->biasAccCovariance = I_3x3 * pow(accBiasRwSigma, 2);
imuPreintegratedParams->gyroscopeCovariance = I_3x3 * pow(gyrNoiseSigma, 2); imuPreintegratedParams->gyroscopeCovariance = I_3x3 * pow(gyrNoiseSigma, 2);
imuPreintegratedParams->biasOmegaCovariance = I_3x3 * pow(gyrBiasRwSigma, 2); imuPreintegratedParams->biasOmegaCovariance = I_3x3 * pow(gyrBiasRwSigma, 2);
@ -86,26 +85,27 @@ TEST(GtsamLibraryTests, LoadedSimulationData) {
imuPreintegratedParams->biasAccOmegaInt = I_6x6 * biasAccOmegaInt; imuPreintegratedParams->biasAccOmegaInt = I_6x6 * biasAccOmegaInt;
// Initial state // Initial state
gtsam::Pose3 priorPose; Pose3 priorPose;
gtsam::Vector3 priorVelocity; Vector3 priorVelocity;
gtsam::imuBias::ConstantBias priorImuBias; imuBias::ConstantBias priorImuBias;
gtsam::PreintegratedCombinedMeasurements imuPreintegrated; PreintegratedCombinedMeasurements imuPreintegrated;
Eigen::Vector3d position; Eigen::Vector3d position;
Eigen::Vector3d velocity; Eigen::Vector3d velocity;
gtsam::NavState propState; NavState propState;
gtsam::NavState initialNavState(priorPose, priorVelocity); NavState initialNavState(priorPose, priorVelocity);
// Bias estimated by my Algorithm // Bias estimated by my Algorithm
priorImuBias = priorImuBias = imuBias::ConstantBias(
gtsam::imuBias::ConstantBias(Eigen::Vector3d(-0.0314648, 0.0219921, 6.95945e-05), Eigen::Vector3d(-0.0314648, 0.0219921, 6.95945e-05),
Eigen::Vector3d(4.88581e-08, -1.04971e-09, -0.000122868)); Eigen::Vector3d(4.88581e-08, -1.04971e-09, -0.000122868));
// zero bias // zero bias
// priorImuBias = gtsam::imuBias::ConstantBias(Eigen::Vector3d(0,0,0), // priorImuBias = imuBias::ConstantBias(Eigen::Vector3d(0,0,0),
// Eigen::Vector3d(0,0,0)); // Eigen::Vector3d(0,0,0));
imuPreintegrated = gtsam::PreintegratedCombinedMeasurements(imuPreintegratedParams, priorImuBias); imuPreintegrated = PreintegratedCombinedMeasurements(
imuPreintegratedParams, priorImuBias);
// Put header row in output csv // Put header row in output csv
outputFile << "X Position," outputFile << "X Position,"
@ -116,27 +116,29 @@ TEST(GtsamLibraryTests, LoadedSimulationData) {
<< "Z Velocity," << "Z Velocity,"
<< "\n"; << "\n";
for (int n = 1; n < imuMeasurements.size(); n++) { //start at 1 to skip header // start at 1 to skip header
for (size_t n = 1; n < imuMeasurements.size(); n++) {
// integrate // integrate
imuPreintegrated.integrateMeasurement(imuMeasurements[n].I_a_WI, imuMeasurements[n].I_w_WI, imuPreintegrated.integrateMeasurement(imuMeasurements[n].I_a_WI,
1 / rate); imuMeasurements[n].I_w_WI, 1 / rate);
// predict // predict
propState = imuPreintegrated.predict(initialNavState, priorImuBias); propState = imuPreintegrated.predict(initialNavState, priorImuBias);
position = propState.pose().translation(); position = propState.pose().translation();
velocity = propState.velocity(); velocity = propState.velocity();
// std::cout << "IMU Position " << position.transpose() << std::endl; // cout << "IMU Position " << position.transpose() << endl;
// std::cout << "IMU Velocity " << velocity.transpose() << std::endl; // cout << "IMU Velocity " << velocity.transpose() << endl;
// Write to csv // Write to csv
outputFile << std::to_string(position.x()) << "," << std::to_string(position.y()) << "," outputFile << to_string(position.x()) << "," << to_string(position.y())
<< std::to_string(position.z()) << "," << std::to_string(velocity.x()) << "," << "," << to_string(position.z()) << ","
<< std::to_string(velocity.y()) << "," << std::to_string(velocity.z()) << "," << to_string(velocity.x()) << "," << to_string(velocity.y())
<< "," << to_string(velocity.z()) << ","
<< "\n"; << "\n";
} }
outputFile.close(); outputFile.close();
gtsam::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
// DOUBLES_EQUAL(0.1, position[0], 1e-2); // DOUBLES_EQUAL(0.1, position[0], 1e-2);
@ -147,6 +149,7 @@ TEST(GtsamLibraryTests, LoadedSimulationData) {
DOUBLES_EQUAL(0.0, rotation[1], 1e-2); DOUBLES_EQUAL(0.0, rotation[1], 1e-2);
DOUBLES_EQUAL(0.0, rotation[2], 1e-2); DOUBLES_EQUAL(0.0, rotation[2], 1e-2);
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;