refactored code for testing ImuPreintegration with impact
Varun Agrawal
parent
8b9f917f43
commit
7f975d194a
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue