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

Merged from branch 'trunk'

release/4.3a0
Richard Roberts 2013-07-31 18:55:15 +00:00
parent 98623f9068 ab083b22c5
commit 2705d85fa7
11 changed files with 12024 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -104,7 +104,7 @@ if(MSVC AND NOT Boost_USE_STATIC_LIBS)
add_definitions(-DBOOST_ALL_DYN_LINK)
endif()
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread date_time regex timer chrono)
find_package(Boost 1.43 COMPONENTS serialization system filesystem thread program_options date_time regex timer chrono)
# Required components
if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR

2
examples/CMakeLists.txt
View File

@ -18,7 +18,7 @@ foreach(example_src ${example_srcs} )
set_target_properties(${example_bin} PROPERTIES EXCLUDE_FROM_ALL ON)
endif()
target_link_libraries(${example_bin} ${gtsam-default})
target_link_libraries(${example_bin} ${gtsam-default} ${Boost_PROGRAM_OPTIONS_LIBRARY})
if(NOT MSVC)
add_custom_target(${example_bin}.run ${EXECUTABLE_OUTPUT_PATH}${example_bin} ${ARGN})
endif()

10608
examples/Data/victoria_park.txt Normal file
View File

File diff suppressed because it is too large Load Diff

516
examples/SolverComparer.cpp Normal file
View File

