12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #305 from borglab/feature/examples 

Feature/examples
release/4.3a0
Frank Dellaert 2020-05-11 17:54:52 -04:00 committed by GitHub
parent 0af2277dc9 0cd1e777bc
commit b1bb0c9ed5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
27 changed files with 455 additions and 442 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

113
examples/FisheyeExample.cpp
View File

@ -12,7 +12,7 @@
/**
* @file FisheyeExample.cpp
* @brief A visualSLAM example for the structure-from-motion problem on a
* simulated dataset. This version uses a fisheye camera model and a GaussNewton
* simulated dataset. This version uses a fisheye camera model and a GaussNewton
* solver to solve the graph in one batch
* @author ghaggin
* @Date Apr 9,2020
@ -55,75 +55,76 @@
using namespace std;
using namespace gtsam;
using symbol_shorthand::L; // for landmarks
using symbol_shorthand::X; // for poses
/* ************************************************************************* */
int main(int argc, char *argv[])
{
// Define the camera calibration parameters
// Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
boost::shared_ptr<Cal3Fisheye> K(new Cal3Fisheye(
278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035, 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625));
int main(int argc, char *argv[]) {
// Define the camera calibration parameters
auto K = boost::make_shared<Cal3Fisheye>(
278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035,
0.020727425669427896, -0.012786476702685545, 0.0025242267320687625);
// Define the camera observation noise model, 1 pixel stddev
auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
// Define the camera observation noise model, 1 pixel stddev
auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
// Create the set of ground-truth landmarks
vector<Point3> points = createPoints();
// Create the set of ground-truth landmarks
const vector<Point3> points = createPoints();
// Create the set of ground-truth poses
vector<Pose3> poses = createPoses();
// Create the set of ground-truth poses
const vector<Pose3> poses = createPoses();
// Create a Factor Graph and Values to hold the new data
NonlinearFactorGraph graph;
Values initialEstimate;
// Create a Factor Graph and Values to hold the new data
NonlinearFactorGraph graph;
Values initialEstimate;
// Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
static auto kPosePrior = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
graph.emplace_shared<PriorFactor<Pose3>>(Symbol('x', 0), poses[0], kPosePrior);
// Add a prior on pose x0, 0.1 rad on roll,pitch,yaw, and 30cm std on x,y,z
auto posePrior = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], posePrior);
// Add a prior on landmark l0
static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3>>(Symbol('l', 0), points[0], kPointPrior);
// Add a prior on landmark l0
auto pointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3>>(L(0), points[0], pointPrior);
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth
static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + kDeltaPoint);
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth
static const Point3 kDeltaPoint(-0.25, 0.20, 0.15);
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert<Point3>(L(j), points[j] + kDeltaPoint);
// Loop over the poses, adding the observations to the graph
for (size_t i = 0; i < poses.size(); ++i)
{
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j)
{
PinholeCamera<Cal3Fisheye> camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3Fisheye>>(
measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
}
// Add an initial guess for the current pose
// Intentionally initialize the variables off from the ground truth
static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose);
// Loop over the poses, adding the observations to the graph
for (size_t i = 0; i < poses.size(); ++i) {
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) {
PinholeCamera<Cal3Fisheye> camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3Fisheye>>(
measurement, measurementNoise, X(i), L(j), K);
}
GaussNewtonParams params;
params.setVerbosity("TERMINATION");
params.maxIterations = 10000;
// Add an initial guess for the current pose
// Intentionally initialize the variables off from the ground truth
static const Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25),
Point3(0.05, -0.10, 0.20));
initialEstimate.insert(X(i), poses[i] * kDeltaPose);
}
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonOptimizer optimizer(graph, initialEstimate, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION");
params.maxIterations = 10000;
std::cout << "initial error=" << graph.error(initialEstimate) << std::endl;
std::cout << "final error=" << graph.error(result) << std::endl;
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonOptimizer optimizer(graph, initialEstimate, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::ofstream os("examples/vio_batch.dot");
graph.saveGraph(os, result);
std::cout << "initial error=" << graph.error(initialEstimate) << std::endl;
std::cout << "final error=" << graph.error(result) << std::endl;
return 0;
std::ofstream os("examples/vio_batch.dot");
graph.saveGraph(os, result);
return 0;
}
/* ************************************************************************* */

190
examples/ImuFactorsExample.cpp
View File

@ -11,12 +11,14 @@
/**
* @file imuFactorsExample
* @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor navigation code.
* @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor
* navigation code.
* @author Garrett (ghemann@gmail.com), Luca Carlone
*/
/**
* Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS
* Example of use of the imuFactors (imuFactor and combinedImuFactor) in
* conjunction with GPS
* - imuFactor is used by default. You can test combinedImuFactor by
* appending a `-c` flag at the end (see below for example command).
* - we read IMU and GPS data from a CSV file, with the following format:
@ -26,8 +28,8 @@
* 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.
* 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:
@ -36,14 +38,15 @@
*/
// 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/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h>
#include <cstring>
#include <fstream>
#include <iostream>
@ -51,26 +54,21 @@
using namespace gtsam;
using namespace std;
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
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)
const string output_filename = "imuFactorExampleResults.csv";
const string use_combined_imu_flag = "-c";
static const char output_filename[] = "imuFactorExampleResults.csv";
static const char use_combined_imu_flag[3] = "-c";
// This will either be PreintegratedImuMeasurements (for ImuFactor) or
// PreintegratedCombinedMeasurements (for CombinedImuFactor).
PreintegrationType *imu_preintegrated_;
int main(int argc, char* argv[])
{
int main(int argc, char* argv[]) {
string data_filename;
bool use_combined_imu = false;
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.c_str()) == 0) { // strcmp returns 0 for a match
} 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");
@ -80,15 +78,17 @@ int main(int argc, char* argv[])
}
} else {
data_filename = argv[1];
if (strcmp(argv[2], use_combined_imu_flag.c_str()) == 0) { // strcmp returns 0 for a match
if (strcmp(argv[2], use_combined_imu_flag) == 0) {
printf("using CombinedImuFactor\n");
use_combined_imu = true;
}
}
// 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");
FILE* fp_out = fopen(output_filename, "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
@ -97,9 +97,9 @@ int main(int argc, char* argv[])
string value;
// Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
Eigen::Matrix<double,10,1> initial_state = Eigen::Matrix<double,10,1>::Zero();
getline(file, value, ','); // i
for (int i=0; i<9; i++) {
Vector10 initial_state;
getline(file, value, ','); // i
for (int i = 0; i < 9; i++) {
getline(file, value, ',');
initial_state(i) = atof(value.c_str());
}
@ -107,13 +107,14 @@ int main(int argc, char* argv[])
initial_state(9) = atof(value.c_str());
cout << "initial state:\n" << initial_state.transpose() << "\n\n";
// Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
// 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
imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
Values initial_values;
int correction_count = 0;
@ -121,57 +122,64 @@ int main(int argc, char* argv[])
initial_values.insert(V(correction_count), prior_velocity);
initial_values.insert(B(correction_count), prior_imu_bias);
// Assemble prior noise model and add it the graph.
noiseModel::Diagonal::shared_ptr 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
noiseModel::Diagonal::shared_ptr velocity_noise_model = noiseModel::Isotropic::Sigma(3,0.1); // m/s
noiseModel::Diagonal::shared_ptr bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3);
// 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 = new NonlinearFactorGraph();
NonlinearFactorGraph* graph = new NonlinearFactorGraph();
graph->addPrior(X(correction_count), prior_pose, pose_noise_model);
graph->addPrior(V(correction_count), prior_velocity,velocity_noise_model);
graph->addPrior(B(correction_count), prior_imu_bias,bias_noise_model);
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 = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
Matrix33 integration_error_cov = Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities
Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2);
Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2);
Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration
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
boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
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
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
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->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> imu_preintegrated_ = nullptr;
std::shared_ptr<PreintegrationType> preintegrated = nullptr;
if (use_combined_imu) {
imu_preintegrated_ =
preintegrated =
std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);
} else {
imu_preintegrated_ =
preintegrated =
std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);
}
assert(imu_preintegrated_);
assert(preintegrated);
// Store previous state for the imu integration and the latest predicted outcome.
// 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 the total error over the entire run for a simple performance metric.
// 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;
@ -180,14 +188,13 @@ int main(int argc, char* argv[])
// All priors have been set up, now iterate through the data file.
while (file.good()) {
// Parse out first value
getline(file, value, ',');
int type = atoi(value.c_str());
if (type == 0) { // IMU measurement
Eigen::Matrix<double,6,1> imu = Eigen::Matrix<double,6,1>::Zero();
for (int i=0; i<5; ++i) {
if (type == 0) { // IMU measurement
Vector6 imu;
for (int i = 0; i < 5; ++i) {
getline(file, value, ',');
imu(i) = atof(value.c_str());
}
@ -195,11 +202,11 @@ int main(int argc, char* argv[])
imu(5) = atof(value.c_str());
// Adding the IMU preintegration.
imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
} else if (type == 1) { // GPS measurement
Eigen::Matrix<double,7,1> gps = Eigen::Matrix<double,7,1>::Zero();
for (int i=0; i<6; ++i) {
} else if (type == 1) { // GPS measurement
Vector7 gps;
for (int i = 0; i < 6; ++i) {
getline(file, value, ',');
gps(i) = atof(value.c_str());
}
@ -210,39 +217,37 @@ int main(int argc, char* argv[])
// Adding IMU factor and GPS factor and optimizing.
if (use_combined_imu) {
const PreintegratedCombinedMeasurements& preint_imu_combined =
auto preint_imu_combined =
dynamic_cast<const PreintegratedCombinedMeasurements&>(
*imu_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);
*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 {
const PreintegratedImuMeasurements& preint_imu =
dynamic_cast<const PreintegratedImuMeasurements&>(
*imu_preintegrated_);
ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
X(correction_count ), V(correction_count ),
B(correction_count-1),
preint_imu);
auto preint_imu =
dynamic_cast<const PreintegratedImuMeasurements&>(*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<imuBias::ConstantBias>(B(correction_count-1),
B(correction_count ),
zero_bias, bias_noise_model));
graph->add(BetweenFactor<imuBias::ConstantBias>(
B(correction_count - 1), B(correction_count), zero_bias,
bias_noise_model));
}
noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0);
auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
GPSFactor gps_factor(X(correction_count),
Point3(gps(0), // N,
gps(1), // E,
gps(2)), // D,
Point3(gps(0), // N,
gps(1), // E,
gps(2)), // D,
correction_noise);
graph->add(gps_factor);
// Now optimize and compare results.
prop_state = imu_preintegrated_->predict(prev_state, prev_bias);
prop_state = preintegrated->predict(prev_state, prev_bias);
initial_values.insert(X(correction_count), prop_state.pose());
initial_values.insert(V(correction_count), prop_state.v());
initial_values.insert(B(correction_count), prev_bias);
@ -256,7 +261,7 @@ int main(int argc, char* argv[])
prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));
// Reset the preintegration object.
imu_preintegrated_->resetIntegrationAndSetBias(prev_bias);
preintegrated->resetIntegrationAndSetBias(prev_bias);
// Print out the position and orientation error for comparison.
Vector3 gtsam_position = prev_state.pose().translation();
@ -267,19 +272,19 @@ int main(int argc, char* argv[])
Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
Quaternion quat_error = gtsam_quat * gps_quat.inverse();
quat_error.normalize();
Vector3 euler_angle_error(quat_error.x()*2,
quat_error.y()*2,
quat_error.z()*2);
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";
cout << "Position error:" << current_position_error << "\t "
<< "Angular error:" << current_orientation_error << "\n";
fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2),
gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(),
gps(0), gps(1), gps(2),
gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
output_time, gtsam_position(0), gtsam_position(1),
gtsam_position(2), gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(),
gtsam_quat.w(), gps(0), gps(1), gps(2), gps_quat.x(),
gps_quat.y(), gps_quat.z(), gps_quat.w());
output_time += 1.0;
@ -289,6 +294,7 @@ int main(int argc, char* argv[])
}
}
fclose(fp_out);
cout << "Complete, results written to " << output_filename << "\n\n";;
cout << "Complete, results written to " << output_filename << "\n\n";
return 0;
}

