diff --git a/examples/CombinedImuFactorsExample.cpp b/examples/CombinedImuFactorsExample.cpp new file mode 100644 index 000000000..c9646e64d --- /dev/null +++ b/examples/CombinedImuFactorsExample.cpp @@ -0,0 +1,303 @@ +/* ---------------------------------------------------------------------------- + + * 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 CombinedImuFactorsExample + * @brief Test example for using GTSAM ImuCombinedFactor + * navigation code. + * @author Varun Agrawal + */ + +/** + * Example of use of the CombinedImuFactor in + * conjunction with GPS + * - we read IMU and GPS data from a CSV file, with the following format: + * A row starting with "i" is the first initial position formatted with + * N, E, D, qx, qY, qZ, qW, velN, velE, velD + * A row starting with "0" is an imu measurement + * linAccN, linAccE, linAccD, angVelN, angVelE, angVelD + * A row starting with "1" is a gps correction formatted with + * N, E, D, qX, qY, qZ, qW + * Note that for GPS correction, we're only using the position not the + * rotation. The rotation is provided in the file for ground truth comparison. + * + * See usage: ./CombinedImuFactorsExample --help + */ + +#include + +// GTSAM related includes. +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace gtsam; +using namespace std; + +using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) +using symbol_shorthand::V; // Vel (xdot,ydot,zdot) +using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) + +namespace po = boost::program_options; + +po::variables_map parseOptions(int argc, char* argv[]) { + po::options_description desc; + desc.add_options()("help,h", "produce help message")( + "data_csv_path", po::value()->default_value("imuAndGPSdata.csv"), + "path to the CSV file with the IMU data")( + "output_filename", + po::value()->default_value("imuFactorExampleResults.csv"), + "path to the result file to use")("use_isam", po::bool_switch(), + "use ISAM as the optimizer"); + + po::variables_map vm; + po::store(po::parse_command_line(argc, argv, desc), vm); + + if (vm.count("help")) { + cout << desc << "\n"; + exit(1); + } + + return vm; +} + +Vector10 readInitialState(ifstream& file) { + string value; + // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD) + Vector10 initial_state; + getline(file, value, ','); // i + for (int i = 0; i < 9; i++) { + getline(file, value, ','); + initial_state(i) = stof(value.c_str()); + } + getline(file, value, '\n'); + initial_state(9) = stof(value.c_str()); + + return initial_state; +} + +boost::shared_ptr imuParams() { + // We use the sensor specs to build the noise model for the IMU factor. + double accel_noise_sigma = 0.0003924; + double gyro_noise_sigma = 0.000205689024915; + double accel_bias_rw_sigma = 0.004905; + double gyro_bias_rw_sigma = 0.000001454441043; + Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2); + Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2); + Matrix33 integration_error_cov = + I_3x3 * 1e-8; // error committed in integrating position from velocities + Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); + Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); + Matrix66 bias_acc_omega_int = + I_6x6 * 1e-5; // error in the bias used for preintegration + + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); + // PreintegrationBase params: + p->accelerometerCovariance = + measured_acc_cov; // acc white noise in continuous + p->integrationCovariance = + integration_error_cov; // integration uncertainty continuous + // should be using 2nd order integration + // PreintegratedRotation params: + p->gyroscopeCovariance = + measured_omega_cov; // gyro white noise in continuous + // PreintegrationCombinedMeasurements params: + p->biasAccCovariance = bias_acc_cov; // acc bias in continuous + p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous + p->biasAccOmegaInt = bias_acc_omega_int; + + return p; +} + +int main(int argc, char* argv[]) { + string data_filename, output_filename; + po::variables_map var_map = parseOptions(argc, argv); + + data_filename = findExampleDataFile(var_map["data_csv_path"].as()); + output_filename = var_map["output_filename"].as(); + + // Set up output file for plotting errors + FILE* fp_out = fopen(output_filename.c_str(), "w+"); + fprintf(fp_out, + "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx," + "gt_qy,gt_qz,gt_qw\n"); + + // Begin parsing the CSV file. Input the first line for initialization. + // From there, we'll iterate through the file and we'll preintegrate the IMU + // or add in the GPS given the input. + ifstream file(data_filename.c_str()); + + Vector10 initial_state = readInitialState(file); + cout << "initial state:\n" << initial_state.transpose() << "\n\n"; + + // Assemble initial quaternion through GTSAM constructor + // ::Quaternion(w,x,y,z); + Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3), + initial_state(4), initial_state(5)); + Point3 prior_point(initial_state.head<3>()); + Pose3 prior_pose(prior_rotation, prior_point); + Vector3 prior_velocity(initial_state.tail<3>()); + + imuBias::ConstantBias prior_imu_bias; // assume zero initial bias + + int index = 0; + + Values initial_values; + + // insert pose at initialization + initial_values.insert(X(index), prior_pose); + initial_values.insert(V(index), prior_velocity); + initial_values.insert(B(index), prior_imu_bias); + + // Assemble prior noise model and add it the graph.` + auto pose_noise_model = noiseModel::Diagonal::Sigmas( + (Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5) + .finished()); // rad,rad,rad,m, m, m + auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1); // m/s + auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 1e-3); + + // Add all prior factors (pose, velocity, bias) to the graph. + NonlinearFactorGraph graph; + graph.addPrior(X(index), prior_pose, pose_noise_model); + graph.addPrior(V(index), prior_velocity, velocity_noise_model); + graph.addPrior(B(index), prior_imu_bias, + bias_noise_model); + + auto p = imuParams(); + + std::shared_ptr preintegrated = + std::make_shared(p, prior_imu_bias); + + assert(preintegrated); + + // Store previous state for imu integration and latest predicted outcome. + NavState prev_state(prior_pose, prior_velocity); + NavState prop_state = prev_state; + imuBias::ConstantBias prev_bias = prior_imu_bias; + + // Keep track of total error over the entire run as simple performance metric. + double current_position_error = 0.0, current_orientation_error = 0.0; + + double output_time = 0.0; + double dt = 0.005; // The real system has noise, but here, results are nearly + // exactly the same, so keeping this for simplicity. + + // All priors have been set up, now iterate through the data file. + while (file.good()) { + // Parse out first value + string value; + getline(file, value, ','); + int type = stoi(value.c_str()); + + if (type == 0) { // IMU measurement + Vector6 imu; + for (int i = 0; i < 5; ++i) { + getline(file, value, ','); + imu(i) = stof(value.c_str()); + } + getline(file, value, '\n'); + imu(5) = stof(value.c_str()); + + // Adding the IMU preintegration. + preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt); + + } else if (type == 1) { // GPS measurement + Vector7 gps; + for (int i = 0; i < 6; ++i) { + getline(file, value, ','); + gps(i) = stof(value.c_str()); + } + getline(file, value, '\n'); + gps(6) = stof(value.c_str()); + + index++; + + // Adding IMU factor and GPS factor and optimizing. + auto preint_imu_combined = + dynamic_cast( + *preintegrated); + CombinedImuFactor imu_factor(X(index - 1), V(index - 1), X(index), + V(index), B(index - 1), B(index), + preint_imu_combined); + graph.add(imu_factor); + + auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0); + GPSFactor gps_factor(X(index), + Point3(gps(0), // N, + gps(1), // E, + gps(2)), // D, + correction_noise); + graph.add(gps_factor); + + // Now optimize and compare results. + prop_state = preintegrated->predict(prev_state, prev_bias); + initial_values.insert(X(index), prop_state.pose()); + initial_values.insert(V(index), prop_state.v()); + initial_values.insert(B(index), prev_bias); + + LevenbergMarquardtParams params; + params.setVerbosityLM("SUMMARY"); + LevenbergMarquardtOptimizer optimizer(graph, initial_values, params); + Values result = optimizer.optimize(); + + // Overwrite the beginning of the preintegration for the next step. + prev_state = + NavState(result.at(X(index)), result.at(V(index))); + prev_bias = result.at(B(index)); + + // Reset the preintegration object. + preintegrated->resetIntegrationAndSetBias(prev_bias); + + // Print out the position and orientation error for comparison. + Vector3 result_position = prev_state.pose().translation(); + Vector3 position_error = result_position - gps.head<3>(); + current_position_error = position_error.norm(); + + Quaternion result_quat = prev_state.pose().rotation().toQuaternion(); + Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5)); + Quaternion quat_error = result_quat * gps_quat.inverse(); + quat_error.normalize(); + Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2, + quat_error.z() * 2); + current_orientation_error = euler_angle_error.norm(); + + // display statistics + cout << "Position error:" << current_position_error << "\t " + << "Angular error:" << current_orientation_error << "\n" + << endl; + + fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", + output_time, result_position(0), result_position(1), + result_position(2), result_quat.x(), result_quat.y(), + result_quat.z(), result_quat.w(), gps(0), gps(1), gps(2), + gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w()); + + output_time += 1.0; + + } else { + cerr << "ERROR parsing file\n"; + return 1; + } + } + fclose(fp_out); + cout << "Complete, results written to " << output_filename << "\n\n"; + + return 0; +} diff --git a/examples/ImuFactorsExample.cpp b/examples/ImuFactorsExample.cpp index 63355631b..a8a9715e0 100644 --- a/examples/ImuFactorsExample.cpp +++ b/examples/ImuFactorsExample.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file imuFactorsExample + * @file ImuFactorsExample * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor * navigation code. * @author Garrett (ghemann@gmail.com), Luca Carlone @@ -31,32 +31,26 @@ * Note that for GPS correction, we're only using the position not the * rotation. The rotation is provided in the file for ground truth comparison. * - * Usage: ./ImuFactorsExample [data_csv_path] [-c] - * optional arguments: - * data_csv_path path to the CSV file with the IMU data. - * -c use CombinedImuFactor - * Note: Define USE_LM to use Levenberg Marquardt Optimizer - * By default ISAM2 is used + * See usage: ./ImuFactorsExample --help */ +#include + // GTSAM related includes. +#include #include #include #include -#include -#include +#include #include #include -#include -#include +#include +#include #include #include #include -// Uncomment the following to use Levenberg Marquardt Optimizer -// #define USE_LM - using namespace gtsam; using namespace std; @@ -64,45 +58,87 @@ using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) using symbol_shorthand::V; // Vel (xdot,ydot,zdot) using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) -static const char output_filename[] = "imuFactorExampleResults.csv"; -static const char use_combined_imu_flag[3] = "-c"; +namespace po = boost::program_options; + +po::variables_map parseOptions(int argc, char* argv[]) { + po::options_description desc; + desc.add_options()("help,h", "produce help message")( + "data_csv_path", po::value()->default_value("imuAndGPSdata.csv"), + "path to the CSV file with the IMU data")( + "output_filename", + po::value()->default_value("imuFactorExampleResults.csv"), + "path to the result file to use")("use_isam", po::bool_switch(), + "use ISAM as the optimizer"); + + po::variables_map vm; + po::store(po::parse_command_line(argc, argv, desc), vm); + + if (vm.count("help")) { + cout << desc << "\n"; + exit(1); + } + + return vm; +} + +boost::shared_ptr imuParams() { + // We use the sensor specs to build the noise model for the IMU factor. + double accel_noise_sigma = 0.0003924; + double gyro_noise_sigma = 0.000205689024915; + double accel_bias_rw_sigma = 0.004905; + double gyro_bias_rw_sigma = 0.000001454441043; + Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2); + Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2); + Matrix33 integration_error_cov = + I_3x3 * 1e-8; // error committed in integrating position from velocities + Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); + Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); + Matrix66 bias_acc_omega_int = + I_6x6 * 1e-5; // error in the bias used for preintegration + + auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); + // PreintegrationBase params: + p->accelerometerCovariance = + measured_acc_cov; // acc white noise in continuous + p->integrationCovariance = + integration_error_cov; // integration uncertainty continuous + // should be using 2nd order integration + // PreintegratedRotation params: + p->gyroscopeCovariance = + measured_omega_cov; // gyro white noise in continuous + // PreintegrationCombinedMeasurements params: + p->biasAccCovariance = bias_acc_cov; // acc bias in continuous + p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous + p->biasAccOmegaInt = bias_acc_omega_int; + + return p; +} int main(int argc, char* argv[]) { - string data_filename; - bool use_combined_imu = false; + string data_filename, output_filename; -#ifndef USE_LM - printf("Using ISAM2\n"); - ISAM2Params parameters; - parameters.relinearizeThreshold = 0.01; - parameters.relinearizeSkip = 1; - ISAM2 isam2(parameters); -#else - printf("Using Levenberg Marquardt Optimizer\n"); -#endif + bool use_isam = false; + + po::variables_map var_map = parseOptions(argc, argv); + + data_filename = findExampleDataFile(var_map["data_csv_path"].as()); + output_filename = var_map["output_filename"].as(); + use_isam = var_map["use_isam"].as(); + + ISAM2* isam2; + if (use_isam) { + printf("Using ISAM2\n"); + ISAM2Params parameters; + parameters.relinearizeThreshold = 0.01; + parameters.relinearizeSkip = 1; + isam2 = new ISAM2(parameters); - if (argc < 2) { - printf("using default CSV file\n"); - data_filename = findExampleDataFile("imuAndGPSdata.csv"); - } else if (argc < 3) { - if (strcmp(argv[1], use_combined_imu_flag) == 0) { - printf("using CombinedImuFactor\n"); - use_combined_imu = true; - printf("using default CSV file\n"); - data_filename = findExampleDataFile("imuAndGPSdata.csv"); - } else { - data_filename = argv[1]; - } } else { - data_filename = argv[1]; - if (strcmp(argv[2], use_combined_imu_flag) == 0) { - printf("using CombinedImuFactor\n"); - use_combined_imu = true; - } + printf("Using Levenberg Marquardt Optimizer\n"); } // Set up output file for plotting errors - FILE* fp_out = fopen(output_filename, "w+"); + FILE* fp_out = fopen(output_filename.c_str(), "w+"); fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx," "gt_qy,gt_qz,gt_qw\n"); @@ -118,10 +154,10 @@ int main(int argc, char* argv[]) { getline(file, value, ','); // i for (int i = 0; i < 9; i++) { getline(file, value, ','); - initial_state(i) = atof(value.c_str()); + initial_state(i) = stof(value.c_str()); } getline(file, value, '\n'); - initial_state(9) = atof(value.c_str()); + initial_state(9) = stof(value.c_str()); cout << "initial state:\n" << initial_state.transpose() << "\n\n"; // Assemble initial quaternion through GTSAM constructor @@ -152,43 +188,11 @@ int main(int argc, char* argv[]) { graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model); graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model); - // We use the sensor specs to build the noise model for the IMU factor. - double accel_noise_sigma = 0.0003924; - double gyro_noise_sigma = 0.000205689024915; - double accel_bias_rw_sigma = 0.004905; - double gyro_bias_rw_sigma = 0.000001454441043; - Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2); - Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2); - Matrix33 integration_error_cov = - I_3x3 * 1e-8; // error committed in integrating position from velocities - Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2); - Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2); - Matrix66 bias_acc_omega_int = - I_6x6 * 1e-5; // error in the bias used for preintegration + auto p = imuParams(); - auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0); - // PreintegrationBase params: - p->accelerometerCovariance = - measured_acc_cov; // acc white noise in continuous - p->integrationCovariance = - integration_error_cov; // integration uncertainty continuous - // should be using 2nd order integration - // PreintegratedRotation params: - p->gyroscopeCovariance = - measured_omega_cov; // gyro white noise in continuous - // PreintegrationCombinedMeasurements params: - p->biasAccCovariance = bias_acc_cov; // acc bias in continuous - p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous - p->biasAccOmegaInt = bias_acc_omega_int; + std::shared_ptr preintegrated = + std::make_shared(p, prior_imu_bias); - std::shared_ptr preintegrated = nullptr; - if (use_combined_imu) { - preintegrated = - std::make_shared(p, prior_imu_bias); - } else { - preintegrated = - std::make_shared(p, prior_imu_bias); - } assert(preintegrated); // Store previous state for imu integration and latest predicted outcome. @@ -207,16 +211,16 @@ int main(int argc, char* argv[]) { while (file.good()) { // Parse out first value getline(file, value, ','); - int type = atoi(value.c_str()); + int type = stoi(value.c_str()); if (type == 0) { // IMU measurement Vector6 imu; for (int i = 0; i < 5; ++i) { getline(file, value, ','); - imu(i) = atof(value.c_str()); + imu(i) = stof(value.c_str()); } getline(file, value, '\n'); - imu(5) = atof(value.c_str()); + imu(5) = stof(value.c_str()); // Adding the IMU preintegration. preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt); @@ -225,35 +229,24 @@ int main(int argc, char* argv[]) { Vector7 gps; for (int i = 0; i < 6; ++i) { getline(file, value, ','); - gps(i) = atof(value.c_str()); + gps(i) = stof(value.c_str()); } getline(file, value, '\n'); - gps(6) = atof(value.c_str()); + gps(6) = stof(value.c_str()); correction_count++; // Adding IMU factor and GPS factor and optimizing. - if (use_combined_imu) { - auto preint_imu_combined = - dynamic_cast( - *preintegrated); - CombinedImuFactor imu_factor( - X(correction_count - 1), V(correction_count - 1), - X(correction_count), V(correction_count), B(correction_count - 1), - B(correction_count), preint_imu_combined); - graph->add(imu_factor); - } else { - auto preint_imu = - dynamic_cast(*preintegrated); - ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1), - X(correction_count), V(correction_count), - B(correction_count - 1), preint_imu); - graph->add(imu_factor); - imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); - graph->add(BetweenFactor( - B(correction_count - 1), B(correction_count), zero_bias, - bias_noise_model)); - } + auto preint_imu = + dynamic_cast(*preintegrated); + ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1), + X(correction_count), V(correction_count), + B(correction_count - 1), preint_imu); + graph->add(imu_factor); + imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); + graph->add(BetweenFactor( + B(correction_count - 1), B(correction_count), zero_bias, + bias_noise_model)); auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0); GPSFactor gps_factor(X(correction_count), @@ -270,18 +263,21 @@ int main(int argc, char* argv[]) { initial_values.insert(B(correction_count), prev_bias); Values result; -#ifdef USE_LM - LevenbergMarquardtOptimizer optimizer(*graph, initial_values); - result = optimizer.optimize(); -#else - isam2.update(*graph, initial_values); - isam2.update(); - result = isam2.calculateEstimate(); - // reset the graph - graph->resize(0); - initial_values.clear(); -#endif + if (use_isam) { + isam2->update(*graph, initial_values); + isam2->update(); + result = isam2->calculateEstimate(); + + // reset the graph + graph->resize(0); + initial_values.clear(); + + } else { + LevenbergMarquardtOptimizer optimizer(*graph, initial_values); + result = optimizer.optimize(); + } + // Overwrite the beginning of the preintegration for the next step. prev_state = NavState(result.at(X(correction_count)), result.at(V(correction_count))); diff --git a/examples/ImuFactorExample2.cpp b/examples/ImuFactorsExample2.cpp similarity index 89% rename from examples/ImuFactorExample2.cpp rename to examples/ImuFactorsExample2.cpp index cb3650421..0031acbe8 100644 --- a/examples/ImuFactorExample2.cpp +++ b/examples/ImuFactorsExample2.cpp @@ -1,3 +1,19 @@ +/* ---------------------------------------------------------------------------- + + * 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 ImuFactorExample2 + * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor with ISAM2. + * @author Robert Truax + */ #include #include diff --git a/python/gtsam/examples/ImuFactorISAM2Example.py b/python/gtsam/examples/ImuFactorISAM2Example.py index 98f6218ea..bb90b95bf 100644 --- a/python/gtsam/examples/ImuFactorISAM2Example.py +++ b/python/gtsam/examples/ImuFactorISAM2Example.py @@ -1,6 +1,6 @@ """ -iSAM2 example with ImuFactor. -Author: Robert Truax (C++), Frank Dellaert, Varun Agrawal +ImuFactor example with iSAM2. +Authors: Robert Truax (C++), Frank Dellaert, Varun Agrawal (Python) """ # pylint: disable=invalid-name, E1101 @@ -8,9 +8,11 @@ from __future__ import print_function import math -import gtsam import matplotlib.pyplot as plt import numpy as np +from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 + +import gtsam from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, ConstantTwistScenario, ImuFactor, NonlinearFactorGraph, PinholeCameraCal3_S2, Point3, Pose3, @@ -18,7 +20,6 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2, PriorFactorVector, Rot3, Values) from gtsam.symbol_shorthand import B, V, X from gtsam.utils import plot -from mpl_toolkits.mplot3d import Axes3D # pylint: disable=W0611 def vector3(x, y, z): @@ -58,7 +59,6 @@ def get_camera(radius): def get_scenario(radius, pose_0, angular_velocity, delta_t): """Create the set of ground-truth landmarks and poses""" - angular_velocity_vector = vector3(0, -angular_velocity, 0) linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) scenario = ConstantTwistScenario( diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index e308bc933..b54919bec 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -111,7 +111,7 @@ class PreintegrationExample(object): def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01): # plot ground truth pose, as well as prediction from integrated IMU measurements actualPose = self.scenario.pose(t) - plot_pose3(POSES_FIG, actualPose, 0.3) + plot_pose3(POSES_FIG, actualPose, scale) t = actualPose.translation() self.maxDim = max([max(np.abs(t)), self.maxDim]) ax = plt.gca()