@ -0,0 +1,516 @@
/* ----------------------------------------------------------------------------
* 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 timeIncremental.cpp
* @brief Incremental and batch solving, timing, and accuracy comparisons
* @author Richard Roberts
*/
#include <gtsam/base/timing.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <fstream>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/export.hpp>
#include <boost/program_options.hpp>
#include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/random.hpp>
using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
namespace po = boost::program_options;
namespace br = boost::range;
typedef Pose2 Pose;
typedef NoiseModelFactor1<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2;
typedef BearingRangeFactor<Pose,Point2> BR;
BOOST_CLASS_EXPORT(Value);
BOOST_CLASS_EXPORT(Pose);
BOOST_CLASS_EXPORT(Rot2);
BOOST_CLASS_EXPORT(Point2);
BOOST_CLASS_EXPORT(NonlinearFactor);
BOOST_CLASS_EXPORT(NoiseModelFactor);
BOOST_CLASS_EXPORT(NM1);
BOOST_CLASS_EXPORT(NM2);
BOOST_CLASS_EXPORT(BetweenFactor<Pose>);
BOOST_CLASS_EXPORT(PriorFactor<Pose>);
BOOST_CLASS_EXPORT(BR);
BOOST_CLASS_EXPORT(noiseModel::Base);
BOOST_CLASS_EXPORT(noiseModel::Isotropic);
BOOST_CLASS_EXPORT(noiseModel::Gaussian);
BOOST_CLASS_EXPORT(noiseModel::Diagonal);
BOOST_CLASS_EXPORT(noiseModel::Unit);
double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
// Compute degrees of freedom (observations - variables)
// In ocaml, +1 was added to the observations to account for the prior, but
// the factor graph already includes a factor for the prior/equality constraint.
// double dof = graph.size() - config.size();
int graph_dim = 0;
BOOST_FOREACH(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf, graph) {
graph_dim += (int)nlf->dim();
}
double dof = double(graph_dim) - double(config.dim()); // kaess: changed to dim
return 2. * graph.error(config) / dof; // kaess: added factor 2, graph.error returns half of actual error
}
// Global variables (these are only set once at program startup and never modified after)
string outputFile;
string inputFile;
string datasetName;
int firstStep;
int lastStep;
bool incremental;
bool batch;
bool compare;
bool perturb;
double perturbationNoise;
string compareFile1, compareFile2;
NonlinearFactorGraph inputFileMeasurements;
Values initial;
// Run functions for each mode
void runIncremental();
void runBatch();
void runCompare();
void runPerturb();
/* ************************************************************************* */
int main(int argc, char *argv[]) {
po::options_description desc("Available options");
desc.add_options()
("help", "Print help message")
("write-solution,o", po::value<string>(&outputFile)->default_value(""), "Write graph and solution to the specified file")
("read-solution,i", po::value<string>(&inputFile)->default_value(""), "Read graph and solution from the specified file")
("dataset,d", po::value<string>(&datasetName)->default_value(""), "Read a dataset file (if and only if --incremental is used)")
("first-step,f", po::value<int>(&firstStep)->default_value(0), "First step to process from the dataset file")
("last-step,l", po::value<int>(&lastStep)->default_value(-1), "Last step to process, or -1 to process until the end of the dataset")
("incremental", "Run in incremental mode using ISAM2 (default)")
("batch", "Run in batch mode, requires an initialization from --read-solution")
("compare", po::value<vector<string> >()->multitoken(), "Compare two solution files")
("perturb", po::value<double>(&perturbationNoise), "Perturb a solution file with the specified noise")
;
po::variables_map vm;
po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
po::notify(vm);
batch = (vm.count("batch") > 0);
compare = (vm.count("compare") > 0);
perturb = (vm.count("perturb") > 0);
incremental = (vm.count("incremental") > 0 || (!batch && !compare && !perturb));
if(compare) {
const vector<string>& compareFiles = vm["compare"].as<vector<string> >();
if(compareFiles.size() != 2) {
cout << "Must specify two files with --compare";
exit(1);
}
compareFile1 = compareFiles[0];
compareFile2 = compareFiles[1];
}
if((batch && incremental) || (batch && compare) || (incremental && compare)) {
cout << "Only one of --incremental, --batch, and --compare may be specified\n" << desc << endl;
exit(1);
}
if(incremental && datasetName.empty()) {
cout << "In incremental mode, a dataset must be specified\n" << desc << endl;
exit(1);
}
if(!incremental && !datasetName.empty()) {
cout << "A dataset may only be specified in incremental mode\n" << desc << endl;
exit(1);
}
if(batch && inputFile.empty()) {
cout << "In batch model, an input file must be specified\n" << desc << endl;
exit(1);
}
if(perturb && (inputFile.empty() || outputFile.empty())) {
cout << "In perturb mode, specify input and output files\n" << desc << endl;
exit(1);
}
// Read input file
if(!inputFile.empty())
{
cout << "Reading input file " << inputFile << endl;
std::ifstream readerStream(inputFile, ios::binary);
boost::archive::binary_iarchive reader(readerStream);
reader >> inputFileMeasurements >> initial;
}
// Run mode
if(incremental)
runIncremental();
else if(batch)
runBatch();
else if(compare)
runCompare();
else if(perturb)
runPerturb();
return 0;
}
/* ************************************************************************* */
void runIncremental()
{
NonlinearFactorGraph datasetMeasurements;
cout << "Loading dataset " << datasetName << endl;
string datasetFile = findExampleDataFile(datasetName);
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> data =
load2D(datasetFile);
datasetMeasurements = *data.first;
ISAM2 isam2;
if(!inputFile.empty())
{
cout << "Adding initial measurements and values" << endl;
isam2.update(inputFileMeasurements, initial);
}
// Look for the first measurement to use
cout << "Looking for first measurement from step " << firstStep << endl;
size_t nextMeasurement = 0;
bool havePreviousPose = false;
Key firstPose;
while(nextMeasurement < datasetMeasurements.size())
{
if(BetweenFactor<Pose>::shared_ptr measurement =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
{
Key key1 = measurement->key1(), key2 = measurement->key2();
if((key1 >= firstStep && key1 < key2) || (key2 >= firstStep && key2 < key1)) {
// We found an odometry starting at firstStep
firstPose = std::min(key1, key2);
break;
}
if((key2 >= firstStep && key1 < key2) || (key1 >= firstStep && key2 < key1)) {
// We found an odometry joining firstStep with a previous pose
havePreviousPose = true;
firstPose = std::max(key1, key2);
break;
}
}
++ nextMeasurement;
}
if(nextMeasurement == datasetMeasurements.size()) {
cout << "The supplied first step is past the end of the dataset" << endl;
exit(1);
}
// If we didn't find an odometry linking to a previous pose, create a first pose and a prior
if(!havePreviousPose) {
cout << "Looks like " << firstPose << " is the first time step, so adding a prior on it" << endl;
NonlinearFactorGraph newFactors;
Values newVariables;
newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(Pose::Dim())));
newVariables.insert(firstPose, Pose());
isam2.update(newFactors, newVariables);
}
cout << "Playing forward time steps..." << endl;
for(size_t step = firstPose; nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || step <= lastStep); ++step)
{
Values newVariables;
NonlinearFactorGraph newFactors;
// Collect measurements and new variables for the current step
gttic_(Collect_measurements);
while(nextMeasurement < datasetMeasurements.size()) {
NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];
if(BetweenFactor<Pose>::shared_ptr measurement =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
{
// Stop collecting measurements that are for future steps
if(measurement->key1() > step || measurement->key2() > step)
break;
// Require that one of the nodes is the current one
if(measurement->key1() != step && measurement->key2() != step)
throw runtime_error("Problem in data file, out-of-sequence measurements");
// Add a new factor
newFactors.push_back(measurement);
// Initialize the new variable
if(measurement->key1() > measurement->key2()) {
if(step == 1)
newVariables.insert(measurement->key1(), measurement->measured().inverse());
else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2());
newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse());
}
// cout << "Initializing " << step << endl;
} else {
if(step == 1)
newVariables.insert(measurement->key2(), measurement->measured());
else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1());
newVariables.insert(measurement->key2(), prevPose * measurement->measured());
}
// cout << "Initializing " << step << endl;
}
}
else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement =
boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
{
Key poseKey = measurement->keys()[0], lmKey = measurement->keys()[1];
// Stop collecting measurements that are for future steps
if(poseKey > step)
throw runtime_error("Problem in data file, out-of-sequence measurements");
// Add new factor
newFactors.push_back(measurement);
// Initialize new landmark
if(!isam2.getLinearizationPoint().exists(lmKey))
{
Pose pose;
if(isam2.getLinearizationPoint().exists(poseKey))
pose = isam2.calculateEstimate<Pose>(poseKey);
else
pose = newVariables.at<Pose>(poseKey);
Rot2 measuredBearing = measurement->measured().first;
double measuredRange = measurement->measured().second;
newVariables.insert(lmKey,
pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
}
}
else
{
throw std::runtime_error("Unknown factor type read from data file");
}
++ nextMeasurement;
}
gttoc_(Collect_measurements);
// Update iSAM2
gttic_(Update_ISAM2);
isam2.update(newFactors, newVariables);
gttoc_(Update_ISAM2);
if((step - firstPose) % 100 == 0) {
gttic_(chi2);
Values estimate(isam2.calculateEstimate());
double chi2 = chi2_red(isam2.getFactorsUnsafe(), estimate);
cout << "chi2 = " << chi2 << endl;
gttoc_(chi2);
}
tictoc_finishedIteration_();
if((step - firstPose) % 1000 == 0) {
cout << "Step " << step << endl;
tictoc_print_();
}
}
if(!outputFile.empty())
{
try {
cout << "Writing output file " << outputFile << endl;
std::ofstream writerStream(outputFile, ios::binary);
boost::archive::binary_oarchive writer(writerStream);
writer << isam2.getFactorsUnsafe() << isam2.calculateEstimate();
} catch(std::exception& e) {
cout << e.what() << endl;
exit(1);
}
}
// Compute marginals
//try {
// Marginals marginals(graph, values);
// int i=0;
// BOOST_REVERSE_FOREACH(Key key1, values.keys()) {
// int j=0;
// BOOST_REVERSE_FOREACH(Key key2, values.keys()) {
// if(i != j) {
// gttic_(jointMarginalInformation);
// std::vector<Key> keys(2);
// keys[0] = key1;
// keys[1] = key2;
// JointMarginal info = marginals.jointMarginalInformation(keys);
// gttoc_(jointMarginalInformation);
// tictoc_finishedIteration_();
// }
// ++j;
// if(j >= 50)
// break;
// }
// ++i;
// if(i >= 50)
// break;
// }
// tictoc_print_();
// BOOST_FOREACH(Key key, values.keys()) {
// gttic_(marginalInformation);
// Matrix info = marginals.marginalInformation(key);
// gttoc_(marginalInformation);
// tictoc_finishedIteration_();
// ++i;
// }
//} catch(std::exception& e) {
// cout << e.what() << endl;
//}
//tictoc_print_();
}
/* ************************************************************************* */
void runBatch()
{
cout << "Creating batch optimizer..." << endl;
gttic_(Create_optimizer);
LevenbergMarquardtOptimizer optimizer(inputFileMeasurements, initial);
gttoc_(Create_optimizer);
double lastError;
do {
lastError = optimizer.error();
gttic_(Iterate_optimizer);
optimizer.iterate();
gttoc_(Iterate_optimizer);
cout << "Error: " << lastError << " -> " << optimizer.error() << ", lambda: " << optimizer.lambda() << endl;
gttic_(chi2);
double chi2 = chi2_red(inputFileMeasurements, optimizer.values());
cout << "chi2 = " << chi2 << endl;
gttoc_(chi2);
} while(!checkConvergence(optimizer.params().relativeErrorTol,
optimizer.params().absoluteErrorTol, optimizer.params().errorTol,
lastError, optimizer.error(), optimizer.params().verbosity));
tictoc_finishedIteration_();
tictoc_print_();
if(!outputFile.empty())
{
try {
cout << "Writing output file " << outputFile << endl;
std::ofstream writerStream(outputFile, ios::binary);
boost::archive::binary_oarchive writer(writerStream);
writer << inputFileMeasurements << optimizer.values();
} catch(std::exception& e) {
cout << e.what() << endl;
exit(1);
}
}
}
/* ************************************************************************* */
void runCompare()
{
NonlinearFactorGraph graph1, graph2;
Values soln1, soln2;
cout << "Reading solution file " << compareFile1 << endl;
{
std::ifstream readerStream(compareFile1, ios::binary);
boost::archive::binary_iarchive reader(readerStream);
reader >> graph1 >> soln1;
}
cout << "Reading solution file " << compareFile2 << endl;
{
std::ifstream readerStream(compareFile2, ios::binary);
boost::archive::binary_iarchive reader(readerStream);
reader >> graph2 >> soln2;
}
// Check factors for equality
cout << "Comparing factor graphs..." << endl;
if(graph1.size() != graph2.size())
cout << " Graph sizes not equal, " << graph1.size() << " vs. " << graph2.size() << endl;
for(NonlinearFactorGraph::const_iterator f1 = graph1.begin(), f2 = graph2.begin();
f1 != graph1.end() && f2 != graph2.end(); ++f1, ++f2)
{
if((*f1 || *f2) && ((*f1 && !*f2) || (!*f1 && *f2) || !(**f1).equals(**f2, 1e-9))) {
cout << " Factor " << f1 - graph1.begin() << " not equal.\n";
(**f1).print(" From graph 1: ");
(**f2).print(" From graph 2: ");
}
}
// Check solution for equality
cout << "Comparing solutions..." << endl;
vector<Key> missingKeys;
br::set_symmetric_difference(soln1.keys(), soln2.keys(), std::back_inserter(missingKeys));
if(!missingKeys.empty()) {
cout << " Keys unique to one solution file: ";
for(size_t i = 0; i < missingKeys.size(); ++i) {
cout << DefaultKeyFormatter(missingKeys[i]);
if(i != missingKeys.size() - 1)
cout << ", ";
}
cout << endl;
}
vector<Key> commonKeys;
br::set_intersection(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys));
double maxDiff = 0.0;
BOOST_FOREACH(Key j, commonKeys)
maxDiff = std::max(maxDiff, soln1.at(j).localCoordinates_(soln2.at(j)).norm());
cout << " Maximum solution difference (norm of logmap): " << maxDiff << endl;
}
/* ************************************************************************* */
void runPerturb()
{
// Set up random number generator
boost::random::mt19937 rng;
boost::random::normal_distribution<double> normal(0.0, perturbationNoise);
// Perturb values
VectorValues noise;
Ordering ordering = *initial.orderingArbitrary();
BOOST_FOREACH(const Values::KeyValuePair& key_val, initial)
{
Vector noisev(key_val.value.dim());
for(Vector::Index i = 0; i < noisev.size(); ++i)
noisev(i) = normal(rng);
noise.insert(ordering[key_val.key], noisev);
}
Values perturbed = initial.retract(noise, ordering);
// Write results
try {
cout << "Writing output file " << outputFile << endl;
std::ofstream writerStream(outputFile, ios::binary);
boost::archive::binary_oarchive writer(writerStream);
writer << inputFileMeasurements << perturbed;
} catch(std::exception& e) {
cout << e.what() << endl;
exit(1);
}
}

6
gtsam/slam/dataset.cpp
View File

@ -91,7 +91,8 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
// load the poses
while (is) {
is >> tag;
if(! (is >> tag))
break;
if ((tag == "VERTEX2") || (tag == "VERTEX")) {
int id;
@ -113,7 +114,8 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
// load the factors
bool haveLandmark = false;
while (is) {
is >> tag;
if(! (is >> tag))
break;
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
int id1, id2;

99
gtsam_unstable/geometry/tests/testTriangulation.cpp Normal file
View File

@ -0,0 +1,99 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------------------------------------------------- */
/**
* testTriangulation.cpp
*
* Created on: July 30th, 2013
* Author: cbeall3
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam_unstable/geometry/InvDepthCamera3.h>
#include <gtsam_unstable/geometry/triangulation.h>
#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */
TEST( triangulation, twoPoses) {
Cal3_S2 K(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, K);
// create second camera 1 meter to the right of first camera
Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
SimpleCamera level_camera_right(level_pose_right, K);
// landmark ~5 meters infront of camera
Point3 landmark(5, 0.5, 1.2);
// 1. Project two landmarks into two cameras and triangulate
Point2 level_uv = level_camera.project(landmark);
Point2 level_uv_right = level_camera_right.project(landmark);
vector<Pose3> poses;
vector<Point2> measurements;
poses += level_pose, level_pose_right;
measurements += level_uv, level_uv_right;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses, measurements, K);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements.at(0) += Point2(0.1,0.5);
measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses, measurements, K);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise
Pose3 pose_top = level_pose * Pose3(Rot3::ypr(0.1,0.2,0.1), Point3(0.1,-2,-.1));
SimpleCamera camera_top(pose_top, K);
Point2 top_uv = camera_top.project(landmark);
poses += pose_top;
measurements += top_uv + Point2(0.1, -0.1);
boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses, measurements, K);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way
Pose3 level_pose180 = Pose3(Rot3::ypr(M_PI/2, 0., -M_PI/2), Point3(0,0,1));
SimpleCamera camera_180(level_pose180, K);
CHECK_EXCEPTION(camera_180.project(landmark);, CheiralityException);
poses += level_pose180;
measurements += Point2(400,400);
boost::optional<Point3> triangulated_4cameras = triangulatePoint3(poses, measurements, K);
EXPECT(boost::none == triangulated_4cameras);
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

81
gtsam_unstable/geometry/triangulation.cpp Normal file
View File

@ -0,0 +1,81 @@
/* ----------------------------------------------------------------------------
* 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 triangulation.cpp
* @brief Functions for triangulation
* @author Chris Beall
*/
#include <gtsam_unstable/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <boost/foreach.hpp>
#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>
using namespace std;
using namespace boost::assign;
namespace gtsam {
/* ************************************************************************* */
// See Hartley and Zisserman, 2nd Ed., page 312
Point3 triangulateDLT(const vector<Matrix>& projection_matrices,
const vector<Point2>& measurements) {
Matrix A = Matrix_(projection_matrices.size() *2, 4);
for(size_t i=0; i< projection_matrices.size(); i++) {
size_t row = i*2;
const Matrix& projection = projection_matrices.at(i);
const Point2& p = measurements.at(i);
// build system of equations
A.row(row) = p.x() * projection.row(2) - projection.row(0);
A.row(row+1) = p.y() * projection.row(2) - projection.row(1);
}
int rank;
double error;
Vector v;
boost::tie(rank, error, v) = DLT(A);
return Point3(sub( (v / v(3)),0,3));
}
/* ************************************************************************* */
boost::optional<Point3> triangulatePoint3(const vector<Pose3>& poses,
const vector<Point2>& measurements, const Cal3_S2& K) {
assert(poses.size() == measurements.size());
if(poses.size() < 2)
return boost::none;
vector<Matrix> projection_matrices;
// construct projection matrices from poses & calibration
BOOST_FOREACH(const Pose3& pose, poses)
projection_matrices += K.matrix() * sub(pose.inverse().matrix(),0,3,0,4);
Point3 triangulated_point = triangulateDLT(projection_matrices, measurements);
// verify that the triangulated point lies infront of all cameras
BOOST_FOREACH(const Pose3& pose, poses) {
const Point3& p_local = pose.transform_to(triangulated_point);
if(p_local.z() <= 0)
return boost::none;
}
return triangulated_point;
}
/* ************************************************************************* */
} // namespace gtsam

45
gtsam_unstable/geometry/triangulation.h Normal file
View File

@ -0,0 +1,45 @@
/* ----------------------------------------------------------------------------
* 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 triangulation.h
* @brief Functions for triangulation
* @date July 31, 2013
* @author Chris Beall
*/
#pragma once
#include <vector>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam_unstable/base/dllexport.h>
namespace gtsam {
/**
* Function to triangulate 3D landmark point from an arbitrary number
* of poses (at least 2) using the DLT. The function checks that the
* resulting point lies in front of all cameras, but has no other checks
* to verify the quality of the triangulation.
* @param poses A vector of camera poses
* @param measurements A vector of camera measurements
* @param K The camera calibration
* @return Returns a Point3 on success, boost::none otherwise.
*/
GTSAM_UNSTABLE_EXPORT boost::optional<Point3> triangulatePoint3(const std::vector<Pose3>& poses,
const std::vector<Point2>& measurements, const Cal3_S2& K);
} // \namespace gtsam

228
gtsam_unstable/slam/MultiProjectionFactor.h Normal file
View File

@ -0,0 +1,228 @@
/* ----------------------------------------------------------------------------
* 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 ProjectionFactor.h
* @brief Basic bearing factor from 2D measurement
* @author Chris Beall
* @author Richard Roberts
* @author Frank Dellaert
* @author Alex Cunningham
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <boost/optional.hpp>
namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
* i.e. the main building block for visual SLAM.
* @addtogroup SLAM
*/
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class MultiProjectionFactor: public NoiseModelFactor {
protected:
// Keep a copy of measurement and calibration for I/O
Vector measured_; ///< 2D measurement for each of the n views
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
// verbosity handling for Cheirality Exceptions
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
public:
/// shorthand for base class type
typedef NoiseModelFactor Base;
/// shorthand for this class
typedef MultiProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
MultiProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {}
/**
* Constructor
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2n dimensional location of the n points in the n views (the measurements)
* @param model is the standard deviation (current version assumes that the uncertainty is the same for all views)
* @param poseKeys is the set of indices corresponding to the cameras observing the same landmark
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
* @param body_P_sensor is the transform from body to sensor frame (default identity)
*/
MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
FastSet<Key> poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
boost::optional<POSE> body_P_sensor = boost::none) :
Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(false), verboseCheirality_(false) {
keys_.assign(poseKeys.begin(), poseKeys.end());
keys_.push_back(pointKey);
}
/**
* Constructor with exception-handling flags
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param poseKey is the index of the camera
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
* @param throwCheirality determines whether Cheirality exceptions are rethrown
* @param verboseCheirality determines whether exceptions are printed for Cheirality
* @param body_P_sensor is the transform from body to sensor frame (default identity)
*/
MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
FastSet<Key> poseKeys, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
bool throwCheirality, bool verboseCheirality,
boost::optional<POSE> body_P_sensor = boost::none) :
Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
/** Virtual destructor */
virtual ~MultiProjectionFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "MultiProjectionFactor, z = ";
std::cout << measured_ << "measurements, z = ";
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
//&& this->measured_.equals(e->measured_, tol)
&& this->K_->equals(*e->K_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
/// Evaluate error h(x)-z and optionally derivatives
Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const{
Vector a;
return a;
// Point3 point = x.at<Point3>(*keys_.end());
//
// std::vector<KeyType>::iterator vit;
// for (vit = keys_.begin(); vit != keys_.end()-1; vit++) {
// Key key = (*vit);
// Pose3 pose = x.at<Pose3>(key);
//
// if(body_P_sensor_) {
// if(H1) {
// gtsam::Matrix H0;
// PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// *H1 = *H1 * H0;
// return reprojectionError.vector();
// } else {
// PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// return reprojectionError.vector();
// }
// } else {
// PinholeCamera<CALIBRATION> camera(pose, *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// return reprojectionError.vector();
// }
// }
}
Vector evaluateError(const Pose3& pose, const Point3& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
try {
if(body_P_sensor_) {
if(H1) {
gtsam::Matrix H0;
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
*H1 = *H1 * H0;
return reprojectionError.vector();
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
return reprojectionError.vector();
}
} else {
PinholeCamera<CALIBRATION> camera(pose, *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
return reprojectionError.vector();
}
} catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
if (throwCheirality_)
throw e;
}
return ones(2) * 2.0 * K_->fx();
}
/** return the measurements */
const Vector& measured() const {
return measured_;
}
/** return the calibration object */
inline const boost::shared_ptr<CALIBRATION> calibration() const {
return K_;
}
/** return verbosity */
inline bool verboseCheirality() const { return verboseCheirality_; }
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const { return throwCheirality_; }
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(K_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
}
};
} // \ namespace gtsam

235
gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp Normal file
View File

@ -0,0 +1,235 @@
/* ----------------------------------------------------------------------------
* 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 testProjectionFactor.cpp
* @brief Unit tests for ProjectionFactor Class
* @author Frank Dellaert
* @date Nov 2009
*/
#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam_unstable/slam/MultiProjectionFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
// make a realistic calibration matrix
static double fov = 60; // degrees
static size_t w=640,h=480;
static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Unit::Create(2));
// Convenience for named keys
//using symbol_shorthand::X;
//using symbol_shorthand::L;
//typedef GenericProjectionFactor<Pose3, Point3> TestProjectionFactor;
///* ************************************************************************* */
TEST( MultiProjectionFactor, create ){
Values theta;
NonlinearFactorGraph graph;
Symbol x1('X', 1);
Symbol x2('X', 2);
Symbol x3('X', 3);
Symbol l1('l', 1);
Vector n_measPixel(6); // Pixel measurements from 3 cameras observing landmark 1
n_measPixel << 10, 10, 10, 10, 10, 10;
const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
FastSet<Key> views;
views.insert(x1);
views.insert(x2);
views.insert(x3);
MultiProjectionFactor<Pose3, Point3> mpFactor(n_measPixel, noiseProjection, views, l1, K);
graph.add(mpFactor);
}
///* ************************************************************************* */
//TEST( ProjectionFactor, nonStandard ) {
// GenericProjectionFactor<Pose3, Point3, Cal3DS2> f;
//}
//
///* ************************************************************************* */
//TEST( ProjectionFactor, Constructor) {
// Key poseKey(X(1));
// Key pointKey(L(1));
//
// Point2 measurement(323.0, 240.0);
//
// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K);
//}
//
///* ************************************************************************* */
//TEST( ProjectionFactor, ConstructorWithTransform) {
// Key poseKey(X(1));
// Key pointKey(L(1));
//
// Point2 measurement(323.0, 240.0);
// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
//
// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor);
//}
//
///* ************************************************************************* */
//TEST( ProjectionFactor, Equals ) {
// // Create two identical factors and make sure they're equal
// Point2 measurement(323.0, 240.0);
//
// TestProjectionFactor factor1(measurement, model, X(1), L(1), K);
// TestProjectionFactor factor2(measurement, model, X(1), L(1), K);
//
// CHECK(assert_equal(factor1, factor2));
//}
//
///* ************************************************************************* */
//TEST( ProjectionFactor, EqualsWithTransform ) {
// // Create two identical factors and make sure they're equal
// Point2 measurement(323.0, 240.0);
// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
//
// TestProjectionFactor factor1(measurement, model, X(1), L(1), K, body_P_sensor);
// TestProjectionFactor factor2(measurement, model, X(1), L(1), K, body_P_sensor);
//
// CHECK(assert_equal(factor1, factor2));
//}
//
///* ************************************************************************* */
//TEST( ProjectionFactor, Error ) {
// // Create the factor with a measurement that is 3 pixels off in x
// Key poseKey(X(1));
// Key pointKey(L(1));
// Point2 measurement(323.0, 240.0);
// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K);
//
// // Set the linearization point
// Pose3 pose(Rot3(), Point3(0,0,-6));
// Point3 point(0.0, 0.0, 0.0);
//
// // Use the factor to calculate the error
// Vector actualError(factor.evaluateError(pose, point));
//
// // The expected error is (-3.0, 0.0) pixels / UnitCovariance
// Vector expectedError = Vector_(2, -3.0, 0.0);
//
// // Verify we get the expected error
// CHECK(assert_equal(expectedError, actualError, 1e-9));
//}
//
///* ************************************************************************* */
//TEST( ProjectionFactor, ErrorWithTransform ) {
// // Create the factor with a measurement that is 3 pixels off in x
// Key poseKey(X(1));
// Key pointKey(L(1));
// Point2 measurement(323.0, 240.0);
// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor);
//
// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
// Point3 point(0.0, 0.0, 0.0);
//
// // Use the factor to calculate the error
// Vector actualError(factor.evaluateError(pose, point));
//
// // The expected error is (-3.0, 0.0) pixels / UnitCovariance
// Vector expectedError = Vector_(2, -3.0, 0.0);
//
// // Verify we get the expected error
// CHECK(assert_equal(expectedError, actualError, 1e-9));
//}
//
///* ************************************************************************* */
//TEST( ProjectionFactor, Jacobian ) {
// // Create the factor with a measurement that is 3 pixels off in x
// Key poseKey(X(1));
// Key pointKey(L(1));
// Point2 measurement(323.0, 240.0);
// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K);
//
// // Set the linearization point
// Pose3 pose(Rot3(), Point3(0,0,-6));
// Point3 point(0.0, 0.0, 0.0);
//
// // Use the factor to calculate the Jacobians
// Matrix H1Actual, H2Actual;
// factor.evaluateError(pose, point, H1Actual, H2Actual);
//
// // The expected Jacobians
// Matrix H1Expected = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
// Matrix H2Expected = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.);
//
// // Verify the Jacobians are correct
// CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
// CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
//}
//
///* ************************************************************************* */
//TEST( ProjectionFactor, JacobianWithTransform ) {
// // Create the factor with a measurement that is 3 pixels off in x
// Key poseKey(X(1));
// Key pointKey(L(1));
// Point2 measurement(323.0, 240.0);
// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
// TestProjectionFactor factor(measurement, model, poseKey, pointKey, K, body_P_sensor);
//
// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
// Point3 point(0.0, 0.0, 0.0);
//
// // Use the factor to calculate the Jacobians
// Matrix H1Actual, H2Actual;
// factor.evaluateError(pose, point, H1Actual, H2Actual);
//
// // The expected Jacobians
// Matrix H1Expected = Matrix_(2, 6, -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376);
// Matrix H2Expected = Matrix_(2, 3, 0., -92.376, 0., 0., 0., -92.376);
//
// // Verify the Jacobians are correct
// CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
// CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
//}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

