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

Removed JacobianFactorGraph - moved its linear algebra interface to GaussianFactorGraph and redirected all uses of it to GaussianFactorGraph

release/4.3a0
Richard Roberts 2012-09-04 22:42:09 +00:00
parent c55f4be0d1
commit b0508cc1a8
23 changed files with 292 additions and 323 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/linear/GaussianBayesNet.cpp
View File

@ -16,7 +16,7 @@
*/
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/BayesNet-inl.h>
@ -242,12 +242,12 @@ double determinant(const GaussianBayesNet& bayesNet) {
/* ************************************************************************* */
VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) {
return gradient(FactorGraph<JacobianFactor>(bayesNet), x0);
return gradient(GaussianFactorGraph(bayesNet), x0);
}
/* ************************************************************************* */
void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) {
gradientAtZero(FactorGraph<JacobianFactor>(bayesNet), g);
gradientAtZero(GaussianFactorGraph(bayesNet), g);
}
/* ************************************************************************* */

2
gtsam/linear/GaussianBayesTree.cpp
View File

@ -18,7 +18,7 @@
*/
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
namespace gtsam {

147
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -521,4 +521,151 @@ break;
} // \EliminatePreferCholesky
/* ************************************************************************* */
Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) {
Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
e.push_back((*Ai)*x);
}
return e;
}
/* ************************************************************************* */
void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e) {
multiplyInPlace(fg,x,e.begin());
}
/* ************************************************************************* */
void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) {
Errors::iterator ei = e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
*ei = (*Ai)*x;
ei++;
}
}
/* ************************************************************************* */
// x += alpha*A'*e
void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) {
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
}
}
/* ************************************************************************* */
VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0) {
// It is crucial for performance to make a zero-valued clone of x
VectorValues g = VectorValues::Zero(x0);
Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
e.push_back(Ai->error_vector(x0));
}
transposeMultiplyAdd(fg, 1.0, e, g);
return g;
}
/* ************************************************************************* */
void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g) {
// Zero-out the gradient
g.setZero();
Errors e;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
e.push_back(-Ai->getb());
}
transposeMultiplyAdd(fg, 1.0, e, g);
}
/* ************************************************************************* */
void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
Index i = 0 ;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
r[i] = Ai->getb();
i++;
}
VectorValues Ax = VectorValues::SameStructure(r);
multiply(fg,x,Ax);
axpy(-1.0,Ax,r);
}
/* ************************************************************************* */
void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
r.vector() = Vector::Zero(r.dim());
Index i = 0;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
SubVector &y = r[i];
for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
y += Ai->getA(j) * x[*j];
}
++i;
}
}
/* ************************************************************************* */
void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) {
x.vector() = Vector::Zero(x.dim());
Index i = 0;
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
x[*j] += Ai->getA(j).transpose() * r[i];
}
++i;
}
}
/* ************************************************************************* */
boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x) {
boost::shared_ptr<Errors> e(new Errors);
BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
JacobianFactor::shared_ptr Ai;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(Ai_G))
Ai = Ai_J;
else
Ai = boost::make_shared<JacobianFactor>(*Ai_G); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
e->push_back(Ai->error_vector(x));
}
return e;
}
} // namespace gtsam

66
gtsam/linear/GaussianFactorGraph.h
View File

@ -31,24 +31,6 @@
namespace gtsam {
/** return A*x-b
* \todo Make this a member function - affects SubgraphPreconditioner */
template<class FACTOR>
Errors gaussianErrors(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
return *gaussianErrors_(fg, x);
}
/** shared pointer version
* \todo Make this a member function - affects SubgraphPreconditioner */
template<class FACTOR>
boost::shared_ptr<Errors> gaussianErrors_(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
boost::shared_ptr<Errors> e(new Errors);
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, fg) {
e->push_back(factor->error_vector(x));
}
return e;
}
/**
* A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
* Factor == GaussianFactor
@ -322,4 +304,52 @@ namespace gtsam {
GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1);
/** return A*x */
Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x);
/** In-place version e <- A*x that overwrites e. */
void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e);
/** In-place version e <- A*x that takes an iterator. */
void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e);
/** x += alpha*A'*e */
void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around \f$ x = x_0 \f$.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @param x0 The center about which to compute the gradient
* @return The gradient as a VectorValues
*/
VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around zero.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues
*/
void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g);
/* matrix-vector operations */
void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x);
/** shared pointer version
* \todo Make this a member function - affects SubgraphPreconditioner */
boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x);
/** return A*x-b
* \todo Make this a member function - affects SubgraphPreconditioner */
inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) {
return *gaussianErrors_(fg, x); }
} // namespace gtsam

