diff --git a/CMakeLists.txt b/CMakeLists.txt index 9dce69903..aa6082cb3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -166,7 +166,7 @@ if(MSVC AND BUILD_SHARED_LIBS) endif() # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such. -set(BOOST_FIND_MINIMUM_VERSION 1.43) +set(BOOST_FIND_MINIMUM_VERSION 1.58) set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex) find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS}) @@ -174,7 +174,7 @@ find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM # Required components if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY) - message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.") + message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.") endif() option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF) @@ -490,7 +490,7 @@ set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION # Deb-package specific cpack set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev") -set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") +set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)") ############################################################################### diff --git a/INSTALL.md b/INSTALL.md index b8f73f153..cf66766a1 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -13,7 +13,7 @@ $ make install ## Important Installation Notes 1. GTSAM requires the following libraries to be installed on your system: - - BOOST version 1.43 or greater (install through Linux repositories or MacPorts) + - BOOST version 1.58 or greater (install through Linux repositories or MacPorts) - Cmake version 3.0 or higher - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher diff --git a/README.md b/README.md index 99903f0b9..cc8af7248 100644 --- a/README.md +++ b/README.md @@ -13,6 +13,8 @@ mapping (SAM) in robotics and vision, using Factor Graphs and Bayes Networks as the underlying computing paradigm rather than sparse matrices. +The current support matrix is: + | Platform | Compiler | Build Status | |:------------:|:---------:|:-------------:| | Ubuntu 18.04 | gcc/clang | ![Linux CI](https://github.com/borglab/gtsam/workflows/Linux%20CI/badge.svg) | @@ -38,7 +40,7 @@ $ make install Prerequisites: -- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`) +- [Boost](http://www.boost.org/users/download/) >= 1.58 (Ubuntu: `sudo apt-get install libboost-all-dev`) - [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`) - A modern compiler, i.e., at least gcc 4.7.3 on Linux. 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/gtsam/base/DSFMap.h b/gtsam/base/DSFMap.h index 1d6bfdefa..3a9d21aad 100644 --- a/gtsam/base/DSFMap.h +++ b/gtsam/base/DSFMap.h @@ -21,6 +21,7 @@ #include // Provides size_t #include #include +#include namespace gtsam { @@ -120,4 +121,12 @@ class IndexPair : public std::pair { inline size_t i() const { return first; }; inline size_t j() const { return second; }; }; + +typedef std::vector IndexPairVector; +typedef std::set IndexPairSet; + +inline IndexPairVector IndexPairSetAsArray(IndexPairSet& set) { return IndexPairVector(set.begin(), set.end()); } + +typedef std::map IndexPairSetMap; +typedef DSFMap DSFMapIndexPair; } // namespace gtsam diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 33d120a85..ae69fc57f 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -154,12 +154,23 @@ namespace gtsam { static Rot3 Rz(double t); /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. - static Rot3 RzRyRx(double x, double y, double z); + static Rot3 RzRyRx(double x, double y, double z, + OptionalJacobian<3, 1> Hx = boost::none, + OptionalJacobian<3, 1> Hy = boost::none, + OptionalJacobian<3, 1> Hz = boost::none); /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. - inline static Rot3 RzRyRx(const Vector& xyz) { + inline static Rot3 RzRyRx(const Vector& xyz, + OptionalJacobian<3, 3> H = boost::none) { assert(xyz.size() == 3); - return RzRyRx(xyz(0), xyz(1), xyz(2)); + Rot3 out; + if (H) { + Vector3 Hx, Hy, Hz; + out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz); + (*H) << Hx, Hy, Hz; + } else + out = RzRyRx(xyz(0), xyz(1), xyz(2)); + return out; } /// Positive yaw is to right (as in aircraft heading). See ypr @@ -185,7 +196,12 @@ namespace gtsam { * Positive pitch is down (decreasing aircraft altitude). * Positive roll is to right (decreasing yaw in aircraft). */ - static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);} + static Rot3 Ypr(double y, double p, double r, + OptionalJacobian<3, 1> Hy = boost::none, + OptionalJacobian<3, 1> Hp = boost::none, + OptionalJacobian<3, 1> Hr = boost::none) { + return RzRyRx(r, p, y, Hr, Hp, Hy); + } /** Create from Quaternion coefficients */ static Rot3 Quaternion(double w, double x, double y, double z) { diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 46a07e50a..500941a16 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -82,7 +82,8 @@ Rot3 Rot3::Rz(double t) { /* ************************************************************************* */ // Considerably faster than composing matrices above ! -Rot3 Rot3::RzRyRx(double x, double y, double z) { +Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx, + OptionalJacobian<3, 1> Hy, OptionalJacobian<3, 1> Hz) { double cx=cos(x),sx=sin(x); double cy=cos(y),sy=sin(y); double cz=cos(z),sz=sin(z); @@ -97,6 +98,9 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) { double s_c = sx * cz; double c_c = cx * cz; double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; + if (Hx) (*Hx) << 1, 0, 0; + if (Hy) (*Hy) << 0, cx, -sx; + if (Hz) (*Hz) << -sy, sc_, cc_; return Rot3( _cc,- c_s + ssc, s_s + csc, _cs, c_c + sss, -s_c + css, diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index d609c289c..6e1871c64 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -67,10 +67,23 @@ namespace gtsam { } /* ************************************************************************* */ - Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3( - gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * - gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * - gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); + Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx, + OptionalJacobian<3, 1> Hy, OptionalJacobian<3, 1> Hz) { + if (Hx) (*Hx) << 1, 0, 0; + + if (Hy or Hz) { + const auto cx = cos(x), sx = sin(x); + if (Hy) (*Hy) << 0, cx, -sx; + if (Hz) { + const auto cy = cos(y), sy = sin(y); + (*Hz) << -sy, sx * cy, cx * cy; + } + } + + return Rot3( + gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * + gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * + gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index d5400494e..fba84c724 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -743,6 +743,57 @@ TEST(Rot3, axisAngle) { CHECK_AXIS_ANGLE(_axis, theta165, Rot3::AxisAngle(axis, theta195)) } +/* ************************************************************************* */ +Rot3 RzRyRx_proxy(double const& a, double const& b, double const& c) { + return Rot3::RzRyRx(a, b, c); +} + +TEST(Rot3, RzRyRx_scalars_derivative) { + const auto x = 0.1, y = 0.4, z = 0.2; + const auto num_x = numericalDerivative31(RzRyRx_proxy, x, y, z); + const auto num_y = numericalDerivative32(RzRyRx_proxy, x, y, z); + const auto num_z = numericalDerivative33(RzRyRx_proxy, x, y, z); + + Vector3 act_x, act_y, act_z; + Rot3::RzRyRx(x, y, z, act_x, act_y, act_z); + + CHECK(assert_equal(num_x, act_x)); + CHECK(assert_equal(num_y, act_y)); + CHECK(assert_equal(num_z, act_z)); +} + +/* ************************************************************************* */ +Rot3 RzRyRx_proxy(Vector3 const& xyz) { return Rot3::RzRyRx(xyz); } + +TEST(Rot3, RzRyRx_vector_derivative) { + const auto xyz = Vector3{-0.3, 0.1, 0.7}; + const auto num = numericalDerivative11(RzRyRx_proxy, xyz); + + Matrix3 act; + Rot3::RzRyRx(xyz, act); + + CHECK(assert_equal(num, act)); +} + +/* ************************************************************************* */ +Rot3 Ypr_proxy(double const& y, double const& p, double const& r) { + return Rot3::Ypr(y, p, r); +} + +TEST(Rot3, Ypr_derivative) { + const auto y = 0.7, p = -0.3, r = 0.1; + const auto num_y = numericalDerivative31(Ypr_proxy, y, p, r); + const auto num_p = numericalDerivative32(Ypr_proxy, y, p, r); + const auto num_r = numericalDerivative33(Ypr_proxy, y, p, r); + + Vector3 act_y, act_p, act_r; + Rot3::Ypr(y, p, r, act_y, act_p, act_r); + + CHECK(assert_equal(num_y, act_y)); + CHECK(assert_equal(num_p, act_p)); + CHECK(assert_equal(num_r, act_r)); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index f778fd742..6c61f3008 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -259,11 +259,59 @@ class IndexPair { size_t j() const; }; -template -class DSFMap { - DSFMap(); - KEY find(const KEY& key) const; - void merge(const KEY& x, const KEY& y); +// template +// class DSFMap { +// DSFMap(); +// KEY find(const KEY& key) const; +// void merge(const KEY& x, const KEY& y); +// std::map sets(); +// }; + +class IndexPairSet { + IndexPairSet(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + void insert(gtsam::IndexPair key); + bool erase(gtsam::IndexPair key); // returns true if value was removed + bool count(gtsam::IndexPair key) const; // returns true if value exists +}; + +class IndexPairVector { + IndexPairVector(); + IndexPairVector(const gtsam::IndexPairVector& other); + + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPair at(size_t i) const; + void push_back(gtsam::IndexPair key) const; +}; + +gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); + +class IndexPairSetMap { + IndexPairSetMap(); + // common STL methods + size_t size() const; + bool empty() const; + void clear(); + + // structure specific methods + gtsam::IndexPairSet at(gtsam::IndexPair& key); +}; + +class DSFMapIndexPair { + DSFMapIndexPair(); + gtsam::IndexPair find(const gtsam::IndexPair& key) const; + void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y); + gtsam::IndexPairSetMap sets(); }; #include @@ -2800,7 +2848,6 @@ class SfmData { gtsam::SfmTrack track(size_t idx) const; }; -string findExampleDataFile(string name); pair load2D(string filename, gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart); pair load2D(string filename, @@ -2925,6 +2972,7 @@ class ShonanAveragingParameters2 { void setOptimalityThreshold(double value); double getOptimalityThreshold() const; void setAnchor(size_t index, const gtsam::Rot2& value); + pair getAnchor(); void setAnchorWeight(double value); double getAnchorWeight() const; void setKarcherWeight(double value); @@ -2940,6 +2988,7 @@ class ShonanAveragingParameters3 { void setOptimalityThreshold(double value); double getOptimalityThreshold() const; void setAnchor(size_t index, const gtsam::Rot3& value); + pair getAnchor(); void setAnchorWeight(double value); double getAnchorWeight() const; void setKarcherWeight(double value); diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 1d191416f..0181ea45f 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -89,8 +89,8 @@ Matrix7 NavState::matrix() const { //------------------------------------------------------------------------------ ostream& operator<<(ostream& os, const NavState& state) { os << "R: " << state.attitude() << "\n"; - os << "p: " << state.position() << "\n"; - os << "v: " << Point3(state.velocity()); + os << "p: " << state.position().transpose() << "\n"; + os << "v: " << state.velocity().transpose(); return os; } @@ -218,28 +218,37 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); - Vector9 xi; - Matrix3 D_dP_R; - dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0); - dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper - dV(xi) << ((-2.0 * dt) * omega_cross_vel); + // Get perturbations in nav frame + Vector9 n_xi, xi; + Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav; + dR(n_xi) << ((-dt) * omega); + dP(n_xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper + dV(n_xi) << ((-2.0 * dt) * omega_cross_vel); if (secondOrder) { const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t)); - dP(xi) -= (0.5 * dt2) * omega_cross2_t; - dV(xi) -= dt * omega_cross2_t; + dP(n_xi) -= (0.5 * dt2) * omega_cross2_t; + dV(n_xi) -= dt * omega_cross2_t; } + + // Transform n_xi into the body frame + xi << nRb.unrotate(dR(n_xi), H ? &D_dR_R : 0, H ? &D_body_nav : 0), + nRb.unrotate(dP(n_xi), H ? &D_dP_R : 0), + nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0); + if (H) { H->setZero(); const Matrix3 Omega = skewSymmetric(omega); const Matrix3 D_cross_state = Omega * R(); H->setZero(); - D_R_R(H) << D_dP_R; - D_t_v(H) << (-dt2) * D_cross_state; - D_v_v(H) << (-2.0 * dt) * D_cross_state; + D_R_R(H) << D_dR_R; + D_t_v(H) << D_body_nav * (-dt2) * D_cross_state; + D_t_R(H) << D_dP_R; + D_v_v(H) << D_body_nav * (-2.0 * dt) * D_cross_state; + D_v_R(H) << D_dV_R; if (secondOrder) { const Matrix3 D_cross2_state = Omega * D_cross_state; - D_t_t(H) -= (0.5 * dt2) * D_cross2_state; - D_v_t(H) -= dt * D_cross2_state; + D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= D_body_nav * dt * D_cross2_state; } } return xi; diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index d38b76255..5bafe4392 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -192,6 +192,49 @@ TEST(NavState, Coriolis2) { EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH)); } +TEST(NavState, Coriolis3) { + /** Consider a massless planet with an attached nav frame at + * n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with + * velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously + * aligned with the nav frame (i.e., nRb != I_3x3). Test that first and + * second order Coriolis corrections are as expected. + */ + + // Get true first and second order coriolis accelerations + double dt = 2.0, dt2 = dt * dt; + Vector3 n_omega(0.0, 0.0, 1.0), n_t(1.0, 0.0, 0.0), n_v(0.0, 1.0, 0.0); + Vector3 n_aCorr1 = -2.0 * n_omega.cross(n_v), + n_aCorr2 = -n_omega.cross(n_omega.cross(n_t)); + Rot3 nRb = Rot3(-1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0), + bRn = nRb.inverse(); + + // Get expected first and second order corrections in the nav frame + Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, + n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2), + n_dV1e = dt * n_aCorr1, + n_dV2e = dt * (n_aCorr1 + n_aCorr2); + + // Get expected first and second order corrections in the body frame + Vector3 dRe = -dt * (bRn * n_omega), + b_dP1e = bRn * n_dP1e, b_dP2e = bRn * n_dP2e, + b_dV1e = bRn * n_dV1e, b_dV2e = bRn * n_dV2e; + + // Get actual first and scond order corrections in body frame + NavState kState2(nRb, n_t, n_v); + Vector9 dXi1a = kState2.coriolis(dt, n_omega, false), + dXi2a = kState2.coriolis(dt, n_omega, true); + Vector3 dRa = NavState::dR(dXi1a), + b_dP1a = NavState::dP(dXi1a), b_dV1a = NavState::dV(dXi1a), + b_dP2a = NavState::dP(dXi2a), b_dV2a = NavState::dV(dXi2a); + + EXPECT(assert_equal(dRe, dRa)); + EXPECT(assert_equal(b_dP1e, b_dP1a)); + EXPECT(assert_equal(b_dV1e, b_dV1a)); + EXPECT(assert_equal(b_dP2e, b_dP2a)); + EXPECT(assert_equal(b_dV2e, b_dV2a)); + +} + /* ************************************************************************* */ TEST(NavState, CorrectPIM) { Vector9 xi; @@ -215,7 +258,7 @@ TEST(NavState, Stream) os << state; string expected; - expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0\n0\n0\nv: 0\n0\n0"; + expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0 0 0\nv: 0 0 0"; EXPECT(os.str() == expected); } diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 5f6cdcc98..0b876f376 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -205,42 +205,49 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values, // Create factors and variable connections for(size_t i = 0; i < size(); ++i) { const NonlinearFactor::shared_ptr& factor = at(i); - if(formatting.plotFactorPoints) { + // If null pointer, move on to the next + if (!factor) { + continue; + } + + if (formatting.plotFactorPoints) { const KeyVector& keys = factor->keys(); - if (formatting.binaryEdges && keys.size()==2) { - stm << " var" << keys[0] << "--" << "var" << keys[1] << ";\n"; + if (formatting.binaryEdges && keys.size() == 2) { + stm << " var" << keys[0] << "--" + << "var" << keys[1] << ";\n"; } else { // Make each factor a dot stm << " factor" << i << "[label=\"\", shape=point"; { - map::const_iterator pos = formatting.factorPositions.find(i); - if(pos != formatting.factorPositions.end()) - stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << "," - << formatting.scale*(pos->second.y() - minY) << "!\""; + map::const_iterator pos = + formatting.factorPositions.find(i); + if (pos != formatting.factorPositions.end()) + stm << ", pos=\"" << formatting.scale * (pos->second.x() - minX) + << "," << formatting.scale * (pos->second.y() - minY) + << "!\""; } stm << "];\n"; // Make factor-variable connections - if(formatting.connectKeysToFactor && factor) { - for(Key key: *factor) { - stm << " var" << key << "--" << "factor" << i << ";\n"; + if (formatting.connectKeysToFactor && factor) { + for (Key key : *factor) { + stm << " var" << key << "--" + << "factor" << i << ";\n"; } } } - } - else { - if(factor) { - Key k; - bool firstTime = true; - for(Key key: *this->at(i)) { - if(firstTime) { - k = key; - firstTime = false; - continue; - } - stm << " var" << key << "--" << "var" << k << ";\n"; + } else { + Key k; + bool firstTime = true; + for (Key key : *this->at(i)) { + if (firstTime) { k = key; + firstTime = false; + continue; } + stm << " var" << key << "--" + << "var" << k << ";\n"; + k = key; } } } diff --git a/gtsam/sfm/ShonanAveraging.cpp b/gtsam/sfm/ShonanAveraging.cpp index 2485418cf..df2d72c28 100644 --- a/gtsam/sfm/ShonanAveraging.cpp +++ b/gtsam/sfm/ShonanAveraging.cpp @@ -16,26 +16,25 @@ * @brief Shonan Averaging algorithm */ -#include - +#include #include #include #include #include #include +#include +#include #include #include #include -#include #include -#include - #include #include #include #include #include +#include #include namespace gtsam { @@ -50,8 +49,11 @@ template ShonanAveragingParameters::ShonanAveragingParameters( const LevenbergMarquardtParams &_lm, const std::string &method, double optimalityThreshold, double alpha, double beta, double gamma) - : lm(_lm), optimalityThreshold(optimalityThreshold), alpha(alpha), - beta(beta), gamma(gamma) { + : lm(_lm), + optimalityThreshold(optimalityThreshold), + alpha(alpha), + beta(beta), + gamma(gamma) { // By default, we will do conjugate gradient lm.linearSolverType = LevenbergMarquardtParams::Iterative; @@ -92,29 +94,40 @@ template struct ShonanAveragingParameters<3>; /* ************************************************************************* */ // Calculate number of unknown rotations referenced by measurements +// Throws exception of keys are not numbered as expected (may change in future). template -static size_t -NrUnknowns(const typename ShonanAveraging::Measurements &measurements) { +static size_t NrUnknowns( + const typename ShonanAveraging::Measurements &measurements) { + Key maxKey = 0; std::set keys; for (const auto &measurement : measurements) { - keys.insert(measurement.key1()); - keys.insert(measurement.key2()); + for (const Key &key : measurement.keys()) { + maxKey = std::max(key, maxKey); + keys.insert(key); + } } - return keys.size(); + size_t N = keys.size(); + if (maxKey != N - 1) { + throw std::invalid_argument( + "As of now, ShonanAveraging expects keys numbered 0..N-1"); + } + return N; } /* ************************************************************************* */ template ShonanAveraging::ShonanAveraging(const Measurements &measurements, const Parameters ¶meters) - : parameters_(parameters), measurements_(measurements), + : parameters_(parameters), + measurements_(measurements), nrUnknowns_(NrUnknowns(measurements)) { for (const auto &measurement : measurements_) { const auto &model = measurement.noiseModel(); if (model && model->dim() != SO::dimension) { measurement.print("Factor with incorrect noise model:\n"); - throw std::invalid_argument("ShonanAveraging: measurements passed to " - "constructor have incorrect dimension."); + throw std::invalid_argument( + "ShonanAveraging: measurements passed to " + "constructor have incorrect dimension."); } } Q_ = buildQ(); @@ -196,7 +209,7 @@ Matrix ShonanAveraging::StiefelElementMatrix(const Values &values) { Matrix S(p, N * d); for (const auto it : values.filter()) { S.middleCols(it.key * d) = - it.value.matrix().leftCols(); // project Qj to Stiefel + it.value.matrix().leftCols(); // project Qj to Stiefel } return S; } @@ -227,7 +240,8 @@ Values ShonanAveraging<3>::projectFrom(size_t p, const Values &values) const { } /* ************************************************************************* */ -template static Matrix RoundSolutionS(const Matrix &S) { +template +static Matrix RoundSolutionS(const Matrix &S) { const size_t N = S.cols() / d; // First, compute a thin SVD of S Eigen::JacobiSVD svd(S, Eigen::ComputeThinV); @@ -246,8 +260,7 @@ template static Matrix RoundSolutionS(const Matrix &S) { for (size_t i = 0; i < N; ++i) { // Compute the determinant of the ith dxd block of R double determinant = R.middleCols(d * i).determinant(); - if (determinant > 0) - ++numPositiveBlocks; + if (determinant > 0) ++numPositiveBlocks; } if (numPositiveBlocks < N / 2) { @@ -263,7 +276,8 @@ template static Matrix RoundSolutionS(const Matrix &S) { } /* ************************************************************************* */ -template <> Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const { +template <> +Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const { // Round to a 2*2N matrix Matrix R = RoundSolutionS<2>(S); @@ -276,7 +290,8 @@ template <> Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const { return values; } -template <> Values ShonanAveraging<3>::roundSolutionS(const Matrix &S) const { +template <> +Values ShonanAveraging<3>::roundSolutionS(const Matrix &S) const { // Round to a 3*3N matrix Matrix R = RoundSolutionS<3>(S); @@ -332,7 +347,8 @@ static double Kappa(const BinaryMeasurement &measurement) { } /* ************************************************************************* */ -template Sparse ShonanAveraging::buildD() const { +template +Sparse ShonanAveraging::buildD() const { // Each measurement contributes 2*d elements along the diagonal of the // degree matrix. static constexpr size_t stride = 2 * d; @@ -366,7 +382,8 @@ template Sparse ShonanAveraging::buildD() const { } /* ************************************************************************* */ -template Sparse ShonanAveraging::buildQ() const { +template +Sparse ShonanAveraging::buildQ() const { // Each measurement contributes 2*d^2 elements on a pair of symmetric // off-diagonal blocks static constexpr size_t stride = 2 * d * d; @@ -513,12 +530,12 @@ struct MatrixProdFunctor { // - We've been using 10^-4 for the nonnegativity tolerance // - for numLanczosVectors, 20 is a good default value -static bool -SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue, - Vector *minEigenVector = 0, size_t *numIterations = 0, - size_t maxIterations = 1000, - double minEigenvalueNonnegativityTolerance = 10e-4, - Eigen::Index numLanczosVectors = 20) { +static bool SparseMinimumEigenValue( + const Sparse &A, const Matrix &S, double *minEigenValue, + Vector *minEigenVector = 0, size_t *numIterations = 0, + size_t maxIterations = 1000, + double minEigenvalueNonnegativityTolerance = 10e-4, + Eigen::Index numLanczosVectors = 20) { // a. Estimate the largest-magnitude eigenvalue of this matrix using Lanczos MatrixProdFunctor lmOperator(A); Spectra::SymEigsSolvernormalize(); // Ensure that this is a unit vector + minEigenVector->normalize(); // Ensure that this is a unit vector } return true; } @@ -578,7 +594,7 @@ SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue, Vector perturbation(v0.size()); perturbation.setRandom(); perturbation.normalize(); - Vector xinit = v0 + (.03 * v0.norm()) * perturbation; // Perturb v0 by ~3% + Vector xinit = v0 + (.03 * v0.norm()) * perturbation; // Perturb v0 by ~3% // Use this to initialize the eigensolver minEigenValueSolver.init(xinit.data()); @@ -590,21 +606,20 @@ SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue, maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN); - if (minConverged != 1) - return false; + if (minConverged != 1) return false; *minEigenValue = minEigenValueSolver.eigenvalues()(0) + 2 * lmEigenValue; if (minEigenVector) { *minEigenVector = minEigenValueSolver.eigenvectors(1).col(0); - minEigenVector->normalize(); // Ensure that this is a unit vector + minEigenVector->normalize(); // Ensure that this is a unit vector } - if (numIterations) - *numIterations = minEigenValueSolver.num_iterations(); + if (numIterations) *numIterations = minEigenValueSolver.num_iterations(); return true; } /* ************************************************************************* */ -template Sparse ShonanAveraging::computeA(const Matrix &S) const { +template +Sparse ShonanAveraging::computeA(const Matrix &S) const { auto Lambda = computeLambda(S); return Lambda - Q_; } @@ -628,8 +643,8 @@ double ShonanAveraging::computeMinEigenValue(const Values &values, /* ************************************************************************* */ template -std::pair -ShonanAveraging::computeMinEigenVector(const Values &values) const { +std::pair ShonanAveraging::computeMinEigenVector( + const Values &values) const { Vector minEigenVector; double minEigenValue = computeMinEigenValue(values, &minEigenVector); return std::make_pair(minEigenValue, minEigenVector); @@ -643,20 +658,25 @@ bool ShonanAveraging::checkOptimality(const Values &values) const { } /* ************************************************************************* */ -/// Create a tangent direction xi with eigenvector segment v_i template -Vector ShonanAveraging::MakeATangentVector(size_t p, const Vector &v, - size_t i) { +VectorValues ShonanAveraging::TangentVectorValues(size_t p, + const Vector &v) { + VectorValues delta; // Create a tangent direction xi with eigenvector segment v_i const size_t dimension = SOn::Dimension(p); - const auto v_i = v.segment(d * i); - Vector xi = Vector::Zero(dimension); - double sign = pow(-1.0, round((p + 1) / 2) + 1); - for (size_t j = 0; j < d; j++) { - xi(j + p - d - 1) = sign * v_i(d - j - 1); - sign = -sign; + double sign0 = pow(-1.0, round((p + 1) / 2) + 1); + for (size_t i = 0; i < v.size() / d; i++) { + // Assumes key is 0-based integer + const auto v_i = v.segment(d * i); + Vector xi = Vector::Zero(dimension); + double sign = sign0; + for (size_t j = 0; j < d; j++) { + xi(j + p - d - 1) = sign * v_i(d - j - 1); + sign = -sign; + } + delta.insert(i, xi); } - return xi; + return delta; } /* ************************************************************************* */ @@ -690,14 +710,8 @@ template Values ShonanAveraging::LiftwithDescent(size_t p, const Values &values, const Vector &minEigenVector) { Values lifted = LiftTo(p, values); - for (auto it : lifted.filter()) { - // Create a tangent direction xi with eigenvector segment v_i - // Assumes key is 0-based integer - const Vector xi = MakeATangentVector(p, minEigenVector, it.key); - // Move the old value in the descent direction - it.value = it.value.retract(xi); - } - return lifted; + VectorValues delta = TangentVectorValues(p, minEigenVector); + return lifted.retract(delta); } /* ************************************************************************* */ @@ -750,7 +764,8 @@ Values ShonanAveraging::initializeRandomly(std::mt19937 &rng) const { } /* ************************************************************************* */ -template Values ShonanAveraging::initializeRandomly() const { +template +Values ShonanAveraging::initializeRandomly() const { return initializeRandomly(kRandomNumberGenerator); } @@ -759,7 +774,7 @@ template Values ShonanAveraging::initializeRandomlyAt(size_t p, std::mt19937 &rng) const { const Values randomRotations = initializeRandomly(rng); - return LiftTo(p, randomRotations); // lift to p! + return LiftTo(p, randomRotations); // lift to p! } /* ************************************************************************* */ @@ -823,8 +838,8 @@ ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters ¶meters) // Extract Rot3 measurement from Pose3 betweenfactors // Modeled after similar function in dataset.cpp -static BinaryMeasurement -convert(const BetweenFactor::shared_ptr &f) { +static BinaryMeasurement convert( + const BetweenFactor::shared_ptr &f) { auto gaussian = boost::dynamic_pointer_cast(f->noiseModel()); if (!gaussian) @@ -837,12 +852,11 @@ convert(const BetweenFactor::shared_ptr &f) { noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true)); } -static ShonanAveraging3::Measurements -extractRot3Measurements(const BetweenFactorPose3s &factors) { +static ShonanAveraging3::Measurements extractRot3Measurements( + const BetweenFactorPose3s &factors) { ShonanAveraging3::Measurements result; result.reserve(factors.size()); - for (auto f : factors) - result.push_back(convert(f)); + for (auto f : factors) result.push_back(convert(f)); return result; } @@ -851,4 +865,4 @@ ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors, : ShonanAveraging<3>(extractRot3Measurements(factors), parameters) {} /* ************************************************************************* */ -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/sfm/ShonanAveraging.h b/gtsam/sfm/ShonanAveraging.h index ed94329a2..edd9f33a2 100644 --- a/gtsam/sfm/ShonanAveraging.h +++ b/gtsam/sfm/ShonanAveraging.h @@ -20,36 +20,39 @@ #include #include +#include #include #include +#include #include #include #include -#include #include #include #include #include #include +#include namespace gtsam { class NonlinearFactorGraph; class LevenbergMarquardtOptimizer; /// Parameters governing optimization etc. -template struct GTSAM_EXPORT ShonanAveragingParameters { +template +struct GTSAM_EXPORT ShonanAveragingParameters { // Select Rot2 or Rot3 interface based template parameter d using Rot = typename std::conditional::type; using Anchor = std::pair; // Paremeters themselves: - LevenbergMarquardtParams lm; // LM parameters - double optimalityThreshold; // threshold used in checkOptimality - Anchor anchor; // pose to use as anchor if not Karcher - double alpha; // weight of anchor-based prior (default 0) - double beta; // weight of Karcher-based prior (default 1) - double gamma; // weight of gauge-fixing factors (default 0) + LevenbergMarquardtParams lm; // LM parameters + double optimalityThreshold; // threshold used in checkOptimality + Anchor anchor; // pose to use as anchor if not Karcher + double alpha; // weight of anchor-based prior (default 0) + double beta; // weight of Karcher-based prior (default 1) + double gamma; // weight of gauge-fixing factors (default 0) ShonanAveragingParameters(const LevenbergMarquardtParams &lm = LevenbergMarquardtParams::CeresDefaults(), @@ -64,6 +67,7 @@ template struct GTSAM_EXPORT ShonanAveragingParameters { double getOptimalityThreshold() const { return optimalityThreshold; } void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; } + std::pair getAnchor() { return anchor; } void setAnchorWeight(double value) { alpha = value; } double getAnchorWeight() { return alpha; } @@ -93,8 +97,9 @@ using ShonanAveragingParameters3 = ShonanAveragingParameters<3>; * European Computer Vision Conference, 2020. * You can view our ECCV spotlight video at https://youtu.be/5ppaqMyHtE0 */ -template class GTSAM_EXPORT ShonanAveraging { -public: +template +class GTSAM_EXPORT ShonanAveraging { + public: using Sparse = Eigen::SparseMatrix; // Define the Parameters type and use its typedef of the rotation type: @@ -105,13 +110,13 @@ public: // TODO(frank): use BinaryMeasurement? using Measurements = std::vector>; -private: + private: Parameters parameters_; Measurements measurements_; size_t nrUnknowns_; - Sparse D_; // Sparse (diagonal) degree matrix - Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr - Sparse L_; // connection Laplacian L = D - Q, needed for optimality check + Sparse D_; // Sparse (diagonal) degree matrix + Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr + Sparse L_; // connection Laplacian L = D - Q, needed for optimality check /** * Build 3Nx3N sparse matrix consisting of rotation measurements, arranged as @@ -122,7 +127,7 @@ private: /// Build 3Nx3N sparse degree matrix D Sparse buildD() const; -public: + public: /// @name Standard Constructors /// @{ @@ -156,12 +161,12 @@ public: /// @name Matrix API (advanced use, debugging) /// @{ - Sparse D() const { return D_; } ///< Sparse version of D - Matrix denseD() const { return Matrix(D_); } ///< Dense version of D - Sparse Q() const { return Q_; } ///< Sparse version of Q - Matrix denseQ() const { return Matrix(Q_); } ///< Dense version of Q - Sparse L() const { return L_; } ///< Sparse version of L - Matrix denseL() const { return Matrix(L_); } ///< Dense version of L + Sparse D() const { return D_; } ///< Sparse version of D + Matrix denseD() const { return Matrix(D_); } ///< Dense version of D + Sparse Q() const { return Q_; } ///< Sparse version of Q + Matrix denseQ() const { return Matrix(Q_); } ///< Dense version of Q + Sparse L() const { return L_; } ///< Sparse version of L + Matrix denseL() const { return Matrix(L_); } ///< Dense version of L /// Version that takes pxdN Stiefel manifold elements Sparse computeLambda(const Matrix &S) const; @@ -200,8 +205,8 @@ public: /// Project pxdN Stiefel manifold matrix S to Rot3^N Values roundSolutionS(const Matrix &S) const; - /// Create a tangent direction xi with eigenvector segment v_i - static Vector MakeATangentVector(size_t p, const Vector &v, size_t i); + /// Create a VectorValues with eigenvector v_i + static VectorValues TangentVectorValues(size_t p, const Vector &v); /// Calculate the riemannian gradient of F(values) at values Matrix riemannianGradient(size_t p, const Values &values) const; @@ -220,11 +225,10 @@ public: * @param minEigenVector corresponding to minEigenValue at level p-1 * @return values of type SO(p) */ - Values - initializeWithDescent(size_t p, const Values &values, - const Vector &minEigenVector, double minEigenValue, - double gradienTolerance = 1e-2, - double preconditionedGradNormTolerance = 1e-4) const; + Values initializeWithDescent( + size_t p, const Values &values, const Vector &minEigenVector, + double minEigenValue, double gradienTolerance = 1e-2, + double preconditionedGradNormTolerance = 1e-4) const; /// @} /// @name Advanced API /// @{ @@ -237,11 +241,11 @@ public: /** * Create initial Values of type SO(p) - * @param p the dimensionality of the rotation manifold + * @param p the dimensionality of the rotation manifold */ Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const; - /// Version of initializeRandomlyAt with fixed random seed. + /// Version of initializeRandomlyAt with fixed random seed. Values initializeRandomlyAt(size_t p) const; /** @@ -300,7 +304,8 @@ public: Values roundSolution(const Values &values) const; /// Lift Values of type T to SO(p) - template static Values LiftTo(size_t p, const Values &values) { + template + static Values LiftTo(size_t p, const Values &values) { Values result; for (const auto it : values.filter()) { result.insert(it.key, SOn::Lift(p, it.value.matrix())); @@ -327,7 +332,7 @@ public: */ Values initializeRandomly(std::mt19937 &rng) const; - /// Random initialization for wrapper, fixed random seed. + /// Random initialization for wrapper, fixed random seed. Values initializeRandomly() const; /** @@ -346,20 +351,22 @@ public: // convenience interface with file access. class ShonanAveraging2 : public ShonanAveraging<2> { -public: + public: ShonanAveraging2(const Measurements &measurements, const Parameters ¶meters = Parameters()); - ShonanAveraging2(string g2oFile, const Parameters ¶meters = Parameters()); + explicit ShonanAveraging2(string g2oFile, + const Parameters ¶meters = Parameters()); }; class ShonanAveraging3 : public ShonanAveraging<3> { -public: + public: ShonanAveraging3(const Measurements &measurements, const Parameters ¶meters = Parameters()); - ShonanAveraging3(string g2oFile, const Parameters ¶meters = Parameters()); + explicit ShonanAveraging3(string g2oFile, + const Parameters ¶meters = Parameters()); // TODO(frank): Deprecate after we land pybind wrapper ShonanAveraging3(const BetweenFactorPose3s &factors, const Parameters ¶meters = Parameters()); }; -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/sfm/tests/testShonanAveraging.cpp b/gtsam/sfm/tests/testShonanAveraging.cpp index bc1449747..1200c8ebb 100644 --- a/gtsam/sfm/tests/testShonanAveraging.cpp +++ b/gtsam/sfm/tests/testShonanAveraging.cpp @@ -121,18 +121,17 @@ TEST(ShonanAveraging3, tryOptimizingAt4) { } /* ************************************************************************* */ -TEST(ShonanAveraging3, MakeATangentVector) { +TEST(ShonanAveraging3, TangentVectorValues) { Vector9 v; v << 1, 2, 3, 4, 5, 6, 7, 8, 9; - Matrix expected(5, 5); - expected << 0, 0, 0, 0, -4, // - 0, 0, 0, 0, -5, // - 0, 0, 0, 0, -6, // - 0, 0, 0, 0, 0, // - 4, 5, 6, 0, 0; - const Vector xi_1 = ShonanAveraging3::MakeATangentVector(5, v, 1); - const auto actual = SOn::Hat(xi_1); - CHECK(assert_equal(expected, actual)); + Vector expected0(10), expected1(10), expected2(10); + expected0 << 0, 3, -2, 1, 0, 0, 0, 0, 0, 0; + expected1 << 0, 6, -5, 4, 0, 0, 0, 0, 0, 0; + expected2 << 0, 9, -8, 7, 0, 0, 0, 0, 0, 0; + const VectorValues xi = ShonanAveraging3::TangentVectorValues(5, v); + EXPECT(assert_equal(expected0, xi[0])); + EXPECT(assert_equal(expected1, xi[1])); + EXPECT(assert_equal(expected2, xi[2])); } /* ************************************************************************* */ @@ -168,7 +167,8 @@ TEST(ShonanAveraging3, CheckWithEigen) { minEigenValue = min(lambdas(i), minEigenValue); // Actual check - EXPECT_DOUBLES_EQUAL(minEigenValue, lambda, 1e-12); + EXPECT_DOUBLES_EQUAL(0, lambda, 1e-11); + EXPECT_DOUBLES_EQUAL(0, minEigenValue, 1e-11); // Construct test descent direction (as minEigenVector is not predictable // across platforms, being one from a basically flat 3d- subspace) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4bd7bc7e2..270dbeb95 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -1129,6 +1129,13 @@ bool readBAL(const string &filename, SfmData &data) { return true; } +/* ************************************************************************* */ +SfmData readBal(const string &filename) { + SfmData data; + readBAL(filename, data); + return data; +} + /* ************************************************************************* */ bool writeBAL(const string &filename, SfmData &data) { // Open the output file diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 2c1bb7a1c..91ceaa5fd 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -277,6 +277,14 @@ GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData &data); */ GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data); +/** + * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data + * as a SfmData structure. Mainly used by wrapped code. + * @param filename The name of the BAL file. + * @return SfM structure where the data is stored. + */ +GTSAM_EXPORT SfmData readBal(const std::string& filename); + /** * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a * SfmData structure diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index fa6ec905f..bec02fb64 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -4,6 +4,10 @@ if (NOT GTSAM_BUILD_PYTHON) return() endif() +# Common directory for storing data/datasets stored with the package. +# This will store the data in the Python site package directly. +set(GTSAM_PYTHON_DATASET_DIR "./gtsam/Data") + # Generate setup.py. file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS) configure_file(${PROJECT_SOURCE_DIR}/python/setup.py.in @@ -26,6 +30,8 @@ set(ignore gtsam::ISAM2ThresholdMapValue gtsam::FactorIndices gtsam::FactorIndexSet + gtsam::IndexPairSetMap + gtsam::IndexPairVector gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s gtsam::Point2Vector diff --git a/python/gtsam/__init__.py b/python/gtsam/__init__.py index b2d1fb7e7..70a00c3dc 100644 --- a/python/gtsam/__init__.py +++ b/python/gtsam/__init__.py @@ -1,4 +1,6 @@ +from . import utils from .gtsam import * +from .utils import findExampleDataFile def _init(): 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() diff --git a/python/gtsam/preamble.h b/python/gtsam/preamble.h index 9f3032dd0..6166f615e 100644 --- a/python/gtsam/preamble.h +++ b/python/gtsam/preamble.h @@ -9,3 +9,4 @@ PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector > >); PYBIND11_MAKE_OPAQUE(std::vector > >); +PYBIND11_MAKE_OPAQUE(std::vector); diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index 52fffab6b..3f6b8fa38 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -10,3 +10,5 @@ py::bind_vector >(m_, "Pose3Vector"); py::bind_vector > > >(m_, "BetweenFactorPose3s"); py::bind_vector > > >(m_, "BetweenFactorPose2s"); py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); +py::bind_map(m_, "IndexPairSetMap"); +py::bind_vector(m_, "IndexPairVector"); diff --git a/python/gtsam/tests/test_dsf_map.py b/python/gtsam/tests/test_dsf_map.py index 73ddcb050..e36657178 100644 --- a/python/gtsam/tests/test_dsf_map.py +++ b/python/gtsam/tests/test_dsf_map.py @@ -35,6 +35,20 @@ class TestDSFMap(GtsamTestCase): dsf.merge(pair1, pair2) self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2))) + def test_sets(self): + from gtsam import IndexPair + dsf = gtsam.DSFMapIndexPair() + dsf.merge(IndexPair(0, 1), IndexPair(1,2)) + dsf.merge(IndexPair(0, 1), IndexPair(3,4)) + dsf.merge(IndexPair(4,5), IndexPair(6,8)) + sets = dsf.sets() + + for i in sets: + s = sets[i] + for val in gtsam.IndexPairSetAsArray(s): + val.i() + val.j() + if __name__ == '__main__': unittest.main() diff --git a/python/gtsam/utils/__init__.py b/python/gtsam/utils/__init__.py index e69de29bb..5ee733ba4 100644 --- a/python/gtsam/utils/__init__.py +++ b/python/gtsam/utils/__init__.py @@ -0,0 +1,22 @@ +import glob +import os.path as osp + + +def findExampleDataFile(name): + """ + Find the example data file specified by `name`. + """ + # This is okay since gtsam will already be loaded + # before this function is accessed. Thus no effect. + import gtsam + + site_package_path = gtsam.__path__[0] + # add the * at the end to glob the entire directory + data_path = osp.join(site_package_path, "Data", "*") + + extensions = ("", ".graph", ".txt", ".out", ".xml", ".g2o") + + for data_file_path in glob.glob(data_path, recursive=True): + for ext in extensions: + if (name + ext) == osp.basename(data_file_path): + return data_file_path diff --git a/python/setup.py.in b/python/setup.py.in index 55431a9ad..1ffe978f3 100644 --- a/python/setup.py.in +++ b/python/setup.py.in @@ -1,3 +1,4 @@ +import glob import os import sys @@ -8,6 +9,11 @@ except ImportError: packages = find_packages(where=".") print("PACKAGES: ", packages) + +data_path = '${GTSAM_SOURCE_DIR}/examples/Data/' +data_files_and_directories = glob.glob(data_path + '**', recursive=True) +data_files = [x for x in data_files_and_directories if not os.path.isdir(x)] + package_data = { '': [ './*.so', @@ -27,6 +33,7 @@ setup( author_email='frank.dellaert@gtsam.org', license='Simplified BSD license', keywords='slam sam robotics localization mapping optimization', + long_description_content_type='text/markdown', long_description=readme_contents, # https://pypi.org/pypi?%3Aaction=list_classifiers classifiers=[ @@ -43,6 +50,7 @@ setup( ], packages=packages, package_data=package_data, + data_files=[('${GTSAM_PYTHON_DATASET_DIR}', data_files),], test_suite="gtsam.tests", install_requires=["numpy"], zip_safe=False,