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

Returning ordering from planarGraph is obsolete since Index change

release/4.3a0
Frank Dellaert 2012-06-09 18:52:22 +00:00
parent 03465500e9
commit 54bfe722ad
3 changed files with 25 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/slam/smallExample.cpp
View File

@ -135,7 +135,7 @@ namespace example {
} }
/* ************************************************************************* */ /* ************************************************************************* */
FactorGraph<JacobianFactor> createGaussianFactorGraph(const Ordering& ordering) { JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
// Create empty graph // Create empty graph
GaussianFactorGraph fg; GaussianFactorGraph fg;
@ -153,7 +153,7 @@ namespace example {
// measurement between x2 and l1: l1-x2=[-0.2;0.3] // measurement between x2 and l1: l1-x2=[-0.2;0.3]
fg.add(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2); fg.add(ordering[X(2)], -5*eye(2), ordering[L(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2);
return *fg.dynamicCastFactors<FactorGraph<JacobianFactor> >(); return *fg.dynamicCastFactors<JacobianFactorGraph >();
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -272,7 +272,7 @@ namespace example {
} }
/* ************************************************************************* */ /* ************************************************************************* */
FactorGraph<JacobianFactor> createSimpleConstraintGraph() { JacobianFactorGraph createSimpleConstraintGraph() {
// create unary factor // create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1 // prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2); Matrix Ax = eye(2);
@ -292,7 +292,7 @@ namespace example {
constraintModel)); constraintModel));
// construct the graph // construct the graph
FactorGraph<JacobianFactor> fg; JacobianFactorGraph fg;
fg.push_back(f1); fg.push_back(f1);
fg.push_back(f2); fg.push_back(f2);
@ -309,7 +309,7 @@ namespace example {
} }
/* ************************************************************************* */ /* ************************************************************************* */
FactorGraph<JacobianFactor> createSingleConstraintGraph() { JacobianFactorGraph createSingleConstraintGraph() {
// create unary factor // create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1 // prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2); Matrix Ax = eye(2);
@ -334,7 +334,7 @@ namespace example {
constraintModel)); constraintModel));
// construct the graph // construct the graph
FactorGraph<JacobianFactor> fg; JacobianFactorGraph fg;
fg.push_back(f1); fg.push_back(f1);
fg.push_back(f2); fg.push_back(f2);
@ -350,7 +350,7 @@ namespace example {
} }
/* ************************************************************************* */ /* ************************************************************************* */
FactorGraph<JacobianFactor> createMultiConstraintGraph() { JacobianFactorGraph createMultiConstraintGraph() {
// unary factor 1 // unary factor 1
Matrix A = eye(2); Matrix A = eye(2);
Vector b = Vector_(2, -2.0, 2.0); Vector b = Vector_(2, -2.0, 2.0);
@ -395,7 +395,7 @@ namespace example {
constraintModel)); constraintModel));
// construct the graph // construct the graph
FactorGraph<JacobianFactor> fg; JacobianFactorGraph fg;
fg.push_back(lf1); fg.push_back(lf1);
fg.push_back(lc1); fg.push_back(lc1);
fg.push_back(lc2); fg.push_back(lc2);
@ -420,7 +420,7 @@ namespace example {
} }
/* ************************************************************************* */ /* ************************************************************************* */
boost::tuple<FactorGraph<GaussianFactor>, Ordering, VectorValues> planarGraph(size_t N) { boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
// create empty graph // create empty graph
NonlinearFactorGraph nlfg; NonlinearFactorGraph nlfg;
@ -457,7 +457,7 @@ namespace example {
xtrue[ordering[key(x, y)]] = Point2(x,y).vector(); xtrue[ordering[key(x, y)]] = Point2(x,y).vector();
// linearize around zero // linearize around zero
return boost::make_tuple(*nlfg.linearize(zeros, ordering), ordering, xtrue); return boost::make_tuple(*nlfg.linearize(zeros, ordering), xtrue);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -470,9 +470,9 @@ namespace example {
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<FactorGraph<JacobianFactor>, FactorGraph<JacobianFactor> > splitOffPlanarTree(size_t N, pair<JacobianFactorGraph, JacobianFactorGraph > splitOffPlanarTree(size_t N,
const FactorGraph<JacobianFactor>& original) { const JacobianFactorGraph& original) {
FactorGraph<JacobianFactor> T, C; JacobianFactorGraph T, C;
// Add the x11 constraint to the tree // Add the x11 constraint to the tree
T.push_back(original[0]); T.push_back(original[0]);

19
gtsam/slam/smallExample.h
View File

@ -21,9 +21,10 @@
#pragma once #pragma once
#include <boost/tuple/tuple.hpp>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/simulated2D.h> #include <gtsam/slam/simulated2D.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <boost/tuple/tuple.hpp>
namespace gtsam { namespace gtsam {
namespace example { namespace example {
@ -65,7 +66,7 @@ namespace gtsam {
* create a linear factor graph * create a linear factor graph
* The non-linear graph above evaluated at NoisyValues * The non-linear graph above evaluated at NoisyValues
*/ */
FactorGraph<JacobianFactor> createGaussianFactorGraph(const Ordering& ordering); JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering);
/** /**
* create small Chordal Bayes Net x <- y * create small Chordal Bayes Net x <- y
@ -99,21 +100,21 @@ namespace gtsam {
* Creates a simple constrained graph with one linear factor and * Creates a simple constrained graph with one linear factor and
* one binary equality constraint that sets x = y * one binary equality constraint that sets x = y
*/ */
FactorGraph<JacobianFactor> createSimpleConstraintGraph(); JacobianFactorGraph createSimpleConstraintGraph();
VectorValues createSimpleConstraintValues(); VectorValues createSimpleConstraintValues();
/** /**
* Creates a simple constrained graph with one linear factor and * Creates a simple constrained graph with one linear factor and
* one binary constraint. * one binary constraint.
*/ */
FactorGraph<JacobianFactor> createSingleConstraintGraph(); JacobianFactorGraph createSingleConstraintGraph();
VectorValues createSingleConstraintValues(); VectorValues createSingleConstraintValues();
/** /**
* Creates a constrained graph with a linear factor and two * Creates a constrained graph with a linear factor and two
* binary constraints that share a node * binary constraints that share a node
*/ */
FactorGraph<JacobianFactor> createMultiConstraintGraph(); JacobianFactorGraph createMultiConstraintGraph();
VectorValues createMultiConstraintValues(); VectorValues createMultiConstraintValues();
/* ******************************************************* */ /* ******************************************************* */
@ -129,7 +130,7 @@ namespace gtsam {
* -x11-x21-x31 * -x11-x21-x31
* with x11 clamped at (1,1), and others related by 2D odometry. * with x11 clamped at (1,1), and others related by 2D odometry.
*/ */
boost::tuple<FactorGraph<GaussianFactor>, Ordering, VectorValues> planarGraph(size_t N); boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N);
/* /*
* Create canonical ordering for planar graph that also works for tree * Create canonical ordering for planar graph that also works for tree
@ -146,8 +147,8 @@ namespace gtsam {
* | * |
* -x11-x21-x31 * -x11-x21-x31
*/ */
std::pair<FactorGraph<JacobianFactor>, FactorGraph<JacobianFactor> > splitOffPlanarTree( std::pair<JacobianFactorGraph, JacobianFactorGraph > splitOffPlanarTree(
size_t N, const FactorGraph<JacobianFactor>& original); size_t N, const JacobianFactorGraph& original);
} // example } // example
} // gtsam } // gtsam

8
tests/timeGaussianFactorGraph.cpp
View File

@ -43,10 +43,8 @@ double timeKalmanSmoother(int T) {
// Create a planar factor graph and optimize // Create a planar factor graph and optimize
// todo: use COLAMD ordering again (removed when linear baked-in ordering added) // todo: use COLAMD ordering again (removed when linear baked-in ordering added)
double timePlanarSmoother(int N, bool old = true) { double timePlanarSmoother(int N, bool old = true) {
boost::tuple<GaussianFactorGraph, Ordering, VectorValues> pg = planarGraph(N); boost::tuple<GaussianFactorGraph, VectorValues> pg = planarGraph(N);
GaussianFactorGraph& fg(pg.get<0>()); GaussianFactorGraph& fg(pg.get<0>());
// Ordering& ordering(pg.get<1>());
// VectorValues& config(pg.get<2>());
clock_t start = clock(); clock_t start = clock();
GaussianSequentialSolver(fg).optimize(); GaussianSequentialSolver(fg).optimize();
clock_t end = clock (); clock_t end = clock ();
@ -58,10 +56,8 @@ double timePlanarSmoother(int N, bool old = true) {
// Create a planar factor graph and eliminate // Create a planar factor graph and eliminate
// todo: use COLAMD ordering again (removed when linear baked-in ordering added) // todo: use COLAMD ordering again (removed when linear baked-in ordering added)
double timePlanarSmootherEliminate(int N, bool old = true) { double timePlanarSmootherEliminate(int N, bool old = true) {
boost::tuple<GaussianFactorGraph, Ordering, VectorValues> pg = planarGraph(N); boost::tuple<GaussianFactorGraph, VectorValues> pg = planarGraph(N);
GaussianFactorGraph& fg(pg.get<0>()); GaussianFactorGraph& fg(pg.get<0>());
// Ordering& ordering(pg.get<1>());
// VectorValues& config(pg.get<2>());
clock_t start = clock(); clock_t start = clock();
GaussianSequentialSolver(fg).eliminate(); GaussianSequentialSolver(fg).eliminate();
clock_t end = clock (); clock_t end = clock ();