135
gtsam/linear/JacobianFactorGraph.cpp
View File

@ -1,135 +0,0 @@
/**
* @file JacobianFactorGraph.h
* @date Jun 6, 2012
* @author Yong Dian Jian
*/
#include <gtsam/linear/JacobianFactorGraph.h>
#include <boost/foreach.hpp>
namespace gtsam {
/* ************************************************************************* */
Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x) {
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
e.push_back((*Ai)*x);
}
return e;
}
/* ************************************************************************* */
void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e) {
multiplyInPlace(fg,x,e.begin());
}
/* ************************************************************************* */
void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) {
Errors::iterator ei = e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
*ei = (*Ai)*x;
ei++;
}
}
/* ************************************************************************* */
// x += alpha*A'*e
void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) {
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
}
}
/* ************************************************************************* */
VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0) {
// It is crucial for performance to make a zero-valued clone of x
VectorValues g = VectorValues::Zero(x0);
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
e.push_back(factor->error_vector(x0));
}
transposeMultiplyAdd(fg, 1.0, e, g);
return g;
}
/* ************************************************************************* */
void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g) {
// Zero-out the gradient
g.setZero();
Errors e;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
e.push_back(-factor->getb());
}
transposeMultiplyAdd(fg, 1.0, e, g);
}
/* ************************************************************************* */
void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
Index i = 0 ;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
r[i] = factor->getb();
i++;
}
VectorValues Ax = VectorValues::SameStructure(r);
multiply(fg,x,Ax);
axpy(-1.0,Ax,r);
}
/* ************************************************************************* */
void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
r.vector() = Vector::Zero(r.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
SubVector &y = r[i];
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
y += factor->getA(j) * x[*j];
}
++i;
}
}
/* ************************************************************************* */
void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x) {
x.vector() = Vector::Zero(x.dim());
Index i = 0;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
x[*j] += factor->getA(j).transpose() * r[i];
}
++i;
}
}
/* ************************************************************************* */
JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg) {
JacobianFactorGraph::shared_ptr jfg(new JacobianFactorGraph());
jfg->reserve(gfg.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) {
JacobianFactor::shared_ptr castedFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
if(castedFactor) jfg->push_back(castedFactor);
else throw std::invalid_argument("dynamicCastFactors(), dynamic_cast failed, meaning an invalid cast was requested.");
}
return jfg;
}
/* ************************************************************************* */
JacobianFactorGraph::shared_ptr convertToJacobianFactorGraph(const GaussianFactorGraph &gfg) {
JacobianFactorGraph::shared_ptr jfg(new JacobianFactorGraph());
jfg->reserve(gfg.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr & factor, gfg) {
if( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor) ) {
jfg->push_back(jf);
}
else {
jfg->push_back(boost::make_shared<JacobianFactor>(*factor));
}
}
return jfg;
}
} // namespace

75
gtsam/linear/JacobianFactorGraph.h
View File

@ -1,75 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 JacobianFactorGraph.cpp
* @date Jun 6, 2012
* @brief Linear Algebra Operations for a JacobianFactorGraph
* @author Yong Dian Jian
*/
#pragma once
#include <gtsam/linear/Errors.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/shared_ptr.hpp>
namespace gtsam {
typedef FactorGraph<JacobianFactor> JacobianFactorGraph;
/** return A*x */
Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x);
/** In-place version e <- A*x that overwrites e. */
void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e);
/** In-place version e <- A*x that takes an iterator. */
void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e);
/** x += alpha*A'*e */
void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around \f$ x = x_0 \f$.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @param x0 The center about which to compute the gradient
* @return The gradient as a VectorValues
*/
VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0);
/**
* Compute the gradient of the energy function,
* \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
* centered around zero.
* The gradient is \f$ A^T(Ax-b) \f$.
* @param fg The Jacobian factor graph $(A,b)$
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues
*/
void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g);
/* matrix-vector operations */
void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r);
void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r);
void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x);
/** dynamic_cast the gaussian factors down to jacobian factors, may throw exception if it contains non-Jacobian Factor */
JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg);
/** convert the gaussian factors down to jacobian factors, may duplicate factors if it contains Hessian Factor */
JacobianFactorGraph::shared_ptr convertToJacobianFactorGraph(const GaussianFactorGraph &gfg);
}

