Merge branch 'develop' into feature/wrap_translation_averaging
Akshay Krishnan
GitHub
commit
79827eb802
|
|
@ -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)")
|
||||
|
||||
|
||||
###############################################################################
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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 |  |
|
||||
|
|
@ -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.
|
||||
|
||||
|
|
|
|||
|
|
@ -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 <boost/program_options.hpp>
|
||||
|
||||
// GTSAM related includes.
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <gtsam/navigation/GPSFactor.h>
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
|
||||
#include <cstring>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
|
||||
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<string>()->default_value("imuAndGPSdata.csv"),
|
||||
"path to the CSV file with the IMU data")(
|
||||
"output_filename",
|
||||
po::value<string>()->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<PreintegratedCombinedMeasurements::Params> 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<string>());
|
||||
output_filename = var_map["output_filename"].as<string>();
|
||||
|
||||
// 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<Pose3>(X(index), prior_pose, pose_noise_model);
|
||||
graph.addPrior<Vector3>(V(index), prior_velocity, velocity_noise_model);
|
||||
graph.addPrior<imuBias::ConstantBias>(B(index), prior_imu_bias,
|
||||
bias_noise_model);
|
||||
|
||||
auto p = imuParams();
|
||||
|
||||
std::shared_ptr<PreintegrationType> preintegrated =
|
||||
std::make_shared<PreintegratedCombinedMeasurements>(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<const PreintegratedCombinedMeasurements&>(
|
||||
*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<Pose3>(X(index)), result.at<Vector3>(V(index)));
|
||||
prev_bias = result.at<imuBias::ConstantBias>(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;
|
||||
}
|
||||
|
|
@ -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 <boost/program_options.hpp>
|
||||
|
||||
// GTSAM related includes.
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/navigation/CombinedImuFactor.h>
|
||||
#include <gtsam/navigation/GPSFactor.h>
|
||||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
|
||||
#include <cstring>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
|
||||
// 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<string>()->default_value("imuAndGPSdata.csv"),
|
||||
"path to the CSV file with the IMU data")(
|
||||
"output_filename",
|
||||
po::value<string>()->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<PreintegratedCombinedMeasurements::Params> 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
|
||||
bool use_isam = false;
|
||||
|
||||
po::variables_map var_map = parseOptions(argc, argv);
|
||||
|
||||
data_filename = findExampleDataFile(var_map["data_csv_path"].as<string>());
|
||||
output_filename = var_map["output_filename"].as<string>();
|
||||
use_isam = var_map["use_isam"].as<bool>();
|
||||
|
||||
ISAM2* isam2;
|
||||
if (use_isam) {
|
||||
printf("Using ISAM2\n");
|
||||
ISAM2Params parameters;
|
||||
parameters.relinearizeThreshold = 0.01;
|
||||
parameters.relinearizeSkip = 1;
|
||||
ISAM2 isam2(parameters);
|
||||
#else
|
||||
printf("Using Levenberg Marquardt Optimizer\n");
|
||||
#endif
|
||||
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<PreintegrationType> preintegrated = nullptr;
|
||||
if (use_combined_imu) {
|
||||
preintegrated =
|
||||
std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);
|
||||
} else {
|
||||
preintegrated =
|
||||
std::shared_ptr<PreintegrationType> preintegrated =
|
||||
std::make_shared<PreintegratedImuMeasurements>(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,24 +229,14 @@ 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<const PreintegratedCombinedMeasurements&>(
|
||||
*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<const PreintegratedImuMeasurements&>(*preintegrated);
|
||||
ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1),
|
||||
|
|
@ -253,7 +247,6 @@ int main(int argc, char* argv[]) {
|
|||
graph->add(BetweenFactor<imuBias::ConstantBias>(
|
||||
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();
|
||||
|
||||
if (use_isam) {
|
||||
isam2->update(*graph, initial_values);
|
||||
isam2->update();
|
||||
result = isam2->calculateEstimate();
|
||||
|
||||
// reset the graph
|
||||
graph->resize(0);
|
||||
initial_values.clear();
|
||||
#endif
|
||||
|
||||
} else {
|
||||
LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
|
||||
result = optimizer.optimize();
|
||||
}
|
||||
|
||||
// Overwrite the beginning of the preintegration for the next step.
|
||||
prev_state = NavState(result.at<Pose3>(X(correction_count)),
|
||||
result.at<Vector3>(V(correction_count)));
|
||||
|
|
|
|||
|
|
@ -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 <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
|
|
@ -21,6 +21,7 @@
|
|||
#include <cstdlib> // Provides size_t
|
||||
#include <map>
|
||||
#include <set>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -120,4 +121,12 @@ class IndexPair : public std::pair<size_t,size_t> {
|
|||
inline size_t i() const { return first; };
|
||||
inline size_t j() const { return second; };
|
||||
};
|
||||
|
||||
typedef std::vector<IndexPair> IndexPairVector;
|
||||
typedef std::set<IndexPair> IndexPairSet;
|
||||
|
||||
inline IndexPairVector IndexPairSetAsArray(IndexPairSet& set) { return IndexPairVector(set.begin(), set.end()); }
|
||||
|
||||
typedef std::map<IndexPair, IndexPairSet> IndexPairSetMap;
|
||||
typedef DSFMap<IndexPair> DSFMapIndexPair;
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -67,7 +67,20 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
|
||||
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())));
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -259,11 +259,59 @@ class IndexPair {
|
|||
size_t j() const;
|
||||
};
|
||||
|
||||
template<KEY = {gtsam::IndexPair}>
|
||||
class DSFMap {
|
||||
DSFMap();
|
||||
KEY find(const KEY& key) const;
|
||||
void merge(const KEY& x, const KEY& y);
|
||||
// template<KEY = {gtsam::IndexPair}>
|
||||
// class DSFMap {
|
||||
// DSFMap();
|
||||
// KEY find(const KEY& key) const;
|
||||
// void merge(const KEY& x, const KEY& y);
|
||||
// std::map<KEY, Set> 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 <gtsam/base/Matrix.h>
|
||||
|
|
@ -2800,7 +2848,6 @@ class SfmData {
|
|||
gtsam::SfmTrack track(size_t idx) const;
|
||||
};
|
||||
|
||||
string findExampleDataFile(string name);
|
||||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
|
||||
gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart);
|
||||
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> 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<size_t, gtsam::Rot2> 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<size_t, gtsam::Rot3> getAnchor();
|
||||
void setAnchorWeight(double value);
|
||||
double getAnchorWeight() const;
|
||||
void setKarcherWeight(double value);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -205,46 +205,53 @@ 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<size_t, Point2>::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<size_t, Point2>::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) {
|
||||
} else {
|
||||
Key k;
|
||||
bool firstTime = true;
|
||||
for(Key key: *this->at(i)) {
|
||||
if(firstTime) {
|
||||
for (Key key : *this->at(i)) {
|
||||
if (firstTime) {
|
||||
k = key;
|
||||
firstTime = false;
|
||||
continue;
|
||||
}
|
||||
stm << " var" << key << "--" << "var" << k << ";\n";
|
||||
stm << " var" << key << "--"
|
||||
<< "var" << k << ";\n";
|
||||
k = key;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
stm << "}\n";
|
||||
}
|
||||
|
|
|
|||
|
|
@ -16,26 +16,25 @@
|
|||
* @brief Shonan Averaging algorithm
|
||||
*/
|
||||
|
||||
#include <gtsam/sfm/ShonanAveraging.h>
|
||||
|
||||
#include <SymEigsSolver.h>
|
||||
#include <gtsam/linear/PCGSolver.h>
|
||||
#include <gtsam/linear/SubgraphPreconditioner.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/sfm/ShonanAveraging.h>
|
||||
#include <gtsam/sfm/ShonanFactor.h>
|
||||
#include <gtsam/sfm/ShonanGaugeFactor.h>
|
||||
#include <gtsam/slam/FrobeniusFactor.h>
|
||||
#include <gtsam/slam/KarcherMeanFactor-inl.h>
|
||||
#include <gtsam/sfm/ShonanFactor.h>
|
||||
|
||||
#include <Eigen/Eigenvalues>
|
||||
#include <SymEigsSolver.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <complex>
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
#include <random>
|
||||
#include <set>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -50,8 +49,11 @@ template <size_t d>
|
|||
ShonanAveragingParameters<d>::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,28 +94,39 @@ 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 <size_t d>
|
||||
static size_t
|
||||
NrUnknowns(const typename ShonanAveraging<d>::Measurements &measurements) {
|
||||
static size_t NrUnknowns(
|
||||
const typename ShonanAveraging<d>::Measurements &measurements) {
|
||||
Key maxKey = 0;
|
||||
std::set<Key> 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 <size_t d>
|
||||
ShonanAveraging<d>::ShonanAveraging(const Measurements &measurements,
|
||||
const Parameters ¶meters)
|
||||
: parameters_(parameters), measurements_(measurements),
|
||||
: parameters_(parameters),
|
||||
measurements_(measurements),
|
||||
nrUnknowns_(NrUnknowns<d>(measurements)) {
|
||||
for (const auto &measurement : measurements_) {
|
||||
const auto &model = measurement.noiseModel();
|
||||
if (model && model->dim() != SO<d>::dimension) {
|
||||
measurement.print("Factor with incorrect noise model:\n");
|
||||
throw std::invalid_argument("ShonanAveraging: measurements passed to "
|
||||
throw std::invalid_argument(
|
||||
"ShonanAveraging: measurements passed to "
|
||||
"constructor have incorrect dimension.");
|
||||
}
|
||||
}
|
||||
|
|
@ -227,7 +240,8 @@ Values ShonanAveraging<3>::projectFrom(size_t p, const Values &values) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <size_t d> static Matrix RoundSolutionS(const Matrix &S) {
|
||||
template <size_t d>
|
||||
static Matrix RoundSolutionS(const Matrix &S) {
|
||||
const size_t N = S.cols() / d;
|
||||
// First, compute a thin SVD of S
|
||||
Eigen::JacobiSVD<Matrix> svd(S, Eigen::ComputeThinV);
|
||||
|
|
@ -246,8 +260,7 @@ template <size_t d> 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>(d * i).determinant();
|
||||
if (determinant > 0)
|
||||
++numPositiveBlocks;
|
||||
if (determinant > 0) ++numPositiveBlocks;
|
||||
}
|
||||
|
||||
if (numPositiveBlocks < N / 2) {
|
||||
|
|
@ -263,7 +276,8 @@ template <size_t d> 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<T> &measurement) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <size_t d> Sparse ShonanAveraging<d>::buildD() const {
|
||||
template <size_t d>
|
||||
Sparse ShonanAveraging<d>::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 <size_t d> Sparse ShonanAveraging<d>::buildD() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <size_t d> Sparse ShonanAveraging<d>::buildQ() const {
|
||||
template <size_t d>
|
||||
Sparse ShonanAveraging<d>::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,8 +530,8 @@ 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,
|
||||
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,
|
||||
|
|
@ -530,8 +547,7 @@ SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue,
|
|||
maxIterations, 1e-4, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
|
||||
|
||||
// Check convergence and bail out if necessary
|
||||
if (lmConverged != 1)
|
||||
return false;
|
||||
if (lmConverged != 1) return false;
|
||||
|
||||
const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0);
|
||||
|
||||
|
|
@ -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
|
||||
}
|
||||
if (numIterations)
|
||||
*numIterations = minEigenValueSolver.num_iterations();
|
||||
if (numIterations) *numIterations = minEigenValueSolver.num_iterations();
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <size_t d> Sparse ShonanAveraging<d>::computeA(const Matrix &S) const {
|
||||
template <size_t d>
|
||||
Sparse ShonanAveraging<d>::computeA(const Matrix &S) const {
|
||||
auto Lambda = computeLambda(S);
|
||||
return Lambda - Q_;
|
||||
}
|
||||
|
|
@ -628,8 +643,8 @@ double ShonanAveraging<d>::computeMinEigenValue(const Values &values,
|
|||
|
||||
/* ************************************************************************* */
|
||||
template <size_t d>
|
||||
std::pair<double, Vector>
|
||||
ShonanAveraging<d>::computeMinEigenVector(const Values &values) const {
|
||||
std::pair<double, Vector> ShonanAveraging<d>::computeMinEigenVector(
|
||||
const Values &values) const {
|
||||
Vector minEigenVector;
|
||||
double minEigenValue = computeMinEigenValue(values, &minEigenVector);
|
||||
return std::make_pair(minEigenValue, minEigenVector);
|
||||
|
|
@ -643,20 +658,25 @@ bool ShonanAveraging<d>::checkOptimality(const Values &values) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/// Create a tangent direction xi with eigenvector segment v_i
|
||||
template <size_t d>
|
||||
Vector ShonanAveraging<d>::MakeATangentVector(size_t p, const Vector &v,
|
||||
size_t i) {
|
||||
VectorValues ShonanAveraging<d>::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);
|
||||
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>(d * i);
|
||||
Vector xi = Vector::Zero(dimension);
|
||||
double sign = pow(-1.0, round((p + 1) / 2) + 1);
|
||||
double sign = sign0;
|
||||
for (size_t j = 0; j < d; j++) {
|
||||
xi(j + p - d - 1) = sign * v_i(d - j - 1);
|
||||
sign = -sign;
|
||||
}
|
||||
return xi;
|
||||
delta.insert(i, xi);
|
||||
}
|
||||
return delta;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -690,14 +710,8 @@ template <size_t d>
|
|||
Values ShonanAveraging<d>::LiftwithDescent(size_t p, const Values &values,
|
||||
const Vector &minEigenVector) {
|
||||
Values lifted = LiftTo<SOn>(p, values);
|
||||
for (auto it : lifted.filter<SOn>()) {
|
||||
// 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<d>::initializeRandomly(std::mt19937 &rng) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <size_t d> Values ShonanAveraging<d>::initializeRandomly() const {
|
||||
template <size_t d>
|
||||
Values ShonanAveraging<d>::initializeRandomly() const {
|
||||
return initializeRandomly(kRandomNumberGenerator);
|
||||
}
|
||||
|
||||
|
|
@ -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<Rot3>
|
||||
convert(const BetweenFactor<Pose3>::shared_ptr &f) {
|
||||
static BinaryMeasurement<Rot3> convert(
|
||||
const BetweenFactor<Pose3>::shared_ptr &f) {
|
||||
auto gaussian =
|
||||
boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
|
||||
if (!gaussian)
|
||||
|
|
@ -837,12 +852,11 @@ convert(const BetweenFactor<Pose3>::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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,25 +20,28 @@
|
|||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
#include <gtsam/geometry/Rot2.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
|
||||
#include <gtsam/sfm/BinaryMeasurement.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
|
||||
#include <Eigen/Sparse>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
class NonlinearFactorGraph;
|
||||
class LevenbergMarquardtOptimizer;
|
||||
|
||||
/// Parameters governing optimization etc.
|
||||
template <size_t d> struct GTSAM_EXPORT ShonanAveragingParameters {
|
||||
template <size_t d>
|
||||
struct GTSAM_EXPORT ShonanAveragingParameters {
|
||||
// Select Rot2 or Rot3 interface based template parameter d
|
||||
using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
|
||||
using Anchor = std::pair<size_t, Rot>;
|
||||
|
|
@ -64,6 +67,7 @@ template <size_t d> struct GTSAM_EXPORT ShonanAveragingParameters {
|
|||
double getOptimalityThreshold() const { return optimalityThreshold; }
|
||||
|
||||
void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; }
|
||||
std::pair<size_t, Rot> 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 <size_t d> class GTSAM_EXPORT ShonanAveraging {
|
||||
public:
|
||||
template <size_t d>
|
||||
class GTSAM_EXPORT ShonanAveraging {
|
||||
public:
|
||||
using Sparse = Eigen::SparseMatrix<double>;
|
||||
|
||||
// Define the Parameters type and use its typedef of the rotation type:
|
||||
|
|
@ -105,7 +110,7 @@ public:
|
|||
// TODO(frank): use BinaryMeasurement?
|
||||
using Measurements = std::vector<BinaryMeasurement<Rot>>;
|
||||
|
||||
private:
|
||||
private:
|
||||
Parameters parameters_;
|
||||
Measurements measurements_;
|
||||
size_t nrUnknowns_;
|
||||
|
|
@ -122,7 +127,7 @@ private:
|
|||
/// Build 3Nx3N sparse degree matrix D
|
||||
Sparse buildD() const;
|
||||
|
||||
public:
|
||||
public:
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
|
|
@ -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,10 +225,9 @@ 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,
|
||||
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
|
||||
|
|
@ -300,7 +304,8 @@ public:
|
|||
Values roundSolution(const Values &values) const;
|
||||
|
||||
/// Lift Values of type T to SO(p)
|
||||
template <class T> static Values LiftTo(size_t p, const Values &values) {
|
||||
template <class T>
|
||||
static Values LiftTo(size_t p, const Values &values) {
|
||||
Values result;
|
||||
for (const auto it : values.filter<T>()) {
|
||||
result.insert(it.key, SOn::Lift(p, it.value.matrix()));
|
||||
|
|
@ -346,17 +351,19 @@ 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,
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -1,4 +1,6 @@
|
|||
from . import utils
|
||||
from .gtsam import *
|
||||
from .utils import findExampleDataFile
|
||||
|
||||
|
||||
def _init():
|
||||
|
|
|
|||
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -9,3 +9,4 @@ PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::
|
|||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
|
||||
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);
|
||||
|
|
|
|||
|
|
@ -10,3 +10,5 @@ py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
|
|||
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
|
||||
py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");
|
||||
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "BinaryMeasurementsUnit3");
|
||||
py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
|
||||
py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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,
|
||||
|
|
|
|||
Loading…
Reference in New Issue