22
examples/LocalizationExample.cpp
View File

@ -66,12 +66,11 @@ using namespace gtsam;
#include <gtsam/nonlinear/NonlinearFactor.h>
class UnaryFactor: public NoiseModelFactor1<Pose2> {
// The factor will hold a measurement consisting of an (X,Y) location
// We could this with a Point2 but here we just use two doubles
double mx_, my_;
public:
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<UnaryFactor> shared_ptr;
@ -85,15 +84,15 @@ public:
// The first is the 'evaluateError' function. This function implements the desired measurement
// function, returning a vector of errors when evaluated at the provided variable value. It
// must also calculate the Jacobians for this measurement function, if requested.
Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const
{
Vector evaluateError(const Pose2& q,
boost::optional<Matrix&> H = boost::none) const {
// The measurement function for a GPS-like measurement is simple:
// error_x = pose.x - measurement.x
// error_y = pose.y - measurement.y
// Consequently, the Jacobians are:
// [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0]
// [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0]
if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
if (H) (*H) = (Matrix(2, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}
@ -107,29 +106,28 @@ public:
// Additionally, we encourage you the use of unit testing your custom factors,
// (as all GTSAM factors are), in which you would need an equals and print, to satisfy the
// GTSAM_CONCEPT_TESTABLE_INST(T) defined in Testable.h, but these are not needed below.
}; // UnaryFactor
}; // UnaryFactor
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise);
// 2b. Add "GPS-like" measurements
// We will use our custom UnaryFactor for this.
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
auto unaryNoise =
noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
graph.print("\nFactor Graph:\n"); // print
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
@ -137,7 +135,7 @@ int main(int argc, char** argv) {
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.print("\nInitial Estimate:\n"); // print
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Optimize using Levenberg-Marquardt optimization. The optimizer
// accepts an optional set of configuration parameters, controlling

21
examples/METISOrderingExample.cpp
View File

@ -11,7 +11,8 @@
/**
* @file METISOrdering.cpp
* @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering
* @brief Simple robot motion example, with prior and two odometry measurements,
* using a METIS ordering
* @author Frank Dellaert
* @author Andrew Melim
*/
@ -22,11 +23,11 @@
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
using namespace std;
using namespace gtsam;
@ -34,26 +35,26 @@ using namespace gtsam;
int main(int argc, char** argv) {
NonlinearFactorGraph graph;
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addPrior(1, priorMean, priorNoise);
Pose2 odometry(2.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
graph.print("\nFactor Graph:\n"); // print
graph.print("\nFactor Graph:\n"); // print
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));
initial.print("\nInitial Estimate:\n"); // print
initial.print("\nInitial Estimate:\n"); // print
// optimize using Levenberg-Marquardt optimization
LevenbergMarquardtParams params;
// In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType"
// By default this parameter is set to OrderingType::COLAMD
// In order to specify the ordering type, we need to se the "orderingType". By
// default this parameter is set to OrderingType::COLAMD
params.orderingType = Ordering::METIS;
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
Values result = optimizer.optimize();

11
examples/OdometryExample.cpp
View File

@ -56,24 +56,23 @@ using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// Create an empty nonlinear factor graph
NonlinearFactorGraph graph;
// Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addPrior(1, priorMean, priorNoise);
// Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
graph.print("\nFactor Graph:\n"); // print
graph.print("\nFactor Graph:\n"); // print
// Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
@ -81,7 +80,7 @@ int main(int argc, char** argv) {
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));
initial.print("\nInitial Estimate:\n"); // print
initial.print("\nInitial Estimate:\n"); // print
// optimize using Levenberg-Marquardt optimization
Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();

34
examples/PlanarSLAMExample.cpp
View File

@ -69,35 +69,36 @@ using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// Create a factor graph
NonlinearFactorGraph graph;
// Create the keys we need for this simple example
static Symbol x1('x',1), x2('x',2), x3('x',3);
static Symbol l1('l',1), l2('l',2);
static Symbol x1('x', 1), x2('x', 2), x3('x', 3);
static Symbol l1('l', 1), l2('l', 2);
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.addPrior(x1, prior, priorNoise); // add directly to graph
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and
// a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
auto priorNoise = noiseModel::Diagonal::Sigmas(
Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.addPrior(x1, prior, priorNoise); // add directly to graph
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
Pose2 odometry(2.0, 0.0, 0.0);
// create a measurement for both factors (the same in this case)
auto odometryNoise = noiseModel::Diagonal::Sigmas(
Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.emplace_shared<BetweenFactor<Pose2> >(x1, x2, odometry, odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(x2, x3, odometry, odometryNoise);
// Add Range-Bearing measurements to two different landmarks
// create a noise model for the landmark measurements
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
auto measurementNoise = noiseModel::Diagonal::Sigmas(
Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
// create the measurement values - indices are (pose id, landmark id)
Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90),
Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90),
bearing32 = Rot2::fromDegrees(90);
double range11 = std::sqrt(4.0+4.0),
range21 = 2.0,
range32 = 2.0;
double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0;
// Add Bearing-Range factors
graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
@ -110,7 +111,7 @@ int main(int argc, char** argv) {
// Create (deliberately inaccurate) initial estimate
Values initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insert(l1, Point2(1.8, 2.1));
initialEstimate.insert(l2, Point2(4.1, 1.8));
@ -138,4 +139,3 @@ int main(int argc, char** argv) {
return 0;
}

19
examples/Pose2SLAMExample.cpp
View File

@ -64,21 +64,20 @@ using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0 ), model);
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0), model);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
@ -88,17 +87,17 @@ int main(int argc, char** argv) {
// these constraints may be identified in many ways, such as appearance-based techniques
// with camera images. We will use another Between Factor to enforce this constraint:
graph.emplace_shared<BetweenFactor<Pose2> >(5, 2, Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 ));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 ));
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI ));
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI));
initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
initialEstimate.print("\nInitial Estimate:\n"); // print
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
// The optimizer accepts an optional set of configuration parameters,

34
examples/Pose2SLAMExampleExpressions.cpp
View File

@ -30,7 +30,6 @@ using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
ExpressionFactorGraph graph;
@ -38,31 +37,32 @@ int main(int argc, char** argv) {
Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5);
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// For simplicity, we use the same noise model for odometry and loop closures
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model);
graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x1, x2), Pose2(2, 0, 0), model);
graph.addExpressionFactor(between(x2, x3), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x3, x4), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x4, x5), Pose2(2, 0, M_PI_2), model);
// 2c. Add the loop closure constraint
graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print
graph.addExpressionFactor(between(x5, x2), Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
// 3. Create the data structure to hold the initialEstimate estimate to the
// solution For illustrative purposes, these have been deliberately set to
// incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 ));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 ));
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI ));
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI));
initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
initialEstimate.print("\nInitial Estimate:\n"); // print
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
GaussNewtonParams parameters;

41
examples/Pose2SLAMExample_g2o.cpp
View File

@ -28,41 +28,41 @@ using namespace gtsam;
// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
int main(const int argc, const char *argv[]) {
string kernelType = "none";
int maxIterations = 100; // default
string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
int maxIterations = 100; // default
string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
// Parse user's inputs
if (argc > 1){
g2oFile = argv[1]; // input dataset filename
// outputFile = g2oFile = argv[2]; // done later
if (argc > 1) {
g2oFile = argv[1]; // input dataset filename
}
if (argc > 3){
maxIterations = atoi(argv[3]); // user can specify either tukey or huber
if (argc > 3) {
maxIterations = atoi(argv[3]); // user can specify either tukey or huber
}
if (argc > 4){
kernelType = argv[4]; // user can specify either tukey or huber
if (argc > 4) {
kernelType = argv[4]; // user can specify either tukey or huber
}
// reading file and creating factor graph
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = false;
if(kernelType.compare("none") == 0){
boost::tie(graph, initial) = readG2o(g2oFile,is3D);
if (kernelType.compare("none") == 0) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
}
if(kernelType.compare("huber") == 0){
if (kernelType.compare("huber") == 0) {
std::cout << "Using robust kernel: huber " << std::endl;
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
boost::tie(graph, initial) =
readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
}
if(kernelType.compare("tukey") == 0){
if (kernelType.compare("tukey") == 0) {
std::cout << "Using robust kernel: tukey " << std::endl;
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
boost::tie(graph, initial) =
readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
}
// Add prior on the pose having index (key) = 0
noiseModel::Diagonal::shared_ptr priorModel = //
auto priorModel = //
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graph->addPrior(0, Pose2(), priorModel);
std::cout << "Adding prior on pose 0 " << std::endl;
@ -71,7 +71,8 @@ int main(const int argc, const char *argv[]) {
params.setVerbosity("TERMINATION");
if (argc > 3) {
params.maxIterations = maxIterations;
std::cout << "User required to perform maximum " << params.maxIterations << " iterations "<< std::endl;
std::cout << "User required to perform maximum " << params.maxIterations
<< " iterations " << std::endl;
}
std::cout << "Optimizing the factor graph" << std::endl;
@ -79,8 +80,8 @@ int main(const int argc, const char *argv[]) {
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->error(result)<< std::endl;
std::cout << "initial error=" << graph->error(*initial) << std::endl;
std::cout << "final error=" << graph->error(result) << std::endl;
if (argc < 3) {
result.print("result");

20
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -25,20 +25,20 @@
using namespace std;
using namespace gtsam;
int main (int argc, char** argv) {
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// For simplicity, we will use the same noise model for odometry and loop
// closures
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0 ), model);
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0), model);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
@ -49,10 +49,10 @@ int main (int argc, char** argv) {
// 3. Create the data structure to hold the initial estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2 ));
initial.insert(2, Pose2(2.3, 0.1, -0.2 ));
initial.insert(3, Pose2(4.1, 0.1, M_PI_2));
initial.insert(4, Pose2(4.0, 2.0, M_PI ));
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, M_PI_2));
initial.insert(4, Pose2(4.0, 2.0, M_PI));
initial.insert(5, Pose2(2.1, 2.1, -M_PI_2));
// Single Step Optimization using Levenberg-Marquardt

4
examples/Pose2SLAMExample_lago.cpp
View File

@ -28,7 +28,6 @@ using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
@ -41,8 +40,7 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile);
// Add prior on the pose having index (key) = 0
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graph->addPrior(0, Pose2(), priorModel);
graph->print();

28
examples/Pose2SLAMwSPCG.cpp
View File

@ -28,45 +28,45 @@ using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addPrior(1, prior, priorNoise);
// 2b. Add odometry factors
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
// 2c. Add the loop closure constraint
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0), model);
graph.print("\nFactor Graph:\n"); // print
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0),
model);
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// 3. Create the data structure to hold the initialEstimate estimate to the
// solution
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1));
initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8));
initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2));
initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8));
initialEstimate.print("\nInitial Estimate:\n"); // print
initialEstimate.insert(5, Pose2(0.1, -0.7, 5.8));
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Single Step Optimization using Levenberg-Marquardt
LevenbergMarquardtParams parameters;
parameters.verbosity = NonlinearOptimizerParams::ERROR;
parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
// LM is still the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.
// In addition, the *type* of the iterativeParams decides on the type of
// LM is still the outer optimization loop, but by specifying "Iterative"
// below We indicate that an iterative linear solver should be used. In
// addition, the *type* of the iterativeParams decides on the type of
// iterative solver, in this case the SPCG (subgraph PCG)
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();