2
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -17,7 +17,7 @@
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/foreach.hpp>

3
gtsam/linear/SubgraphPreconditioner.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
namespace gtsam {
@ -35,7 +36,7 @@ namespace gtsam {
typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet;
typedef boost::shared_ptr<const FactorGraph<JacobianFactor> > sharedFG;
typedef boost::shared_ptr<const GaussianFactorGraph> sharedFG;
typedef boost::shared_ptr<const VectorValues> sharedValues;
typedef boost::shared_ptr<const Errors> sharedErrors;

46
gtsam/linear/SubgraphSolver.cpp
View File

@ -14,7 +14,7 @@
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph-inl.h>
@ -31,12 +31,11 @@ namespace gtsam {
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters)
: parameters_(parameters)
{
JacobianFactorGraph::shared_ptr jfg = dynamicCastFactors(gfg);
initialize(*jfg);
initialize(gfg);
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &jfg, const Parameters &parameters)
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters &parameters)
: parameters_(parameters)
{
initialize(*jfg);
@ -47,13 +46,12 @@ SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFac
: parameters_(parameters) {
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(Ab1)->eliminate(&EliminateQR);
JacobianFactorGraph::shared_ptr Ab2Jacobian = dynamicCastFactors(Ab2);
initialize(Rc1, Ab2Jacobian);
initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1,
const JacobianFactorGraph::shared_ptr &Ab2, const Parameters &parameters)
SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1,
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters)
: parameters_(parameters) {
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
@ -64,13 +62,12 @@ SubgraphSolver::SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1,
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2,
const Parameters &parameters) : parameters_(parameters)
{
JacobianFactorGraph::shared_ptr Ab2Jacobians = dynamicCastFactors(Ab2);
initialize(Rc1, Ab2Jacobians);
initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
}
/**************************************************************************************************/
SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
const JacobianFactorGraph::shared_ptr &Ab2, const Parameters &parameters) : parameters_(parameters)
const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters) : parameters_(parameters)
{
initialize(Rc1, Ab2);
}
@ -80,10 +77,10 @@ VectorValues SubgraphSolver::optimize() {
return pc_->x(ybar);
}
void SubgraphSolver::initialize(const JacobianFactorGraph &jfg)
void SubgraphSolver::initialize(const GaussianFactorGraph &jfg)
{
JacobianFactorGraph::shared_ptr Ab1 = boost::make_shared<JacobianFactorGraph>(),
Ab2 = boost::make_shared<JacobianFactorGraph>();
GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>(),
Ab2 = boost::make_shared<GaussianFactorGraph>();
boost::tie(Ab1, Ab2) = splitGraph(jfg) ;
if (parameters_.verbosity())
@ -91,28 +88,33 @@ void SubgraphSolver::initialize(const JacobianFactorGraph &jfg)
GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
JacobianFactorGraph::shared_ptr Ab2Jacobians = convertToJacobianFactorGraph(*Ab2);
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2Jacobians, Rc1, xbar);
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2)
void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2)
{
VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
}
boost::tuple<JacobianFactorGraph::shared_ptr, JacobianFactorGraph::shared_ptr>
SubgraphSolver::splitGraph(const JacobianFactorGraph &jfg) {
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
const VariableIndex index(jfg);
const size_t n = index.size(), m = jfg.size();
DisjointSet D(n) ;
JacobianFactorGraph::shared_ptr At(new JacobianFactorGraph());
JacobianFactorGraph::shared_ptr Ac( new JacobianFactorGraph());
GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph());
size_t t = 0;
BOOST_FOREACH ( const JacobianFactor::shared_ptr &jf, jfg ) {
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) {
JacobianFactor::shared_ptr jf;
if(JacobianFactor::shared_ptr Ai_J = boost::dynamic_pointer_cast<JacobianFactor>(gf))
jf = Ai_J;
else
jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
if ( jf->keys().size() > 2 ) {
throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");

16
gtsam/linear/SubgraphSolver.h
View File

@ -13,7 +13,7 @@
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianBayesNet.h>
@ -62,26 +62,26 @@ protected:
public:
/* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */
SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters);
SubgraphSolver(const JacobianFactorGraph::shared_ptr &A, const Parameters &parameters);
SubgraphSolver(const GaussianFactorGraph::shared_ptr &A, const Parameters &parameters);
/* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */
SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters);
SubgraphSolver(const JacobianFactorGraph::shared_ptr &Ab1, const JacobianFactorGraph::shared_ptr &Ab2, const Parameters &parameters);
SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters);
/* The same as above, but the A1 is solved before */
SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters &parameters);
SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2, const Parameters &parameters);
SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters);
virtual ~SubgraphSolver() {}
virtual VectorValues optimize () ;
protected:
void initialize(const JacobianFactorGraph &jfg);
void initialize(const GaussianBayesNet::shared_ptr &Rc1, const JacobianFactorGraph::shared_ptr &Ab2);
void initialize(const GaussianFactorGraph &jfg);
void initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2);
boost::tuple<JacobianFactorGraph::shared_ptr, JacobianFactorGraph::shared_ptr>
splitGraph(const JacobianFactorGraph &gfg) ;
boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
splitGraph(const GaussianFactorGraph &gfg) ;
public:

10
gtsam/linear/iterative.cpp
View File

@ -19,7 +19,7 @@
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeSolver.h>
#include <iostream>
@ -61,15 +61,15 @@ namespace gtsam {
}
/* ************************************************************************* */
VectorValues steepestDescent(const FactorGraph<JacobianFactor>& fg,
VectorValues steepestDescent(const GaussianFactorGraph& fg,
const VectorValues& x, const ConjugateGradientParameters & parameters) {
return conjugateGradients<FactorGraph<JacobianFactor>, VectorValues, Errors>(
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors>(
fg, x, parameters, true);
}
VectorValues conjugateGradientDescent(const FactorGraph<JacobianFactor>& fg,
VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg,
const VectorValues& x, const ConjugateGradientParameters & parameters) {
return conjugateGradients<FactorGraph<JacobianFactor>, VectorValues, Errors>(
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors>(
fg, x, parameters);
}

4
gtsam/linear/iterative.h
View File

@ -130,7 +130,7 @@ namespace gtsam {
* Method of steepest gradients, Gaussian Factor Graph version
*/
VectorValues steepestDescent(
const FactorGraph<JacobianFactor>& fg,
const GaussianFactorGraph& fg,
const VectorValues& x,
const ConjugateGradientParameters & parameters);
@ -138,7 +138,7 @@ namespace gtsam {
* Method of conjugate gradients (CG), Gaussian Factor Graph version
*/
VectorValues conjugateGradientDescent(
const FactorGraph<JacobianFactor>& fg,
const GaussianFactorGraph& fg,
const VectorValues& x,
const ConjugateGradientParameters & parameters);

2
gtsam/linear/tests/testJacobianFactorGraph.cpp
View File

@ -11,7 +11,7 @@
/**
* @file testJacobianFactorGraph.cpp
* @brief Unit tests for JacobianFactorGraph
* @brief Unit tests for GaussianFactorGraph
* @author Yong Dian Jian
**/

2
gtsam/nonlinear/ISAM2.cpp
View File

@ -27,7 +27,7 @@ using namespace boost::assign;
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>

2
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp
View File

@ -8,7 +8,7 @@
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <cmath>

26
tests/smallExample.cpp
View File

@ -138,9 +138,9 @@ namespace example {
}
/* ************************************************************************* */
JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
FactorGraph<JacobianFactor> createGaussianFactorGraph(const Ordering& ordering) {
// Create empty graph
JacobianFactorGraph fg;
FactorGraph<JacobianFactor> fg;
SharedDiagonal unit2 = noiseModel::Unit::Create(2);
@ -273,7 +273,7 @@ namespace example {
}
/* ************************************************************************* */
JacobianFactorGraph createSimpleConstraintGraph() {
GaussianFactorGraph createSimpleConstraintGraph() {
// create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2);
@ -293,7 +293,7 @@ namespace example {
constraintModel));
// construct the graph
JacobianFactorGraph fg;
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
@ -310,7 +310,7 @@ namespace example {
}
/* ************************************************************************* */
JacobianFactorGraph createSingleConstraintGraph() {
GaussianFactorGraph createSingleConstraintGraph() {
// create unary factor
// prior on _x_, mean = [1,-1], sigma=0.1
Matrix Ax = eye(2);
@ -335,7 +335,7 @@ namespace example {
constraintModel));
// construct the graph
JacobianFactorGraph fg;
GaussianFactorGraph fg;
fg.push_back(f1);
fg.push_back(f2);
@ -351,7 +351,7 @@ namespace example {
}
/* ************************************************************************* */
JacobianFactorGraph createMultiConstraintGraph() {
GaussianFactorGraph createMultiConstraintGraph() {
// unary factor 1
Matrix A = eye(2);
Vector b = Vector_(2, -2.0, 2.0);
@ -396,7 +396,7 @@ namespace example {
constraintModel));
// construct the graph
JacobianFactorGraph fg;
GaussianFactorGraph fg;
fg.push_back(lf1);
fg.push_back(lc1);
fg.push_back(lc2);
@ -421,7 +421,7 @@ namespace example {
}
/* ************************************************************************* */
boost::tuple<JacobianFactorGraph, VectorValues> planarGraph(size_t N) {
boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
// create empty graph
NonlinearFactorGraph nlfg;
@ -460,7 +460,7 @@ namespace example {
// linearize around zero
boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros, ordering);
JacobianFactorGraph jfg;
GaussianFactorGraph jfg;
BOOST_FOREACH(GaussianFactorGraph::sharedFactor factor, *gfg)
jfg.push_back(boost::dynamic_pointer_cast<JacobianFactor>(factor));
@ -477,9 +477,9 @@ namespace example {
}
/* ************************************************************************* */
pair<JacobianFactorGraph, JacobianFactorGraph > splitOffPlanarTree(size_t N,
const JacobianFactorGraph& original) {
JacobianFactorGraph T, C;
pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N,
const GaussianFactorGraph& original) {
GaussianFactorGraph T, C;
// Add the x11 constraint to the tree
T.push_back(boost::dynamic_pointer_cast<JacobianFactor>(original[0]));

15
tests/smallExample.h
View File

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

6
tests/testGaussianFactor.cpp
View File

@ -72,7 +72,7 @@ TEST( GaussianFactor, getDim )
const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
// get a factor
Ordering ordering; ordering += kx1,kx2,kl1;
GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);
FactorGraph<JacobianFactor> fg = example::createGaussianFactorGraph(ordering);
GaussianFactor::shared_ptr factor = fg[0];
// get the size of a variable
@ -89,7 +89,7 @@ TEST( GaussianFactor, error )
const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
// create a small linear factor graph
Ordering ordering; ordering += kx1,kx2,kl1;
GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);
FactorGraph<JacobianFactor> fg = example::createGaussianFactorGraph(ordering);
// get the first factor from the factor graph
GaussianFactor::shared_ptr lf = fg[0];
@ -210,7 +210,7 @@ TEST( GaussianFactor, size )
// create a linear factor graph
const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
Ordering ordering; ordering += kx1,kx2,kl1;
GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);
FactorGraph<JacobianFactor> fg = example::createGaussianFactorGraph(ordering);
// get some factors from the graph
boost::shared_ptr<GaussianFactor> factor1 = fg[0];

2
tests/testGaussianFactorGraphB.cpp
View File

@ -19,7 +19,7 @@
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Matrix.h>

2
tests/testGaussianISAM2.cpp
View File

@ -12,7 +12,7 @@
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

2
tests/testIterative.cpp
View File

@ -40,7 +40,7 @@ TEST( Iterative, steepestDescent )
// Create factor graph
Ordering ord;
ord += L(1), X(1), X(2);
JacobianFactorGraph fg = createGaussianFactorGraph(ord);
FactorGraph<JacobianFactor> fg = createGaussianFactorGraph(ord);
// eliminate and solve
VectorValues expected = *GaussianSequentialSolver(fg).optimize();

28
tests/testSubgraphPreconditioner.cpp
View File

@ -19,7 +19,7 @@
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/iterative.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/base/numericalDerivative.h>
@ -54,9 +54,9 @@ TEST( SubgraphPreconditioner, planarOrdering ) {
/* ************************************************************************* */
/** unnormalized error */
static double error(const JacobianFactorGraph& fg, const VectorValues& x) {
static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
double total_error = 0.;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg)
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg)
total_error += factor->error(x);
return total_error;
}
@ -65,7 +65,7 @@ static double error(const JacobianFactorGraph& fg, const VectorValues& x) {
TEST( SubgraphPreconditioner, planarGraph )
{
// Check planar graph construction
JacobianFactorGraph A;
GaussianFactorGraph A;
VectorValues xtrue;
boost::tie(A, xtrue) = planarGraph(3);
LONGS_EQUAL(13,A.size());
@ -82,12 +82,12 @@ TEST( SubgraphPreconditioner, planarGraph )
TEST( SubgraphPreconditioner, splitOffPlanarTree )
{
// Build a planar graph
JacobianFactorGraph A;
GaussianFactorGraph A;
VectorValues xtrue;
boost::tie(A, xtrue) = planarGraph(3);
// Get the spanning tree and constraints, and check their sizes
JacobianFactorGraph T, C;
GaussianFactorGraph T, C;
boost::tie(T, C) = splitOffPlanarTree(3, A);
LONGS_EQUAL(9,T.size());
LONGS_EQUAL(4,C.size());
@ -103,16 +103,16 @@ TEST( SubgraphPreconditioner, splitOffPlanarTree )
TEST( SubgraphPreconditioner, system )
{
// Build a planar graph
JacobianFactorGraph Ab;
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
SubgraphPreconditioner::sharedFG Ab1(new JacobianFactorGraph(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new JacobianFactorGraph(Ab2_));
SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
// Eliminate the spanning tree to build a prior
SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
@ -179,16 +179,16 @@ TEST( SubgraphPreconditioner, system )
TEST( SubgraphPreconditioner, conjugateGradients )
{
// Build a planar graph
JacobianFactorGraph Ab;
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
SubgraphPreconditioner::sharedFG Ab1(new JacobianFactorGraph(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new JacobianFactorGraph(Ab2_));
SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
// Eliminate the spanning tree to build a prior
Ordering ordering = planarOrdering(N);

16
tests/testSubgraphSolver.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/iterative.h>
#include <gtsam/linear/JacobianFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/base/numericalDerivative.h>
@ -38,9 +38,9 @@ using namespace example;
/* ************************************************************************* */
/** unnormalized error */
static double error(const JacobianFactorGraph& fg, const VectorValues& x) {
static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
double total_error = 0.;
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg)
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg)
total_error += factor->error(x);
return total_error;
}
@ -50,7 +50,7 @@ static double error(const JacobianFactorGraph& fg, const VectorValues& x) {
TEST( SubgraphSolver, constructor1 )
{
// Build a planar graph
JacobianFactorGraph Ab;
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
@ -67,13 +67,13 @@ TEST( SubgraphSolver, constructor1 )
TEST( SubgraphSolver, constructor2 )
{
// Build a planar graph
JacobianFactorGraph Ab;
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
// The second constructor takes two factor graphs,
@ -88,13 +88,13 @@ TEST( SubgraphSolver, constructor2 )
TEST( SubgraphSolver, constructor3 )
{
// Build a planar graph
JacobianFactorGraph Ab;
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
JacobianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
// The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT