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

Merge branch 'feature/lago'

release/4.3a0
Luca 2014-05-28 19:05:13 -04:00
parent bc13ec9f92 515b612ca0
commit 4157648d0c
17 changed files with 1291 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
examples/Data/noisyToyGraph.txt Normal file
View File

@ -0,0 +1,9 @@
VERTEX_SE2 0 0.000000 0.000000 0.000000
VERTEX_SE2 1 0.774115 1.183389 1.576173
VERTEX_SE2 2 -0.262420 2.047059 -3.127594
VERTEX_SE2 3 -1.605649 0.993891 -1.561134
EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000

9
examples/Data/optimizedNoisyToyGraph.txt Normal file
View File

@ -0,0 +1,9 @@
VERTEX_SE2 0 0.000000 -0.000000 0.000000
VERTEX_SE2 1 0.955797 1.137643 1.543041
VERTEX_SE2 2 0.129867 1.989651 3.201259
VERTEX_SE2 3 -1.047715 0.933789 4.743682
EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000

9
examples/Data/orientationsNoisyToyGraph.txt Normal file
View File

@ -0,0 +1,9 @@
VERTEX_SE2 0 0.000000 0.000000 0.000000
VERTEX_SE2 1 0.000000 0.000000 1.565449
VERTEX_SE2 2 0.000000 0.000000 3.134143
VERTEX_SE2 3 0.000000 0.000000 4.710123
EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000

23
examples/Data/pose2example-rewritten.txt Normal file
View File

@ -0,0 +1,23 @@
VERTEX_SE2 0 0 0 0
VERTEX_SE2 1 1.03039 0.01135 -0.081596
VERTEX_SE2 2 2.03614 -0.129733 -0.301887
VERTEX_SE2 3 3.0151 -0.442395 -0.345514
VERTEX_SE2 4 3.34395 0.506678 1.21471
VERTEX_SE2 5 3.68449 1.46405 1.18379
VERTEX_SE2 6 4.06463 2.41478 1.17633
VERTEX_SE2 7 4.42978 3.30018 1.25917
VERTEX_SE2 8 4.12888 2.32148 -1.82539
VERTEX_SE2 9 3.88465 1.32751 -1.95302
VERTEX_SE2 10 3.53107 0.388263 -2.14893
EDGE_SE2 0 1 1.03039 0.01135 -0.081596 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 1 2 1.0139 -0.058639 -0.220291 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 2 3 1.02765 -0.007456 -0.043627 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 3 4 -0.012016 1.00436 1.56023 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 4 5 1.01603 0.014565 -0.03093 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 5 6 1.02389 0.006808 -0.007452 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 6 7 0.957734 0.003159 0.082836 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 7 8 -1.02382 -0.013668 -3.08456 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 8 9 1.02344 0.013984 -0.127624 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 9 10 1.00335 0.02225 -0.195918 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 5 9 0.033943 0.032439 3.07364 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 3 10 0.04402 0.988477 -1.55351 44.7214 0 0 44.7214 0 30.9017

23
examples/Data/pose2example.txt Normal file
View File

@ -0,0 +1,23 @@
VERTEX_SE2 0 0.000000 0.000000 0.000000
VERTEX_SE2 1 1.030390 0.011350 -0.081596
VERTEX_SE2 2 2.036137 -0.129733 -0.301887
VERTEX_SE2 3 3.015097 -0.442395 -0.345514
VERTEX_SE2 4 3.343949 0.506678 1.214715
VERTEX_SE2 5 3.684491 1.464049 1.183785
VERTEX_SE2 6 4.064626 2.414783 1.176333
VERTEX_SE2 7 4.429778 3.300180 1.259169
VERTEX_SE2 8 4.128877 2.321481 -1.825391
VERTEX_SE2 9 3.884653 1.327509 -1.953016
VERTEX_SE2 10 3.531067 0.388263 -2.148934
EDGE_SE2 0 1 1.030390 0.011350 -0.081596 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 1 2 1.013900 -0.058639 -0.220291 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 2 3 1.027650 -0.007456 -0.043627 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 3 4 -0.012016 1.004360 1.560229 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 4 5 1.016030 0.014565 -0.030930 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 5 6 1.023890 0.006808 -0.007452 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 6 7 0.957734 0.003159 0.082836 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 7 8 -1.023820 -0.013668 -3.084560 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 8 9 1.023440 0.013984 -0.127624 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 9 10 1.003350 0.022250 -0.195918 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 5 9 0.033943 0.032439 3.073637 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 3 10 0.044020 0.988477 -1.553511 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699

63
examples/Pose2SLAMExample_g2o.cpp Normal file
View File

@ -0,0 +1,63 @@
/* ----------------------------------------------------------------------------
* 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 Pose2SLAMExample_g2o.cpp
* @brief A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph and does the
* optimization. Output is written on a file, in g2o format
* Syntax for the script is ./Pose2SLAMExample_g2o input.g2o output.g2o
* @date May 15, 2014
* @author Luca Carlone
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Key.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <fstream>
#include <sstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]){
if (argc < 2)
std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl;
const string g2oFile = argv[1];
NonlinearFactorGraph graph;
Values initial;
readG2o(g2oFile, graph, initial);
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = graph;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(outputFile, graph, result);
std::cout << "done! " << std::endl;
return 0;
}

62
examples/Pose2SLAMExample_lago.cpp Normal file
View File

@ -0,0 +1,62 @@
/* ----------------------------------------------------------------------------
* 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 Pose2SLAMExample_lago.cpp
* @brief A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem
* using LAGO (Linear Approximation for Graph Optimization). See class LagoInitializer.h
* Output is written on a file, in g2o format
* Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o
* @date May 15, 2014
* @author Luca Carlone
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Key.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/LagoInitializer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <fstream>
#include <sstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]){
if (argc < 2)
std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl;
const string g2oFile = argv[1];
NonlinearFactorGraph graph;
Values initial;
readG2o(g2oFile, graph, initial);
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = graph;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
std::cout << "Computing LAGO estimate" << std::endl;
Values estimateLago = initializeLago(graphWithPrior);
std::cout << "done!" << std::endl;
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(outputFile, graph, estimateLago);
std::cout << "done! " << std::endl;
return 0;
}

1
gtsam/geometry/Pose2.cpp
View File

@ -178,6 +178,7 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
// t = R1' * (t2-t1)
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above

4
gtsam/geometry/tests/testPose2.cpp
View File

@ -398,7 +398,7 @@ TEST( Pose2, matrix )
TEST( Pose2, compose_matrix )
{
Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) loooking at negative x
Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) looking at negative x
Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2));
EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
}
@ -412,7 +412,7 @@ TEST( Pose2, between )
//
// *--0--*--*
Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) looking at negative x
Matrix actualH1,actualH2;
Pose2 expected(M_PI/2.0, Point2(2,2));

35
gtsam/inference/graph-inl.h
View File

@ -22,7 +22,7 @@
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration"
//#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration"
#endif
#include <boost/graph/breadth_first_search.hpp>
#ifdef __GNUC__
@ -73,34 +73,41 @@ SDGraph<KEY> toBoostGraph(const G& graph) {
SDGraph<KEY> g;
typedef typename boost::graph_traits<SDGraph<KEY> >::vertex_descriptor BoostVertex;
std::map<KEY, BoostVertex> key2vertex;
BoostVertex v1, v2;
typename G::const_iterator itFactor;
// Loop over the factors
for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) {
if ((*itFactor)->keys().size() > 2)
throw(std::invalid_argument("toBoostGraph: only support factors with at most two keys"));
if ((*itFactor)->keys().size() == 1)
// Ignore factors that are not binary
if ((*itFactor)->keys().size() != 2)
continue;
// Cast the factor to the user-specified factor type F
boost::shared_ptr<F> factor = boost::dynamic_pointer_cast<F>(*itFactor);
// Ignore factors that are not of type F
if (!factor) continue;
KEY key1 = factor->key1();
KEY key2 = factor->key2();
// Retrieve the 2 keys (nodes) the factor (edge) is incident on
KEY key1 = factor->keys()[0];
KEY key2 = factor->keys()[1];
BoostVertex v1, v2;
// If key1 is a new key, add it to the key2vertex map, else get the corresponding vertex id
if (key2vertex.find(key1) == key2vertex.end()) {
v1 = add_vertex(key1, g);
key2vertex.insert(make_pair(key1, v1));
key2vertex.insert(std::pair<KEY,KEY>(key1, v1));
} else
v1 = key2vertex[key1];
// If key2 is a new key, add it to the key2vertex map, else get the corresponding vertex id
if (key2vertex.find(key2) == key2vertex.end()) {
v2 = add_vertex(key2, g);
key2vertex.insert(make_pair(key2, v2));
key2vertex.insert(std::pair<KEY,KEY>(key2, v2));
} else
v2 = key2vertex[key2];
// Add an edge with weight 1.0
boost::property<boost::edge_weight_t, double> edge_property(1.0); // assume constant edge weight here
boost::add_edge(v1, v2, edge_property, g);
}
@ -222,12 +229,11 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>
return config;
}
/* ************************************************************************* */
/* ************************************************************************* */
template<class G, class KEY, class FACTOR2>
PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) {
// Convert to a graph that boost understands
SDGraph<KEY> g = gtsam::toBoostGraph<G, FACTOR2, KEY>(fg);
// find minimum spanning tree
@ -237,13 +243,12 @@ PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) {
// convert edge to string pairs
PredecessorMap<KEY> tree;
typename SDGraph<KEY>::vertex_iterator itVertex = boost::vertices(g).first;
typename std::vector<typename SDGraph<KEY>::Vertex>::iterator vi;
for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) {
BOOST_FOREACH(const typename SDGraph<KEY>::Vertex& vi, p_map){
KEY key = boost::get(boost::vertex_name, g, *itVertex);
KEY parent = boost::get(boost::vertex_name, g, *vi);
KEY parent = boost::get(boost::vertex_name, g, vi);
tree.insert(key, parent);
itVertex++;
}
return tree;
}

346
gtsam/nonlinear/LagoInitializer.cpp Normal file
View File

@ -0,0 +1,346 @@
/* ----------------------------------------------------------------------------
* 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 LagoInitializer.h
* @author Luca Carlone
* @author Frank Dellaert
* @date May 14, 2014
*/
#include <gtsam/nonlinear/LagoInitializer.h>
#include <gtsam/slam/dataset.h>
namespace gtsam {
static Matrix I = eye(1);
static Matrix I3 = eye(3);
/* ************************************************************************* */
double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap) {
double nodeTheta = 0;
Key key_child = nodeKey; // the node
Key key_parent = 0; // the initialization does not matter
while(1){
// We check if we reached the root
if(tree.at(key_child)==key_child) // if we reached the root
break;
// we sum the delta theta corresponding to the edge parent->child
nodeTheta += deltaThetaMap.at(key_child);
// we get the parent
key_parent = tree.at(key_child); // the parent
// we check if we connected to some part of the tree we know
if(thetaFromRootMap.find(key_parent)!=thetaFromRootMap.end()){
nodeTheta += thetaFromRootMap.at(key_parent);
break;
}
key_child = key_parent; // we move upwards in the tree
}
return nodeTheta;
}
/* ************************************************************************* */
key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
const PredecessorMap<Key>& tree) {
key2doubleMap thetaToRootMap;
key2doubleMap::const_iterator it;
// Orientation of the roo
thetaToRootMap.insert(std::pair<Key, double>(keyAnchor, 0.0));
// for all nodes in the tree
for(it = deltaThetaMap.begin(); it != deltaThetaMap.end(); ++it ){
// compute the orientation wrt root
Key nodeKey = it->first;
double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
thetaToRootMap);
thetaToRootMap.insert(std::pair<Key, double>(nodeKey, nodeTheta));
}
return thetaToRootMap;
}
/* ************************************************************************* */
void getSymbolicGraph(
/*OUTPUTS*/ std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, key2doubleMap& deltaThetaMap,
/*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g){
// Get keys for which you want the orientation
size_t id=0;
// Loop over the factors
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g){
if (factor->keys().size() == 2){
Key key1 = factor->keys()[0];
Key key2 = factor->keys()[1];
// recast to a between
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
if (!pose2Between) continue;
// get the orientation - measured().theta();
double deltaTheta = pose2Between->measured().theta();
// insert (directed) orientations in the map "deltaThetaMap"
bool inTree=false;
if(tree.at(key1)==key2){ // key2 -> key1
deltaThetaMap.insert(std::pair<Key, double>(key1, -deltaTheta));
inTree = true;
} else if(tree.at(key2)==key1){ // key1 -> key2
deltaThetaMap.insert(std::pair<Key, double>(key2, deltaTheta));
inTree = true;
}
// store factor slot, distinguishing spanning tree edges from chordsIds
if(inTree == true)
spanningTreeIds.push_back(id);
else // it's a chord!
chordsIds.push_back(id);
}
id++;
}
}
/* ************************************************************************* */
void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) {
// Get the relative rotation measurement from the between factor
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (!pose2Between)
throw std::invalid_argument("buildLinearOrientationGraph: invalid between factor!");
deltaTheta = (Vector(1) << pose2Between->measured().theta());
// Retrieve the noise model for the relative rotation
SharedNoiseModel model = pose2Between->get_noiseModel();
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel)
throw std::invalid_argument("buildLinearOrientationGraph: invalid noise model "
"(current version assumes diagonal noise model)!");
Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2) ); // std on the angular measurement
model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta);
}
/* ************************************************************************* */
GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spanningTreeIds, const std::vector<size_t>& chordsIds,
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree){
GaussianFactorGraph lagoGraph;
Vector deltaTheta;
noiseModel::Diagonal::shared_ptr model_deltaTheta;
// put original measurements in the spanning tree
BOOST_FOREACH(const size_t& factorId, spanningTreeIds){
const FastVector<Key>& keys = g[factorId]->keys();
Key key1 = keys[0], key2 = keys[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta));
}
// put regularized measurements in the chordsIds
BOOST_FOREACH(const size_t& factorId, chordsIds){
const FastVector<Key>& keys = g[factorId]->keys();
Key key1 = keys[0], key2 = keys[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
double key1_DeltaTheta_key2 = deltaTheta(0);
///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl;
double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord
double k = round(k2pi_noise/(2*M_PI));
//if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug
Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI);
lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta));
}
// prior on the anchor orientation
lagoGraph.add(JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise));
return lagoGraph;
}
/* ************************************************************************* */
NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph){
NonlinearFactorGraph pose2Graph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph){
// recast to a between on Pose2
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
if (pose2Between)
pose2Graph.add(pose2Between);
// recast PriorFactor<Pose2> to BetweenFactor<Pose2>
boost::shared_ptr< PriorFactor<Pose2> > pose2Prior =
boost::dynamic_pointer_cast< PriorFactor<Pose2> >(factor);
if (pose2Prior)
pose2Graph.add(BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
pose2Prior->prior(), pose2Prior->get_noiseModel()));
}
return pose2Graph;
}
/* ************************************************************************* */
PredecessorMap<Key> findOdometricPath(const NonlinearFactorGraph& pose2Graph) {
PredecessorMap<Key> tree;
Key minKey;
bool minUnassigned = true;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2Graph){
Key key1 = std::min(factor->keys()[0], factor->keys()[1]);
Key key2 = std::max(factor->keys()[0], factor->keys()[1]);
if(minUnassigned){
minKey = key1;
minUnassigned = false;
}
if( key2 - key1 == 1){ // consecutive keys
tree.insert(key2, key1);
if(key1 < minKey)
minKey = key1;
}
}
tree.insert(minKey,keyAnchor);
tree.insert(keyAnchor,keyAnchor); // root
return tree;
}
/* ************************************************************************* */
VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){
// Find a minimum spanning tree
PredecessorMap<Key> tree;
if (useOdometricPath)
tree = findOdometricPath(pose2Graph);
else
tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, BetweenFactor<Pose2> >(pose2Graph);
// Create a linear factor graph (LFG) of scalars
key2doubleMap deltaThetaMap;
std::vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
std::vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph);
// temporary structure to correct wraparounds along loops
key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
// regularize measurements and plug everything in a factor graph
GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree);
// Solve the LFG
VectorValues orientationsLago = lagoGraph.optimize();
return orientationsLago;
}
/* ************************************************************************* */
VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath) {
// We "extract" the Pose2 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
// Get orientations from relative orientation measurements
return computeLagoOrientations(pose2Graph, useOdometricPath);
}
/* ************************************************************************* */
Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) {
// Linearized graph on full poses
GaussianFactorGraph linearPose2graph;
// We include the linear version of each between factor
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2graph){
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
if(pose2Between){
Key key1 = pose2Between->keys()[0];
double theta1 = orientationsLago.at(key1)(0);
double s1 = sin(theta1); double c1 = cos(theta1);
Key key2 = pose2Between->keys()[1];
double theta2 = orientationsLago.at(key2)(0);
double linearDeltaRot = theta2 - theta1 - pose2Between->measured().theta();
linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize
double dx = pose2Between->measured().x();
double dy = pose2Between->measured().y();
Vector globalDeltaCart = (Vector(2) << c1*dx - s1*dy, s1*dx + c1*dy);
Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot );// rhs
Matrix J1 = - I3;
J1(0,2) = s1*dx + c1*dy;
J1(1,2) = -c1*dx + s1*dy;
// Retrieve the noise model for the relative rotation
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(pose2Between->get_noiseModel());
linearPose2graph.add(JacobianFactor(key1, J1, key2, I3, b, diagonalModel));
}else{
throw std::invalid_argument("computeLagoPoses: cannot manage non between factor here!");
}
}
// add prior
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
linearPose2graph.add(JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0,0.0,0.0), priorModel));
// optimize
VectorValues posesLago = linearPose2graph.optimize();
// put into Values structure
Values initialGuessLago;
for(VectorValues::const_iterator it = posesLago.begin(); it != posesLago.end(); ++it ){
Key key = it->first;
if (key != keyAnchor){
Vector poseVector = posesLago.at(key);
Pose2 poseLago = Pose2(poseVector(0),poseVector(1),orientationsLago.at(key)(0)+poseVector(2));
initialGuessLago.insert(key, poseLago);
}
}
return initialGuessLago;
}
/* ************************************************************************* */
Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath) {
// We "extract" the Pose2 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
// Get orientations from relative orientation measurements
VectorValues orientationsLago = computeLagoOrientations(pose2Graph, useOdometricPath);
// Compute the full poses
return computeLagoPoses(pose2Graph, orientationsLago);
}
/* ************************************************************************* */
Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) {
Values initialGuessLago;
// get the orientation estimates from LAGO
VectorValues orientations = initializeOrientationsLago(graph);
// for all nodes in the tree
for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){
Key key = it->first;
if (key != keyAnchor){
Pose2 pose = initialGuess.at<Pose2>(key);
Vector orientation = orientations.at(key);
Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0));
initialGuessLago.insert(key, poseLago);
}
}
return initialGuessLago;
}
} // end of namespace gtsam

100
gtsam/nonlinear/LagoInitializer.h Normal file
View File

@ -0,0 +1,100 @@
/* ----------------------------------------------------------------------------
* 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 LagoInitializer.h
* @brief Initialize Pose2 in a factor graph using LAGO
* (Linear Approximation for Graph Optimization). see papers:
*
* L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate
* approximation for planar pose graph optimization, IJRR, 2014.
*
* L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation
* for graph-based simultaneous localization and mapping, RSS, 2011.
*
* @param graph: nonlinear factor graph (can include arbitrary factors but we assume
* that there is a subgraph involving Pose2 and betweenFactors). Also in the current
* version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc)
* and a prior on x0. This assumption can be relaxed by using the extra argument
* useOdometricPath = false, although this part of code is not stable yet.
* @return Values: initial guess from LAGO (only pose2 are initialized)
*
* @author Luca Carlone
* @author Frank Dellaert
* @date May 14, 2014
*/
#pragma once
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
namespace gtsam {
typedef std::map<Key,double> key2doubleMap;
const Key keyAnchor = symbol('Z',9999999);
noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8));
noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
/* This function computes the cumulative orientation (without wrapping) wrt the root of a spanning tree (tree)
* for a node (nodeKey). The function starts at the nodes and moves towards the root
* summing up the (directed) rotation measurements. Relative measurements are encoded in "deltaThetaMap"
* The root is assumed to have orientation zero.
*/
double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap);
/* This function computes the cumulative orientations (without wrapping)
* for all node wrt the root (root has zero orientation)
*/
key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
const PredecessorMap<Key>& tree);
/* Given a factor graph "g", and a spanning tree "tree", the function selects the nodes belonging to the tree and to g,
* and stores the factor slots corresponding to edges in the tree and to chordsIds wrt this tree
* Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree:
* for a node key2, s.t. tree[key2]=key1, the values deltaThetaMap[key2] is the relative orientation theta[key2]-theta[key1]
*/
void getSymbolicGraph(
/*OUTPUTS*/ std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, key2doubleMap& deltaThetaMap,
/*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
/* Retrieves the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2> */
void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta);
/* Linear factor graph with regularized orientation measurements */
GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spanningTreeIds, const std::vector<size_t>& chordsIds,
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
/* Selects the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */
NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph);
/* Returns the orientations of a graph including only BetweenFactors<Pose2> */
VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true);
/* LAGO: Returns the orientations of the Pose2 in a generic factor graph */
VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/* Returns the values for the Pose2 in a generic factor graph */
Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/* Only corrects the orientation part in initialGuess */
Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess);
} // end of namespace gtsam

332
gtsam/nonlinear/tests/testLagoInitializer.cpp Normal file
View File

@ -0,0 +1,332 @@
/* ----------------------------------------------------------------------------
* 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 testPlanarSLAMExample_lago.cpp
* @brief Unit tests for planar SLAM example using the initialization technique
* LAGO (Linear Approximation for Graph Optimization)
*
* @author Luca Carlone
* @author Frank Dellaert
* @date May 14, 2014
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LagoInitializer.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3);
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
namespace simple {
// We consider a small graph:
// symbolic FG
// x2 0 1
// / | \ 1 2
// / | \ 2 3
// x3 | x1 2 0
// \ | / 0 3
// \ | /
// x0
//
Pose2 pose0 = Pose2(0.000000, 0.000000, 0.000000);
Pose2 pose1 = Pose2(1.000000, 1.000000, 1.570796);
Pose2 pose2 = Pose2(0.000000, 2.000000, 3.141593);
Pose2 pose3 = Pose2(-1.000000, 1.000000, 4.712389);
NonlinearFactorGraph graph() {
NonlinearFactorGraph g;
g.add(BetweenFactor<Pose2>(x0, x1, pose0.between(pose1), model));
g.add(BetweenFactor<Pose2>(x1, x2, pose1.between(pose2), model));
g.add(BetweenFactor<Pose2>(x2, x3, pose2.between(pose3), model));
g.add(BetweenFactor<Pose2>(x2, x0, pose2.between(pose0), model));
g.add(BetweenFactor<Pose2>(x0, x3, pose0.between(pose3), model));
g.add(PriorFactor<Pose2>(x0, pose0, model));
return g;
}
}
/* *************************************************************************** */
TEST( Lago, checkSTandChords ) {
NonlinearFactorGraph g = simple::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g);
key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
DOUBLES_EQUAL(spanningTreeIds[0], 0, 1e-6); // factor 0 is the first in the ST (0->1)
DOUBLES_EQUAL(spanningTreeIds[1], 3, 1e-6); // factor 3 is the second in the ST(2->0)
DOUBLES_EQUAL(spanningTreeIds[2], 4, 1e-6); // factor 4 is the third in the ST(0->3)
}
/* *************************************************************************** */
TEST( Lago, orientationsOverSpanningTree ) {
NonlinearFactorGraph g = simple::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g);
// check the tree structure
EXPECT_LONGS_EQUAL(tree[x0], x0);
EXPECT_LONGS_EQUAL(tree[x1], x0);
EXPECT_LONGS_EQUAL(tree[x2], x0);
EXPECT_LONGS_EQUAL(tree[x3], x0);
key2doubleMap expected;
expected[x0]= 0;
expected[x1]= M_PI/2; // edge x0->x1 (consistent with edge (x0,x1))
expected[x2]= -M_PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0))
expected[x3]= -M_PI/2; // edge x0->x3 (consistent with edge (x0,x3))
key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
key2doubleMap actual;
actual = computeThetasToRoot(deltaThetaMap, tree);
DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6);
DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6);
DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6);
DOUBLES_EQUAL(expected[x3], actual[x3], 1e-6);
}
/* *************************************************************************** */
TEST( Lago, regularizedMeasurements ) {
NonlinearFactorGraph g = simple::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g);
key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree);
std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian();
// jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded)
Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4));
// this is the whitened error, so we multiply by the std to unwhiten
actual = 0.1 * actual;
// Expected regularized measurements (same for the spanning tree, corrected for the chordsIds)
Vector expected = (Vector(5) << M_PI/2, M_PI, -M_PI/2, M_PI/2 - 2*M_PI , M_PI/2);
EXPECT(assert_equal(expected, actual, 1e-6));
}
/* *************************************************************************** */
TEST( Lago, smallGraphVectorValues ) {
bool useOdometricPath = false;
VectorValues initialGuessLago = initializeOrientationsLago(simple::graph(), useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, smallGraphVectorValuesSP ) {
VectorValues initialGuessLago = initializeOrientationsLago(simple::graph());
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, multiplePosePriors ) {
bool useOdometricPath = false;
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, multiplePosePriorsSP ) {
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
VectorValues initialGuessLago = initializeOrientationsLago(g);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, multiplePoseAndRotPriors ) {
bool useOdometricPath = false;
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, multiplePoseAndRotPriorsSP ) {
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
VectorValues initialGuessLago = initializeOrientationsLago(g);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, smallGraphValues ) {
// we set the orientations in the initial guess to zero
Values initialGuess;
initialGuess.insert(x0,Pose2(simple::pose0.x(),simple::pose0.y(),0.0));
initialGuess.insert(x1,Pose2(simple::pose1.x(),simple::pose1.y(),0.0));
initialGuess.insert(x2,Pose2(simple::pose2.x(),simple::pose2.y(),0.0));
initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0));
// lago does not touch the Cartesian part and only fixed the orientations
Values actual = initializeLago(simple::graph(), initialGuess);
// we are in a noiseless case
Values expected;
expected.insert(x0,simple::pose0);
expected.insert(x1,simple::pose1);
expected.insert(x2,simple::pose2);
expected.insert(x3,simple::pose3);
EXPECT(assert_equal(expected, actual, 1e-6));
}
/* *************************************************************************** */
TEST( Lago, smallGraph2 ) {
// lago does not touch the Cartesian part and only fixed the orientations
Values actual = initializeLago(simple::graph());
// we are in a noiseless case
Values expected;
expected.insert(x0,simple::pose0);
expected.insert(x1,simple::pose1);
expected.insert(x2,simple::pose2);
expected.insert(x3,simple::pose3);
EXPECT(assert_equal(expected, actual, 1e-6));
}
/* *************************************************************************** */
TEST( Lago, largeGraphNoisy_orientations ) {
NonlinearFactorGraph g;
Values initial;
string inputFile = findExampleDataFile("noisyToyGraph");
readG2o(inputFile, g, initial);
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = g;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
VectorValues actualVV = initializeOrientationsLago(graphWithPrior);
Values actual;
Key keyAnc = symbol('Z',9999999);
for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){
Key key = it->first;
if (key != keyAnc){
Vector orientation = actualVV.at(key);
Pose2 poseLago = Pose2(0.0,0.0,orientation(0));
actual.insert(key, poseLago);
}
}
NonlinearFactorGraph gmatlab;
Values expected;
string matlabFile = findExampleDataFile("orientationsNoisyToyGraph");
readG2o(matlabFile, gmatlab, expected);
BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){
Key k = key_val.key;
EXPECT(assert_equal(expected.at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
}
}
/* *************************************************************************** */
TEST( Lago, largeGraphNoisy ) {
NonlinearFactorGraph g;
Values initial;
string inputFile = findExampleDataFile("noisyToyGraph");
readG2o(inputFile, g, initial);
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = g;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
Values actual = initializeLago(graphWithPrior);
NonlinearFactorGraph gmatlab;
Values expected;
string matlabFile = findExampleDataFile("optimizedNoisyToyGraph");
readG2o(matlabFile, gmatlab, expected);
BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){
Key k = key_val.key;
EXPECT(assert_equal(expected.at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

111
gtsam/slam/dataset.cpp
View File

@ -544,6 +544,117 @@ bool readBundler(const string& filename, SfM_data &data)
return true;
}
/* ************************************************************************* */
bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial,
const kernelFunctionType kernelFunction){
ifstream is(g2oFile.c_str());
if (!is){
throw std::invalid_argument("File not found!");
return false;
}
// READ INITIAL GUESS FROM G2O FILE
string tag;
while (is) {
if(! (is >> tag))
break;
if (tag == "VERTEX_SE2" || tag == "VERTEX2") {
int id;
double x, y, yaw;
is >> id >> x >> y >> yaw;
initial.insert(id, Pose2(x, y, yaw));
}
is.ignore(LINESIZE, '\n');
}
is.clear(); /* clears the end-of-file and error flags */
is.seekg(0, ios::beg);
// initial.print("initial guess");
// READ MEASUREMENTS FROM G2O FILE
while (is) {
if(! (is >> tag))
break;
if (tag == "EDGE_SE2" || tag == "EDGE2") {
int id1, id2;
double x, y, yaw;
double I11, I12, I13, I22, I23, I33;
is >> id1 >> id2 >> x >> y >> yaw;
is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33;
Pose2 l1Xl2(x, y, yaw);
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << I11, I22, I33));
switch (kernelFunction) {
{case QUADRATIC:
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
graph.add(factor);
break;}
{case HUBER:
NonlinearFactor::shared_ptr huberFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model)));
graph.add(huberFactor);
break;}
{case TUKEY:
NonlinearFactor::shared_ptr tukeyFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model)));
graph.add(tukeyFactor);
break;}
}
}
is.ignore(LINESIZE, '\n');
}
// Output which kernel is used
switch (kernelFunction) {
case QUADRATIC:
break;
case HUBER:
std::cout << "Robust kernel: Huber" << std::endl; break;
case TUKEY:
std::cout << "Robust kernel: Tukey" << std::endl; break;
}
return true;
}
/* ************************************************************************* */
bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate){
fstream stream(filename.c_str(), fstream::out);
// save poses
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate)
{
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
<< pose.y() << " " << pose.theta() << endl;
}
// save edges
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph)
{
boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (!factor)
continue;
SharedNoiseModel model = factor->get_noiseModel();
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel)
throw std::invalid_argument("writeG2o: invalid noise model (current version assumes diagonal noise model)!");
Pose2 pose = factor->measured(); //.inverse();
stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
<< diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
<< diagonalModel->precision(1) << " " << 0.0 << " " << diagonalModel->precision(2) << endl;
}
stream.close();
return true;
}
/* ************************************************************************* */
bool readBAL(const string& filename, SfM_data &data)
{

19
gtsam/slam/dataset.h
View File

@ -117,6 +117,25 @@ struct SfM_data
*/
GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
/**
* @brief This function parses a g2o file and stores the measurements into a
* NonlinearFactorGraph and the initial guess in a Values structure
* @param filename The name of the g2o file
* @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal.
* @return initial Values containing the initial guess (VERTEX_SE2)
*/
enum kernelFunctionType { QUADRATIC, HUBER, TUKEY };
bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC);
/**
* @brief This function writes a g2o file from
* NonlinearFactorGraph and a Values structure
* @param filename The name of the g2o file to write
* @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
* @return estimate Values containing the values (VERTEX_SE2)
*/
bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate);
/**
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
* SfM_data structure

127
gtsam/slam/tests/testDataset.cpp
View File

@ -22,6 +22,8 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h>
using namespace gtsam::symbol_shorthand;
@ -37,6 +39,20 @@ TEST(dataSet, findExampleDataFile) {
EXPECT(assert_equal(expected_end, actual_end));
}
/* ************************************************************************* */
//TEST( dataSet, load2D)
//{
// ///< The structure where we will save the SfM data
// const string filename = findExampleDataFile("smallGraph.g2o");
// boost::tie(graph,initialGuess) = load2D(filename, boost::none, 10000,
// false, false);
//// print
////
//// print
////
//// EXPECT(assert_equal(expected,actual,12));
//}
/* ************************************************************************* */
TEST( dataSet, Balbianello)
{
@ -58,6 +74,117 @@ TEST( dataSet, Balbianello)
EXPECT(assert_equal(expected,actual,1));
}
/* ************************************************************************* */
TEST( dataSet, readG2o)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph actualGraph;
Values actualValues;
readG2o(g2oFile, actualGraph, actualValues);
Values expectedValues;
expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596));
expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887));
expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514));
expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715));
expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785));
expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333));
expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169));
expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391));
expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016));
expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
EXPECT(assert_equal(expectedValues,actualValues,1e-5));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
NonlinearFactorGraph expectedGraph;
expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
expectedGraph.add(BetweenFactor<Pose2>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model));
expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model));
expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model));
expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model));
expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model));
expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model));
expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model));
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, readG2oHuber)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph actualGraph;
Values actualValues;
readG2o(g2oFile, actualGraph, actualValues, HUBER);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
NonlinearFactorGraph expectedGraph;
expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
expectedGraph.add(BetweenFactor<Pose2>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model));
expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model));
expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model));
expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model));
expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model));
expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model));
expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model));
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, readG2oTukey)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph actualGraph;
Values actualValues;
readG2o(g2oFile, actualGraph, actualValues, TUKEY);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
NonlinearFactorGraph expectedGraph;
expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
expectedGraph.add(BetweenFactor<Pose2>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model));
expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model));
expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model));
expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model));
expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model));
expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model));
expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model));
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, writeG2o)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph expectedGraph;
Values expectedValues;
readG2o(g2oFile, expectedGraph, expectedValues);
const string filenameToWrite = findExampleDataFile("pose2example-rewritten");
writeG2o(filenameToWrite, expectedGraph, expectedValues);
NonlinearFactorGraph actualGraph;
Values actualValues;
readG2o(filenameToWrite, actualGraph, actualValues);
EXPECT(assert_equal(expectedValues,actualValues,1e-5));
EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, readBAL_Dubrovnik)
{

51
tests/testGraph.cpp
View File

@ -18,7 +18,10 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/graph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <CppUnitLite/TestHarness.h>
@ -105,24 +108,38 @@ TEST( Graph, composePoses )
CHECK(assert_equal(expected, *actual));
}
// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree )
//{
// GaussianFactorGraph g;
// Matrix I = eye(2);
// Vector b = Vector_(0, 0, 0);
// g += X(1), I, X(2), I, b, model;
// g += X(1), I, X(3), I, b, model;
// g += X(1), I, X(4), I, b, model;
// g += X(2), I, X(3), I, b, model;
// g += X(2), I, X(4), I, b, model;
// g += X(3), I, X(4), I, b, model;
/* ************************************************************************* */
TEST( GaussianFactorGraph, findMinimumSpanningTree )
{
GaussianFactorGraph g;
Matrix I = eye(2);
Vector2 b(0, 0);
const SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(2) << 0.5, 0.5));
using namespace symbol_shorthand;
g += JacobianFactor(X(1), I, X(2), I, b, model);
g += JacobianFactor(X(1), I, X(3), I, b, model);
g += JacobianFactor(X(1), I, X(4), I, b, model);
g += JacobianFactor(X(2), I, X(3), I, b, model);
g += JacobianFactor(X(2), I, X(4), I, b, model);
g += JacobianFactor(X(3), I, X(4), I, b, model);
PredecessorMap<Key> tree = findMinimumSpanningTree<GaussianFactorGraph, Key, JacobianFactor>(g);
EXPECT_LONGS_EQUAL(X(1),tree[X(1)]);
EXPECT_LONGS_EQUAL(X(1),tree[X(2)]);
EXPECT_LONGS_EQUAL(X(1),tree[X(3)]);
EXPECT_LONGS_EQUAL(X(1),tree[X(4)]);
// we add a disconnected component - does not work yet
// g += JacobianFactor(X(5), I, X(6), I, b, model);
//
// map<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>();
// EXPECT(tree[X(1)].compare(X(1))==0);
// EXPECT(tree[X(2)].compare(X(1))==0);
// EXPECT(tree[X(3)].compare(X(1))==0);
// EXPECT(tree[X(4)].compare(X(1))==0);
//}
// PredecessorMap<Key> forest = findMinimumSpanningTree<GaussianFactorGraph, Key, JacobianFactor>(g);
// EXPECT_LONGS_EQUAL(X(1),forest[X(1)]);
// EXPECT_LONGS_EQUAL(X(1),forest[X(2)]);
// EXPECT_LONGS_EQUAL(X(1),forest[X(3)]);
// EXPECT_LONGS_EQUAL(X(1),forest[X(4)]);
// EXPECT_LONGS_EQUAL(X(5),forest[X(5)]);
// EXPECT_LONGS_EQUAL(X(5),forest[X(6)]);
}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactorGraph, split )