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
* @brief Unit tests for IMU Preintegration
* @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 <fstream>
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#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 std;
using namespace gtsam;
/* ************************************************************************* */
/// \brief Uses the GTSAM library to perform IMU preintegration on an acceleration input.
///
TEST(GtsamLibraryTests, LoadedSimulationData) {
/**
* \brief Uses the GTSAM library to perform IMU preintegration on an
* acceleration input.
*/
TEST(TestImuPreintegration, LoadedSimulationData) {
Eigen::Vector3d finalPos;
std::vector<drs::ImuMeasurement> imuMeasurements;
vector<ImuMeasurement> imuMeasurements;
double accNoiseSigma = 0.001249;
double accBiasRwSigma = 0.000106;
@ -52,33 +48,36 @@ TEST(GtsamLibraryTests, LoadedSimulationData) {
double gravity = 9.81;
double rate = 400.0; // Hz
/// @todo Update directory to correct location
std::string inFileString = "/home/russell/imu_data.csv";
std::ofstream outputFile;
outputFile.open("/home/russell/gtsam_output.csv", std::ofstream::out);
std::ifstream inputFile(inFileString);
std::string line;
while (std::getline(inputFile, line)) {
std::stringstream ss(line);
std::string str;
std::vector<double> results;
string inFileString = findExampleDataFile("quadraped_imu_data.csv");
ofstream outputFile;
outputFile.open("imu_preint_output.csv", ios::out);
ifstream inputFile(inFileString);
string line;
while (getline(inputFile, line)) {
stringstream ss(line);
string str;
vector<double> results;
while (getline(ss, str, ',')) {
results.push_back(std::atof(str.c_str()));
results.push_back(atof(str.c_str()));
}
drs::ImuMeasurement measurement;
measurement.dt = static_cast<uint64_t>(1e9 * (1 / rate));
ImuMeasurement measurement;
measurement.dt = static_cast<size_t>(1e9 * (1 / rate));
measurement.time = results[2];
measurement.I_a_WI = {results[29], results[30], results[31]};
measurement.I_w_WI = {results[17], results[18], results[19]};
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).
boost::shared_ptr<gtsam::PreintegratedCombinedMeasurements::Params> imuPreintegratedParams =
gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedU(gravity);
imuPreintegratedParams->accelerometerCovariance = I_3x3 * pow(accNoiseSigma, 2);
// Assume a Z-up navigation (assuming we are performing optimization in the
// IMU frame).
boost::shared_ptr<PreintegratedCombinedMeasurements::Params>
imuPreintegratedParams =
PreintegratedCombinedMeasurements::Params::MakeSharedU(
gravity);
imuPreintegratedParams->accelerometerCovariance =
I_3x3 * pow(accNoiseSigma, 2);
imuPreintegratedParams->biasAccCovariance = I_3x3 * pow(accBiasRwSigma, 2);
imuPreintegratedParams->gyroscopeCovariance = I_3x3 * pow(gyrNoiseSigma, 2);
imuPreintegratedParams->biasOmegaCovariance = I_3x3 * pow(gyrBiasRwSigma, 2);
@ -86,26 +85,27 @@ TEST(GtsamLibraryTests, LoadedSimulationData) {
imuPreintegratedParams->biasAccOmegaInt = I_6x6 * biasAccOmegaInt;
// Initial state
gtsam::Pose3 priorPose;
gtsam::Vector3 priorVelocity;
gtsam::imuBias::ConstantBias priorImuBias;
gtsam::PreintegratedCombinedMeasurements imuPreintegrated;
Pose3 priorPose;
Vector3 priorVelocity;
imuBias::ConstantBias priorImuBias;
PreintegratedCombinedMeasurements imuPreintegrated;
Eigen::Vector3d position;
Eigen::Vector3d velocity;
gtsam::NavState propState;
NavState propState;
gtsam::NavState initialNavState(priorPose, priorVelocity);
NavState initialNavState(priorPose, priorVelocity);
// Bias estimated by my Algorithm
priorImuBias =
gtsam::imuBias::ConstantBias(Eigen::Vector3d(-0.0314648, 0.0219921, 6.95945e-05),
Eigen::Vector3d(4.88581e-08, -1.04971e-09, -0.000122868));
priorImuBias = imuBias::ConstantBias(
Eigen::Vector3d(-0.0314648, 0.0219921, 6.95945e-05),
Eigen::Vector3d(4.88581e-08, -1.04971e-09, -0.000122868));
// zero bias
// priorImuBias = gtsam::imuBias::ConstantBias(Eigen::Vector3d(0,0,0),
// priorImuBias = imuBias::ConstantBias(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
outputFile << "X Position,"
@ -116,27 +116,29 @@ TEST(GtsamLibraryTests, LoadedSimulationData) {
<< "Z Velocity,"
<< "\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
imuPreintegrated.integrateMeasurement(imuMeasurements[n].I_a_WI, imuMeasurements[n].I_w_WI,
1 / rate);
imuPreintegrated.integrateMeasurement(imuMeasurements[n].I_a_WI,
imuMeasurements[n].I_w_WI, 1 / rate);
// predict
propState = imuPreintegrated.predict(initialNavState, priorImuBias);
position = propState.pose().translation();
velocity = propState.velocity();
// std::cout << "IMU Position " << position.transpose() << std::endl;
// std::cout << "IMU Velocity " << velocity.transpose() << std::endl;
// cout << "IMU Position " << position.transpose() << endl;
// cout << "IMU Velocity " << velocity.transpose() << endl;
// Write to csv
outputFile << std::to_string(position.x()) << "," << std::to_string(position.y()) << ","
<< std::to_string(position.z()) << "," << std::to_string(velocity.x()) << ","
<< std::to_string(velocity.y()) << "," << std::to_string(velocity.z()) << ","
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();
gtsam::Vector3 rotation = propState.pose().rotation().rpy();
Vector3 rotation = propState.pose().rotation().rpy();
// Dont have ground truth for x and y position yet
// 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[2], 1e-2);
}
/* ************************************************************************* */
int main() {
TestResult tr;