206
matlab/gtsam_examples/IMUKittiExample.m Normal file
View File

@ -0,0 +1,206 @@
%close all
%clc
import gtsam.*;
%% Read metadata and compute relative sensor pose transforms
IMU_metadata = importdata('KittiEquivBiasedImu_metadata.txt');
IMU_metadata = cell2struct(num2cell(IMU_metadata.data), IMU_metadata.colheaders, 2);
IMUinBody = Pose3.Expmap([
IMU_metadata.BodyPtx; IMU_metadata.BodyPty; IMU_metadata.BodyPtz;
IMU_metadata.BodyPrx; IMU_metadata.BodyPry; IMU_metadata.BodyPrz; ]);
VO_metadata = importdata('KittiRelativePose_metadata.txt');
VO_metadata = cell2struct(num2cell(VO_metadata.data), VO_metadata.colheaders, 2);
VOinBody = Pose3.Expmap([
VO_metadata.BodyPtx; VO_metadata.BodyPty; VO_metadata.BodyPtz;
VO_metadata.BodyPrx; VO_metadata.BodyPry; VO_metadata.BodyPrz; ]);
GPS_metadata = importdata('KittiGps_metadata.txt');
GPS_metadata = cell2struct(num2cell(GPS_metadata.data), GPS_metadata.colheaders, 2);
GPSinBody = Pose3.Expmap([
GPS_metadata.BodyPtx; GPS_metadata.BodyPty; GPS_metadata.BodyPtz;
GPS_metadata.BodyPrx; GPS_metadata.BodyPry; GPS_metadata.BodyPrz; ]);
VOinIMU = IMUinBody.inverse().compose(VOinBody);
GPSinIMU = IMUinBody.inverse().compose(GPSinBody);
%% Read data and change coordinate frame of GPS and VO measurements to IMU frame
IMU_data = importdata('KittiEquivBiasedImu.txt');
IMU_data = cell2struct(num2cell(IMU_data.data), IMU_data.colheaders, 2);
imum = cellfun(@(x) x', num2cell([ [IMU_data.accelX]' [IMU_data.accelY]' [IMU_data.accelZ]' [IMU_data.omegaX]' [IMU_data.omegaY]' [IMU_data.omegaZ]' ], 2), 'UniformOutput', false);
[IMU_data.acc_omega] = deal(imum{:});
IMU_data = rmfield(IMU_data, { 'accelX' 'accelY' 'accelZ' 'omegaX' 'omegaY' 'omegaZ' });
clear imum
VO_data = importdata('KittiRelativePose.txt');
VO_data = cell2struct(num2cell(VO_data.data), VO_data.colheaders, 2);
% Merge relative pose fields and convert to Pose3
logposes = [ [VO_data.dtx]' [VO_data.dty]' [VO_data.dtz]' [VO_data.drx]' [VO_data.dry]' [VO_data.drz]' ];
logposes = num2cell(logposes, 2);
relposes = arrayfun(@(x) {gtsam.Pose3.Expmap(x{1}')}, logposes);
% TODO: convert to IMU frame %relposes = arrayfun(
[VO_data.RelativePose] = deal(relposes{:});
VO_data = rmfield(VO_data, { 'dtx' 'dty' 'dtz' 'drx' 'dry' 'drz' });
clear logposes relposes
GPS_data = importdata('KittiGps.txt');
GPS_data = cell2struct(num2cell(GPS_data.data), GPS_data.colheaders, 2);
%%
SummaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ...
gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ...
1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3));
%% Set initial conditions for the estimated trajectory
disp('TODO: we have GPS so this initialization is not right')
currentPoseGlobal = Pose3; % initial pose is the reference frame (navigation frame)
currentVelocityGlobal = [0;0;0]; % the vehicle is stationary at the beginning
bias_acc = [0;0;0]; % we initialize accelerometer biases to zero
bias_omega = [0;0;0]; % we initialize gyro biases to zero
%% Solver object
isamParams = ISAM2Params;
isamParams.setRelinearizeSkip(1);
isam = gtsam.ISAM2(isamParams);
%% create nonlinear factor graph
factors = NonlinearFactorGraph;
values = Values;
%% Create prior on initial pose, velocity, and biases
sigma_init_x = 1.0
sigma_init_v = 1.0
sigma_init_b = 1.0
values.insert(symbol('x',0), currentPoseGlobal);
values.insert(symbol('v',0), LieVector(currentVelocityGlobal) );
values.insert(symbol('b',0), imuBias.ConstantBias(bias_acc,bias_omega) );
% Prior on initial pose
factors.add(PriorFactorPose3(symbol('x',0), ...
currentPoseGlobal, noiseModel.Isotropic.Sigma(6, sigma_init_x)));
% Prior on initial velocity
factors.add(PriorFactorLieVector(symbol('v',0), ...
LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, sigma_init_v)));
% Prior on initial bias
factors.add(PriorFactorConstantBias(symbol('b',0), ...
imuBias.ConstantBias(bias_acc,bias_omega), noiseModel.Isotropic.Sigma(6, sigma_init_b)));
%% Main loop:
% (1) we read the measurements
% (2) we create the corresponding factors in the graph
% (3) we solve the graph to obtain and optimal estimate of robot trajectory
i = 2;
lastTime = 0;
lastIndex = 0;
currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
times = sort([VO_data(:,1); GPS_data(:,1)]); % this are the time-stamps at which we want to initialize a new node in the graph
IMU_times = IMU_data(:,1);
disp('TODO: still needed to take care of the initial time')
for t = times
% At each non=IMU measurement we initialize a new node in the graph
currentIndex = find( times == t );
currentPoseKey = symbol('x',currentIndex);
currentVelKey = symbol('v',currentIndex);
currentBiasKey = symbol('b',currentIndex);
%% add preintegrated IMU factor between previous state and current state
% =======================================================================
IMUbetweenTimesIndices = find( IMU_times>+t_previous & IMU_times<= t);
% all imu measurements occurred between t_previous and t
% we assume that each row of the IMU.txt file has the following structure:
% timestamp delta_t acc_x acc_y acc_z omega_x omega_y omega_z
disp('TODO: We want don t want to preintegrate with zero bias, but to use the most recent estimate')
currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
for i=1:length(IMUbetweenTimesIndices)
index = IMUbetweenTimesIndices(i); % the row of the IMU_data matrix that we have to integrate
deltaT = IMU_data(index,2);
accMeas = IMU_data(index,3:5);
omegaMeas = IMU_data(index,6:8);
% Accumulate preintegrated measurement
currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT);
end
disp('TODO: is the imu noise right?')
% Create IMU factor
factors.add(ImuFactor( ...
previousPoseKey, previousVelKey, ...
currentPoseKey, currentVelKey, ...
currentBiasKey, currentSummarizedMeasurement, g, cor_v, ...
noiseModel.Isotropic.Sigma(9, 1e-6)));
% =======================================================================
%% add factor corresponding to GPS measurements (if available at the current time)
% =======================================================================
if isempty( find(GPS_data(:,1) == t ) ) == 0 % it is a GPS measurement
if length( find(GPS_data(:,1)) ) > 1
error('more GPS measurements at the same time stamp: it should be an error')
end
index = find(GPS_data(:,1) == t ); % the row of the IMU_data matrix that we have to integrate
GPSmeas = GPS_data(index,2:4);
noiseModelGPS = ???; % noiseModelGPS.Isotropic.Sigma(6, sigma_init_x))
% add factor
disp('TODO: is the GPS noise right?')
factors.add(PriorFactor???(currentPoseKey, GPSmeas, noiseModelGPS) );
end
% =======================================================================
%% add factor corresponding to VO measurements (if available at the current time)
% =======================================================================
if isempty( find(VO_data(:,1) == t ) )== 0 % it is a GPS measurement
if length( find(VO_data(:,1)) ) > 1
error('more VO measurements at the same time stamp: it should be an error')
end
index = find( VO_data(:,1) == t ); % the row of the IMU_data matrix that we have to integrate
VOmeas_pos = VO_data(index,2:4)';
VOmeas_ang = VO_data(index,5:7)';
VOpose = Pose3( Rot3(VOmeas_ang) , Point3(VOmeas_pos) );
noiseModelVO = ???; % noiseModelGPS.Isotropic.Sigma(6, sigma_init_x))
% add factor
disp('TODO: is the VO noise right?')
factors.add(BetweenFactorPose3(lastVOPoseKey, currentPoseKey, VOpose, noiseModelVO));
lastVOPoseKey = currentPoseKey;
end
% =======================================================================
disp('TODO: add values')
% values.insert(, initialPose);
% values.insert(symbol('v',lastIndex+1), initialVel);
%% Update solver
% =======================================================================
isam.update(factors, values);
factors = NonlinearFactorGraph;
values = Values;
isam.calculateEstimate(currentPoseKey);
% M = isam.marginalCovariance(key_pose);
% =======================================================================
previousPoseKey = currentPoseKey;
previousVelKey = currentVelKey;
t_previous = t;
end
disp('TODO: display results')
% figure(1)
% hold on;
% plot(positions(1,:), positions(2,:), '-b');
% plot3DTrajectory(isam.calculateEstimate, 'g-');
% axis equal;
% legend('true trajectory', 'traj integrated in body', 'traj integrated in nav')