21
examples/Pose3Localization.cpp
View File

@ -10,9 +10,9 @@
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @file Pose3Localization.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* Syntax for the script is ./Pose3Localization input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
@ -26,8 +26,7 @@
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
int main(const int argc, const char* argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
@ -41,10 +40,10 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
auto priorModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) {
for (const Values::ConstKeyValuePair& key_value : *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->addPrior(firstKey, Pose3(), priorModel);
@ -53,13 +52,13 @@ int main(const int argc, const char *argv[]) {
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
params.setVerbosity("TERMINATION"); // show info about stopping conditions
GaussNewtonOptimizer optimizer(*graph, *initial, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->error(result)<< std::endl;
std::cout << "initial error=" << graph->error(*initial) << std::endl;
std::cout << "final error=" << graph->error(result) << std::endl;
if (argc < 3) {
result.print("result");
@ -75,7 +74,7 @@ int main(const int argc, const char *argv[]) {
// Calculate and print marginal covariances for all variables
Marginals marginals(*graph, result);
for(const auto& key_value: result) {
for (const auto& key_value : result) {
auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
if (!p) continue;
std::cout << marginals.marginalCovariance(key_value.key) << endl;

19
examples/Pose3SLAMExample_g2o.cpp
View File

@ -10,9 +10,9 @@
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @file Pose3SLAMExample_g2o.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* Syntax for the script is ./Pose3SLAMExample_g2o input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
@ -25,8 +25,7 @@
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
int main(const int argc, const char* argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
@ -40,10 +39,10 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
auto priorModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) {
for (const Values::ConstKeyValuePair& key_value : *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->addPrior(firstKey, Pose3(), priorModel);
@ -52,13 +51,13 @@ int main(const int argc, const char *argv[]) {
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
params.setVerbosity("TERMINATION"); // show info about stopping conditions
GaussNewtonOptimizer optimizer(*graph, *initial, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->error(result)<< std::endl;
std::cout << "initial error=" << graph->error(*initial) << std::endl;
std::cout << "final error=" << graph->error(result) << std::endl;
if (argc < 3) {
result.print("result");

15
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

@ -10,9 +10,9 @@
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @file Pose3SLAMExample_initializePose3Chordal.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* Syntax for the script is ./Pose3SLAMExample_initializePose3Chordal input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
@ -25,8 +25,7 @@
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
int main(const int argc, const char* argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
@ -40,10 +39,10 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
auto priorModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) {
for (const Values::ConstKeyValuePair& key_value : *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->addPrior(firstKey, Pose3(), priorModel);
@ -58,7 +57,7 @@ int main(const int argc, const char *argv[]) {
initialization.print("initialization");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
std::cout << "Writing results to file: " << outputFile << std::endl;
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);

23
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -10,9 +10,9 @@
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @file Pose3SLAMExample_initializePose3Gradient.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* Syntax for the script is ./Pose3SLAMExample_initializePose3Gradient input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
@ -25,8 +25,7 @@
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
int main(const int argc, const char* argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
@ -40,10 +39,10 @@ int main(const int argc, const char *argv[]) {
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
auto priorModel = noiseModel::Diagonal::Variances(
(Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
for(const Values::ConstKeyValuePair& key_value: *initial) {
for (const Values::ConstKeyValuePair& key_value : *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->addPrior(firstKey, Pose3(), priorModel);
@ -52,17 +51,19 @@ int main(const int argc, const char *argv[]) {
std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
bool useGradient = true;
Values initialization = InitializePose3::initialize(*graph, *initial, useGradient);
Values initialization =
InitializePose3::initialize(*graph, *initial, useGradient);
std::cout << "done!" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "initialization error=" <<graph->error(initialization)<< std::endl;
std::cout << "initial error=" << graph->error(*initial) << std::endl;
std::cout << "initialization error=" << graph->error(initialization)
<< std::endl;
if (argc < 3) {
initialization.print("initialization");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
std::cout << "Writing results to file: " << outputFile << std::endl;
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);

43
examples/SFMExample.cpp
View File

@ -56,12 +56,12 @@ using namespace gtsam;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
auto measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks
vector<Point3> points = createPoints();
@ -73,32 +73,45 @@ int main(int argc, char* argv[]) {
NonlinearFactorGraph graph;
// Add a prior on pose x1. This indirectly specifies where the origin is.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.addPrior(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
auto poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
.finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.addPrior(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
// Simulated measurements from each camera pose, adding them to the factor graph
// Simulated measurements from each camera pose, adding them to the factor
// graph
for (size_t i = 0; i < poses.size(); ++i) {
PinholeCamera<Cal3_S2> camera(poses[i], *K);
for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(
measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
}
}
// Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
// Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
// between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.addPrior(Symbol('l', 0), points[0], pointNoise); // add directly to graph
// Because the structure-from-motion problem has a scale ambiguity, the
// problem is still under-constrained Here we add a prior on the position of
// the first landmark. This fixes the scale by indicating the distance between
// the first camera and the first landmark. All other landmark positions are
// interpreted using this scale.
auto pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.addPrior(Symbol('l', 0), points[0],
pointNoise); // add directly to graph
graph.print("Factor Graph:\n");
// Create the data structure to hold the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth
Values initialEstimate;
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
for (size_t i = 0; i < poses.size(); ++i) {
auto corrupted_pose = poses[i].compose(
Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)));
initialEstimate.insert(
Symbol('x', i), corrupted_pose);
}
for (size_t j = 0; j < points.size(); ++j) {
auto corrupted_point = points[j] + Point3(-0.25, 0.20, 0.15);
initialEstimate.insert<Point3>(Symbol('l', j), corrupted_point);
}
initialEstimate.print("Initial Estimates:\n");
/* Optimize the graph and print results */

34
examples/SFMExampleExpressions_bal.cpp
View File

@ -28,7 +28,7 @@
// Header order is close to far
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <vector>
using namespace std;
@ -40,20 +40,16 @@ using symbol_shorthand::P;
// An SfmCamera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// and has a total of 9 free parameters
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre");
if (argc > 1)
filename = string(argv[1]);
if (argc > 1) filename = string(argv[1]);
// Load the SfM data from file
SfmData mydata;
readBAL(filename, mydata);
cout
<< boost::format("read %1% tracks on %2% cameras\n")
% mydata.number_tracks() % mydata.number_cameras();
cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
ExpressionFactorGraph graph;
@ -65,23 +61,23 @@ int main(int argc, char* argv[]) {
Pose3_ pose0_(&SfmCamera::getPose, camera0_);
// Finally, we say it should be equal to first guess
graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(),
noiseModel::Isotropic::Sigma(6, 0.1));
noiseModel::Isotropic::Sigma(6, 0.1));
// similarly, we create a prior on the first point
Point3_ point0_(P(0));
graph.addExpressionFactor(point0_, mydata.tracks[0].p,
noiseModel::Isotropic::Sigma(3, 0.1));
noiseModel::Isotropic::Sigma(3, 0.1));
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2,
1.0); // one pixel in u and v
auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Simulated measurements from each camera pose, adding them to the factor graph
// Simulated measurements from each camera pose, adding them to the factor
// graph
size_t j = 0;
for(const SfmTrack& track: mydata.tracks) {
for (const SfmTrack& track : mydata.tracks) {
// Leaf expression for j^th point
Point3_ point_('p', j);
for(const SfmMeasurement& m: track.measurements) {
for (const SfmMeasurement& m : track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
// Leaf expression for i^th camera
@ -98,10 +94,8 @@ int main(int argc, char* argv[]) {
Values initial;
size_t i = 0;
j = 0;
for(const SfmCamera& camera: mydata.cameras)
initial.insert(C(i++), camera);
for(const SfmTrack& track: mydata.tracks)
initial.insert(P(j++), track.p);
for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
/* Optimize the graph and print results */
Values result;
@ -117,5 +111,3 @@ int main(int argc, char* argv[]) {
return 0;
}
/* ************************************************************************* */

4
examples/SFMExample_SmartFactor.cpp
View File

@ -44,7 +44,7 @@ int main(int argc, char* argv[]) {
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise =
auto measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks and poses
@ -80,7 +80,7 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
auto noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
graph.addPrior(0, poses[0], noise);

25
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -35,13 +35,12 @@ typedef PinholePose<Cal3_S2> Camera;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
auto measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks and poses
vector<Point3> points = createPoints();
@ -52,17 +51,16 @@ int main(int argc, char* argv[]) {
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
// every landmark represent a single landmark, we use shared pointer to init
// the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
for (size_t i = 0; i < poses.size(); ++i) {
// generate the 2D measurement
Camera camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add:
// call add() function to add measurement into a single factor
smartfactor->add(measurement, i);
}
@ -72,7 +70,7 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
auto noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
graph.addPrior(0, poses[0], noise);
@ -85,9 +83,9 @@ int main(int argc, char* argv[]) {
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, poses[i].compose(delta));
// We will use LM in the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.
// In addition, the *type* of the iterativeParams decides on the type of
// We will use LM in the outer optimization loop, but by specifying
// "Iterative" below We indicate that an iterative linear solver should be
// used. In addition, the *type* of the iterativeParams decides on the type of
// iterative solver, in this case the SPCG (subgraph PCG)
LevenbergMarquardtParams parameters;
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
@ -110,11 +108,10 @@ int main(int argc, char* argv[]) {
result.print("Final results:\n");
Values landmark_result;
for (size_t j = 0; j < points.size(); ++j) {
SmartFactor::shared_ptr smart = //
boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
auto smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return nullptr
if (point) // ignore if boost::optional return nullptr
landmark_result.insert(j, *point);
}
}

2
examples/SFMExample_bal.cpp
View File

@ -49,7 +49,7 @@ int main (int argc, char* argv[]) {
NonlinearFactorGraph graph;
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise =
auto noise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Add measurements to the factor graph

51
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file SFMExample.cpp
* @file SFMExample_bal_COLAMD_METIS.cpp
* @brief This file is to compare the ordering performance for COLAMD vs METIS.
* Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file.
* @author Frank Dellaert, Zhaoyang Lv
@ -22,7 +22,7 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/base/timing.h>
@ -34,50 +34,52 @@ using symbol_shorthand::C;
using symbol_shorthand::P;
// We will be using a projection factor that ties a SFM_Camera to a 3D point.
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// and has a total of 9 free parameters
typedef GeneralSFMFactor<SfmCamera,Point3> MyFactor;
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler
// calibration and has a total of 9 free parameters
typedef GeneralSFMFactor<SfmCamera, Point3> MyFactor;
/* ************************************************************************* */
int main (int argc, char* argv[]) {
int main(int argc, char* argv[]) {
// Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre");
if (argc>1) filename = string(argv[1]);
if (argc > 1) filename = string(argv[1]);
// Load the SfM data from file
SfmData mydata;
readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
NonlinearFactorGraph graph;
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Add measurements to the factor graph
size_t j = 0;
for(const SfmTrack& track: mydata.tracks) {
for(const SfmMeasurement& m: track.measurements) {
for (const SfmTrack& track : mydata.tracks) {
for (const SfmMeasurement& m : track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
graph.emplace_shared<MyFactor>(
uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.addPrior(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
graph.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.addPrior(P(0), mydata.tracks[0].p,
noiseModel::Isotropic::Sigma(3, 0.1));
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfmTrack& track: mydata.tracks) initial.insert(P(j++), track.p);
size_t i = 0;
j = 0;
for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
/** --------------- COMPARISON -----------------------**/
/** ----------------------------------------------------**/
@ -98,7 +100,7 @@ int main (int argc, char* argv[]) {
}
// expect they have different ordering results
if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
if (params_using_COLAMD.ordering == params_using_METIS.ordering) {
cout << "COLAMD and METIS produce the same ordering. "
<< "Problem here!!!" << endl;
}
@ -120,8 +122,7 @@ int main (int argc, char* argv[]) {
cout << e.what();
}
{ // printing the result
{ // printing the result
cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
cout << "METIS final error: " << graph.error(result_METIS) << endl;
@ -129,15 +130,13 @@ int main (int argc, char* argv[]) {
cout << endl << endl;
cout << "Time comparison by solving " << filename << " results:" << endl;
cout << boost::format("%1% point tracks and %2% cameras\n") \
% mydata.number_tracks() % mydata.number_cameras() \
cout << boost::format("%1% point tracks and %2% cameras\n") %
mydata.number_tracks() % mydata.number_cameras()
<< endl;
tictoc_print_();
}
return 0;
}
/* ************************************************************************* */

40
examples/SelfCalibrationExample.cpp
View File

@ -33,7 +33,7 @@
#include <gtsam/nonlinear/Values.h>
// SFM-specific factors
#include <gtsam/slam/GeneralSFMFactor.h> // does calibration !
#include <gtsam/slam/GeneralSFMFactor.h> // does calibration !
// Standard headers
#include <vector>
@ -41,9 +41,7 @@
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Create the set of ground-truth
vector<Point3> points = createPoints();
vector<Pose3> poses = createPoses();
@ -52,28 +50,38 @@ int main(int argc, char* argv[]) {
NonlinearFactorGraph graph;
// Add a prior on pose x1.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
auto poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
.finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
// Simulated measurements from each camera pose, adding them to the factor graph
// Simulated measurements from each camera pose, adding them to the factor
// graph
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
auto measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0);
for (size_t i = 0; i < poses.size(); ++i) {
for (size_t j = 0; j < points.size(); ++j) {
PinholeCamera<Cal3_S2> camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// The only real difference with the Visual SLAM example is that here we use a
// different factor type, that also calculates the Jacobian with respect to calibration
graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0));
// The only real difference with the Visual SLAM example is that here we
// use a different factor type, that also calculates the Jacobian with
// respect to calibration
graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(
measurement, measurementNoise, Symbol('x', i), Symbol('l', j),
Symbol('K', 0));
}
}
// Add a prior on the position of the first landmark.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.addPrior(Symbol('l', 0), points[0], pointNoise); // add directly to graph
auto pointNoise =
noiseModel::Isotropic::Sigma(3, 0.1);
graph.addPrior(Symbol('l', 0), points[0],
pointNoise); // add directly to graph
// Add a prior on the calibration.
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
auto calNoise = noiseModel::Diagonal::Sigmas(
(Vector(5) << 500, 500, 0.1, 100, 100).finished());
graph.addPrior(Symbol('K', 0), K, calNoise);
// Create the initial estimate to the solution
@ -81,9 +89,12 @@ int main(int argc, char* argv[]) {
Values initialEstimate;
initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
initialEstimate.insert(
Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25),
Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
initialEstimate.insert<Point3>(Symbol('l', j),
points[j] + Point3(-0.25, 0.20, 0.15));
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
@ -91,5 +102,4 @@ int main(int argc, char* argv[]) {
return 0;
}
/* ************************************************************************* */

5
examples/SimpleRotation.cpp
View File

@ -59,7 +59,6 @@ using namespace gtsam;
const double degree = M_PI / 180;
int main() {
/**
* Step 1: Create a factor to express a unary constraint
* The "prior" in this case is the measurement from a sensor,
@ -76,8 +75,8 @@ int main() {
*/
Rot2 prior = Rot2::fromAngle(30 * degree);
prior.print("goal angle");
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
Symbol key('x',1);
auto model = noiseModel::Isotropic::Sigma(1, 1 * degree);
Symbol key('x', 1);
/**
* Step 2: Create a graph container and add the factor to it

23
examples/StereoVOExample.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file SteroVOExample.cpp
* @file StereoVOExample.cpp
* @brief A stereo visual odometry example
* @date May 25, 2014
* @author Stephen Camp
@ -34,17 +34,17 @@
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
//create graph object, add first pose at origin with key '1'
int main(int argc, char** argv) {
// create graph object, add first pose at origin with key '1'
NonlinearFactorGraph graph;
Pose3 first_pose;
graph.emplace_shared<NonlinearEquality<Pose3> >(1, Pose3());
//create factor noise model with 3 sigmas of value 1
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
//create stereo camera calibration object with .2m between cameras
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
// create factor noise model with 3 sigmas of value 1
const auto model = noiseModel::Isotropic::Sigma(3, 1);
// create stereo camera calibration object with .2m between cameras
const Cal3_S2Stereo::shared_ptr K(
new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
//create and add stereo factors between first pose (key value 1) and the three landmarks
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(520, 480, 440), model, 1, 3, K);
@ -56,17 +56,18 @@ int main(int argc, char** argv){
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(70, 20, 490), model, 2, 4, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 270, 115), model, 2, 5, K);
//create Values object to contain initial estimates of camera poses and landmark locations
// create Values object to contain initial estimates of camera poses and
// landmark locations
Values initial_estimate;
//create and add iniital estimates
// create and add iniital estimates
initial_estimate.insert(1, first_pose);
initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
initial_estimate.insert(3, Point3(1, 1, 5));
initial_estimate.insert(4, Point3(-1, 1, 5));
initial_estimate.insert(5, Point3(0, -0.5, 5));
//create Levenberg-Marquardt optimizer for resulting factor graph, optimize
// create Levenberg-Marquardt optimizer for resulting factor graph, optimize
LevenbergMarquardtOptimizer optimizer(graph, initial_estimate);
Values result = optimizer.optimize();

46
examples/StereoVOExample_large.cpp
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */
/**
* @file SteroVOExample.cpp
* @file StereoVOExample_large.cpp
* @brief A stereo visual odometry example
* @date May 25, 2014
* @author Stephen Camp
@ -41,31 +41,31 @@
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
int main(int argc, char** argv) {
Values initial_estimate;
NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
const auto model = noiseModel::Isotropic::Sigma(3, 1);
string calibration_loc = findExampleDataFile("VO_calibration.txt");
string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
//read camera calibration info from file
// read camera calibration info from file
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b
double fx, fy, s, u0, v0, b;
ifstream calibration_file(calibration_loc.c_str());
cout << "Reading calibration info" << endl;
calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
//create stereo camera calibration object
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
// create stereo camera calibration object
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0, b));
ifstream pose_file(pose_loc.c_str());
cout << "Reading camera poses" << endl;
int pose_id;
MatrixRowMajor m(4,4);
//read camera pose parameters and use to make initial estimates of camera poses
MatrixRowMajor m(4, 4);
// read camera pose parameters and use to make initial estimates of camera
// poses
while (pose_file >> pose_id) {
for (int i = 0; i < 16; i++) {
pose_file >> m.data()[i];
@ -76,33 +76,37 @@ int main(int argc, char** argv){
// camera and landmark keys
size_t x, l;
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
// pixel coordinates uL, uR, v (same for left/right images due to
// rectification) landmark coordinates X, Y, Z in camera frame, resulting from
// triangulation
double uL, uR, v, X, Y, Z;
ifstream factor_file(factor_loc.c_str());
cout << "Reading stereo factors" << endl;
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
// read stereo measurement details from file and use to create and add
// GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
graph.emplace_shared<
GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K);
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(
StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K);
// if the landmark variable included in this factor has not yet been added
// to the initial variable value estimate, add it
if (!initial_estimate.exists(Symbol('l', l))) {
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
//transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
// transformFrom() transforms the input Point3 from the camera pose space,
// camPose, to the global space
Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z));
initial_estimate.insert(Symbol('l', l), worldPoint);
}
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
//constrain the first pose such that it cannot change from its original value during optimization
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x', 1));
// constrain the first pose such that it cannot change from its original value
// during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x', 1), first_pose);
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph
// create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtParams params;
params.orderingType = Ordering::METIS;
LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params);

9
examples/VisualISAMExample.cpp
View File

@ -56,13 +56,11 @@ using namespace gtsam;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr noise = //
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
auto noise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks
vector<Point3> points = createPoints();
@ -81,7 +79,6 @@ int main(int argc, char* argv[]) {
// Loop over the different poses, adding the observations to iSAM incrementally
for (size_t i = 0; i < poses.size(); ++i) {
// Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) {
// Create ground truth measurement
@ -105,12 +102,12 @@ int main(int argc, char* argv[]) {
// adding it to iSAM.
if (i == 0) {
// Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
auto poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise =
auto pointNoise =
noiseModel::Isotropic::Sigma(3, 0.1);
graph.addPrior(Symbol('l', 0), points[0], pointNoise);