Returning ordering from planarGraph is obsolete since Index change
Frank Dellaert
parent
03465500e9
commit
54bfe722ad
|
|
@ -135,7 +135,7 @@ namespace example {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FactorGraph<JacobianFactor> createGaussianFactorGraph(const Ordering& ordering) {
|
||||
JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
|
||||
// Create empty graph
|
||||
GaussianFactorGraph fg;
|
||||
|
||||
|
|
@ -153,7 +153,7 @@ namespace example {
|
|||
// 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);
|
||||
|
||||
return *fg.dynamicCastFactors<FactorGraph<JacobianFactor> >();
|
||||
return *fg.dynamicCastFactors<JacobianFactorGraph >();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -272,7 +272,7 @@ namespace example {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FactorGraph<JacobianFactor> createSimpleConstraintGraph() {
|
||||
JacobianFactorGraph createSimpleConstraintGraph() {
|
||||
// create unary factor
|
||||
// prior on _x_, mean = [1,-1], sigma=0.1
|
||||
Matrix Ax = eye(2);
|
||||
|
|
@ -292,7 +292,7 @@ namespace example {
|
|||
constraintModel));
|
||||
|
||||
// construct the graph
|
||||
FactorGraph<JacobianFactor> fg;
|
||||
JacobianFactorGraph fg;
|
||||
fg.push_back(f1);
|
||||
fg.push_back(f2);
|
||||
|
||||
|
|
@ -309,7 +309,7 @@ namespace example {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FactorGraph<JacobianFactor> createSingleConstraintGraph() {
|
||||
JacobianFactorGraph createSingleConstraintGraph() {
|
||||
// create unary factor
|
||||
// prior on _x_, mean = [1,-1], sigma=0.1
|
||||
Matrix Ax = eye(2);
|
||||
|
|
@ -334,7 +334,7 @@ namespace example {
|
|||
constraintModel));
|
||||
|
||||
// construct the graph
|
||||
FactorGraph<JacobianFactor> fg;
|
||||
JacobianFactorGraph fg;
|
||||
fg.push_back(f1);
|
||||
fg.push_back(f2);
|
||||
|
||||
|
|
@ -350,7 +350,7 @@ namespace example {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
FactorGraph<JacobianFactor> createMultiConstraintGraph() {
|
||||
JacobianFactorGraph createMultiConstraintGraph() {
|
||||
// unary factor 1
|
||||
Matrix A = eye(2);
|
||||
Vector b = Vector_(2, -2.0, 2.0);
|
||||
|
|
@ -395,7 +395,7 @@ namespace example {
|
|||
constraintModel));
|
||||
|
||||
// construct the graph
|
||||
FactorGraph<JacobianFactor> fg;
|
||||
JacobianFactorGraph fg;
|
||||
fg.push_back(lf1);
|
||||
fg.push_back(lc1);
|
||||
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
|
||||
NonlinearFactorGraph nlfg;
|
||||
|
|
@ -457,7 +457,7 @@ namespace example {
|
|||
xtrue[ordering[key(x, y)]] = Point2(x,y).vector();
|
||||
|
||||
// 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,
|
||||
const FactorGraph<JacobianFactor>& original) {
|
||||
FactorGraph<JacobianFactor> T, C;
|
||||
pair<JacobianFactorGraph, JacobianFactorGraph > splitOffPlanarTree(size_t N,
|
||||
const JacobianFactorGraph& original) {
|
||||
JacobianFactorGraph T, C;
|
||||
|
||||
// Add the x11 constraint to the tree
|
||||
T.push_back(original[0]);
|
||||
|
|
|
|||
|
|
@ -21,9 +21,10 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/slam/simulated2D.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/linear/JacobianFactorGraph.h>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
namespace example {
|
||||
|
|
@ -65,7 +66,7 @@ namespace gtsam {
|
|||
* create a linear factor graph
|
||||
* 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
|
||||
|
|
@ -99,21 +100,21 @@ namespace gtsam {
|
|||
* Creates a simple constrained graph with one linear factor and
|
||||
* one binary equality constraint that sets x = y
|
||||
*/
|
||||
FactorGraph<JacobianFactor> createSimpleConstraintGraph();
|
||||
JacobianFactorGraph createSimpleConstraintGraph();
|
||||
VectorValues createSimpleConstraintValues();
|
||||
|
||||
/**
|
||||
* Creates a simple constrained graph with one linear factor and
|
||||
* one binary constraint.
|
||||
*/
|
||||
FactorGraph<JacobianFactor> createSingleConstraintGraph();
|
||||
JacobianFactorGraph createSingleConstraintGraph();
|
||||
VectorValues createSingleConstraintValues();
|
||||
|
||||
/**
|
||||
* Creates a constrained graph with a linear factor and two
|
||||
* binary constraints that share a node
|
||||
*/
|
||||
FactorGraph<JacobianFactor> createMultiConstraintGraph();
|
||||
JacobianFactorGraph createMultiConstraintGraph();
|
||||
VectorValues createMultiConstraintValues();
|
||||
|
||||
/* ******************************************************* */
|
||||
|
|
@ -129,7 +130,7 @@ namespace gtsam {
|
|||
* -x11-x21-x31
|
||||
* 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
|
||||
|
|
@ -146,8 +147,8 @@ namespace gtsam {
|
|||
* |
|
||||
* -x11-x21-x31
|
||||
*/
|
||||
std::pair<FactorGraph<JacobianFactor>, FactorGraph<JacobianFactor> > splitOffPlanarTree(
|
||||
size_t N, const FactorGraph<JacobianFactor>& original);
|
||||
std::pair<JacobianFactorGraph, JacobianFactorGraph > splitOffPlanarTree(
|
||||
size_t N, const JacobianFactorGraph& original);
|
||||
|
||||
} // example
|
||||
} // gtsam
|
||||
|
|
|
|||
|
|
@ -43,10 +43,8 @@ double timeKalmanSmoother(int T) {
|
|||
// Create a planar factor graph and optimize
|
||||
// todo: use COLAMD ordering again (removed when linear baked-in ordering added)
|
||||
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>());
|
||||
// Ordering& ordering(pg.get<1>());
|
||||
// VectorValues& config(pg.get<2>());
|
||||
clock_t start = clock();
|
||||
GaussianSequentialSolver(fg).optimize();
|
||||
clock_t end = clock ();
|
||||
|
|
@ -58,10 +56,8 @@ double timePlanarSmoother(int N, bool old = true) {
|
|||
// Create a planar factor graph and eliminate
|
||||
// todo: use COLAMD ordering again (removed when linear baked-in ordering added)
|
||||
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>());
|
||||
// Ordering& ordering(pg.get<1>());
|
||||
// VectorValues& config(pg.get<2>());
|
||||
clock_t start = clock();
|
||||
GaussianSequentialSolver(fg).eliminate();
|
||||
clock_t end = clock ();
|
||||
|
|
|
|||
Loading…
Reference in New Issue