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

Global find+replace to remove Ordered

release/4.3a0
Richard Roberts 2013-08-05 22:31:44 +00:00
parent cb7eb1b510
commit ede0805fac
77 changed files with 1160 additions and 1155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
examples/UnorderedLinear.cpp
View File

@ -129,15 +129,15 @@ ISAM2 solveWithOldISAM2(const NonlinearFactorGraph& measurements)
}
/* ************************************************************************* */
GaussianFactorGraph convertToUnordered(const GaussianFactorGraphOrdered& gfg, const OrderingOrdered& ordering)
GaussianFactorGraph convertToUnordered(const GaussianFactorGraph& gfg, const Ordering& ordering)
{
GaussianFactorGraph gfgu;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, gfg)
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg)
{
vector<std::pair<Key, Matrix> > terms;
const JacobianFactorOrdered& jacobian = dynamic_cast<const JacobianFactorOrdered&>(*factor);
for(GaussianFactorOrdered::const_iterator term = jacobian.begin(); term != jacobian.end(); ++term)
const JacobianFactor& jacobian = dynamic_cast<const JacobianFactor&>(*factor);
for(GaussianFactor::const_iterator term = jacobian.begin(); term != jacobian.end(); ++term)
{
terms.push_back(make_pair(
ordering.key(*term),
@ -150,7 +150,7 @@ GaussianFactorGraph convertToUnordered(const GaussianFactorGraphOrdered& gfg, co
}
/* ************************************************************************* */
void compareSolutions(const VectorValuesOrdered& orderedSoln, const OrderingOrdered& ordering, const VectorValues& unorderedSoln)
void compareSolutions(const VectorValues& orderedSoln, const Ordering& ordering, const VectorValues& unorderedSoln)
{
if(orderedSoln.size() != unorderedSoln.size())
{
@ -217,9 +217,9 @@ int main(int argc, char *argv[])
// Get linear graph
cout << "Converting to unordered linear graph" << endl;
OrderingOrdered ordering = *isamsoln.orderingArbitrary();
OrderingOrdered orderingCOLAMD = *nlfg.orderingCOLAMD(isamsoln);
GaussianFactorGraphOrdered gfg = *nlfg.linearize(isamsoln, ordering);
Ordering ordering = *isamsoln.orderingArbitrary();
Ordering orderingCOLAMD = *nlfg.orderingCOLAMD(isamsoln);
GaussianFactorGraph gfg = *nlfg.linearize(isamsoln, ordering);
GaussianFactorGraph gfgu = convertToUnordered(gfg, ordering);
//Ordering orderingUnordered;
@ -233,9 +233,9 @@ int main(int argc, char *argv[])
gttic_(Solve_unordered);
VectorValues unorderedSoln;
for(size_t i = 0; i < 1; ++i) {
gttic_(VariableIndexOrdered);
gttic_(VariableIndex);
VariableIndex vi(gfgu);
gttoc_(VariableIndexOrdered);
gttoc_(VariableIndex);
gttic_(COLAMD);
Ordering orderingUnordered = Ordering::COLAMD(vi);
gttoc_(COLAMD);
@ -252,22 +252,22 @@ int main(int argc, char *argv[])
// Solve linear graph with old code
cout << "Optimizing using old ordered code" << endl;
VectorValuesOrdered orderedSolnFinal;
VectorValues orderedSolnFinal;
{
OrderingOrdered orderingToUse = ordering;
GaussianFactorGraphOrdered::shared_ptr orderedGraph = nlfg.linearize(isamsoln, *nlfg.orderingCOLAMD(isamsoln));
Ordering orderingToUse = ordering;
GaussianFactorGraph::shared_ptr orderedGraph = nlfg.linearize(isamsoln, *nlfg.orderingCOLAMD(isamsoln));
gttic_(Solve_ordered);
VectorValuesOrdered orderedSoln;
VectorValues orderedSoln;
for(size_t i = 0; i < 1; ++i) {
gttic_(VariableIndexOrdered);
boost::shared_ptr<VariableIndexOrdered> vi = boost::make_shared<VariableIndexOrdered>(gfg);
gttoc_(VariableIndexOrdered);
gttic_(VariableIndex);
boost::shared_ptr<VariableIndex> vi = boost::make_shared<VariableIndex>(gfg);
gttoc_(VariableIndex);
gttic_(COLAMD);
boost::shared_ptr<Permutation> permutation = inference::PermutationCOLAMD(*vi);
orderingToUse.permuteInPlace(*permutation);
gttoc_(COLAMD);
gttic_(eliminate);
boost::shared_ptr<GaussianBayesTreeOrdered> bt = GaussianMultifrontalSolver(*orderedGraph, true).eliminate();
boost::shared_ptr<GaussianBayesTree> bt = GaussianMultifrontalSolver(*orderedGraph, true).eliminate();
gttoc_(eliminate);
gttic_(optimize);
orderedSoln = optimize(*bt);

46
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -38,10 +38,10 @@ int main() {
// [code below basically does SRIF with Cholesky]
// Create a factor graph to perform the inference
GaussianFactorGraphOrdered::shared_ptr linearFactorGraph(new GaussianFactorGraphOrdered);
GaussianFactorGraph::shared_ptr linearFactorGraph(new GaussianFactorGraph);
// Create the desired ordering
OrderingOrdered::shared_ptr ordering(new OrderingOrdered);
Ordering::shared_ptr ordering(new Ordering);
// Create a structure to hold the linearization points
Values linearizationPoints;
@ -110,10 +110,10 @@ int main() {
// Solve the linear factor graph, converting it into a linear Bayes Network ( P(x0,x1) = P(x0|x1)*P(x1) )
GaussianSequentialSolver solver0(*linearFactorGraph);
GaussianBayesNetOrdered::shared_ptr linearBayesNet = solver0.eliminate();
GaussianBayesNet::shared_ptr linearBayesNet = solver0.eliminate();
// Extract the current estimate of x1,P1 from the Bayes Network
VectorValuesOrdered result = optimize(*linearBayesNet);
VectorValues result = optimize(*linearBayesNet);
Point2 x1_predict = linearizationPoints.at<Point2>(x1).retract(result[ordering->at(x1)]);
x1_predict.print("X1 Predict");
@ -123,7 +123,7 @@ int main() {
// Create a new, empty graph and add the prior from the previous step
linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered);
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step
// Some care must be done here, as the linearization point in future steps will be different
@ -138,13 +138,13 @@ int main() {
// -> b'' = b' - F(dx1' - dx1'')
// = || F*dx1'' - (b' - F(dx1' - dx1'')) ||^2
// = || F*dx1'' - (b' - F(x_predict - x_inital)) ||^2
const GaussianConditionalOrdered::shared_ptr& cg0 = linearBayesNet->back();
const GaussianConditional::shared_ptr& cg0 = linearBayesNet->back();
assert(cg0->nrFrontals() == 1);
assert(cg0->nrParents() == 0);
linearFactorGraph->add(0, cg0->get_R(), cg0->get_d() - cg0->get_R()*result[ordering->at(x1)], noiseModel::Diagonal::Sigmas(cg0->get_sigmas(), true));
// Create the desired ordering
ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering = Ordering::shared_ptr(new Ordering);
ordering->insert(x1, 0);
// Now, a measurement, z1, has been received, and the Kalman Filter should be "Updated"/"Corrected"
@ -199,22 +199,22 @@ int main() {
// Wash, rinse, repeat for another time step
// Create a new, empty graph and add the prior from the previous step
linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered);
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step
// The linearization point of this prior must be moved to the new estimate of x, and the key/index needs to be reset to 0,
// the first key in the next iteration
const GaussianConditionalOrdered::shared_ptr& cg1 = linearBayesNet->back();
const GaussianConditional::shared_ptr& cg1 = linearBayesNet->back();
assert(cg1->nrFrontals() == 1);
assert(cg1->nrParents() == 0);
JacobianFactorOrdered tmpPrior1 = JacobianFactorOrdered(*cg1);
JacobianFactor tmpPrior1 = JacobianFactor(*cg1);
linearFactorGraph->add(0, tmpPrior1.getA(tmpPrior1.begin()), tmpPrior1.getb() - tmpPrior1.getA(tmpPrior1.begin()) * result[ordering->at(x1)], tmpPrior1.get_model());
// Create a key for the new state
Symbol x2('x',2);
// Create the desired ordering
ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering = Ordering::shared_ptr(new Ordering);
ordering->insert(x1, 0);
ordering->insert(x2, 1);
@ -243,17 +243,17 @@ int main() {
// Now add the next measurement
// Create a new, empty graph and add the prior from the previous step
linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered);
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step
const GaussianConditionalOrdered::shared_ptr& cg2 = linearBayesNet->back();
const GaussianConditional::shared_ptr& cg2 = linearBayesNet->back();
assert(cg2->nrFrontals() == 1);
assert(cg2->nrParents() == 0);
JacobianFactorOrdered tmpPrior2 = JacobianFactorOrdered(*cg2);
JacobianFactor tmpPrior2 = JacobianFactor(*cg2);
linearFactorGraph->add(0, tmpPrior2.getA(tmpPrior2.begin()), tmpPrior2.getb() - tmpPrior2.getA(tmpPrior2.begin()) * result[ordering->at(x2)], tmpPrior2.get_model());
// Create the desired ordering
ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering = Ordering::shared_ptr(new Ordering);
ordering->insert(x2, 0);
// And update using z2 ...
@ -290,20 +290,20 @@ int main() {
// Wash, rinse, repeat for a third time step
// Create a new, empty graph and add the prior from the previous step
linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered);
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step
const GaussianConditionalOrdered::shared_ptr& cg3 = linearBayesNet->back();
const GaussianConditional::shared_ptr& cg3 = linearBayesNet->back();
assert(cg3->nrFrontals() == 1);
assert(cg3->nrParents() == 0);
JacobianFactorOrdered tmpPrior3 = JacobianFactorOrdered(*cg3);
JacobianFactor tmpPrior3 = JacobianFactor(*cg3);
linearFactorGraph->add(0, tmpPrior3.getA(tmpPrior3.begin()), tmpPrior3.getb() - tmpPrior3.getA(tmpPrior3.begin()) * result[ordering->at(x2)], tmpPrior3.get_model());
// Create a key for the new state
Symbol x3('x',3);
// Create the desired ordering
ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering = Ordering::shared_ptr(new Ordering);
ordering->insert(x2, 0);
ordering->insert(x3, 1);
@ -332,17 +332,17 @@ int main() {
// Now add the next measurement
// Create a new, empty graph and add the prior from the previous step
linearFactorGraph = GaussianFactorGraphOrdered::shared_ptr(new GaussianFactorGraphOrdered);
linearFactorGraph = GaussianFactorGraph::shared_ptr(new GaussianFactorGraph);
// Convert the root conditional, P(x1) in this case, into a Prior for the next step
const GaussianConditionalOrdered::shared_ptr& cg4 = linearBayesNet->back();
const GaussianConditional::shared_ptr& cg4 = linearBayesNet->back();
assert(cg4->nrFrontals() == 1);
assert(cg4->nrParents() == 0);
JacobianFactorOrdered tmpPrior4 = JacobianFactorOrdered(*cg4);
JacobianFactor tmpPrior4 = JacobianFactor(*cg4);
linearFactorGraph->add(0, tmpPrior4.getA(tmpPrior4.begin()), tmpPrior4.getb() - tmpPrior4.getA(tmpPrior4.begin()) * result[ordering->at(x3)], tmpPrior4.get_model());
// Create the desired ordering
ordering = OrderingOrdered::shared_ptr(new OrderingOrdered);
ordering = Ordering::shared_ptr(new Ordering);
ordering->insert(x3, 0);
// And update using z3 ...

2
gtsam/linear/GaussianBayesTree-inl.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
/* ************************************************************************* */
namespace internal {
template<class BAYESTREE>
void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValuesOrdered& result) {
void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result) {
// parents are assumed to already be solved and available in result
clique->conditional()->solveInPlace(result);

2
gtsam/linear/GaussianBayesTreeOrdered-inl.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
/* ************************************************************************* */
namespace internal {
template<class BAYESTREE>
void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValuesOrdered& result) {
void optimizeInPlace(const typename BAYESTREE::sharedClique& clique, VectorValues& result) {
// parents are assumed to already be solved and available in result
clique->conditional()->solveInPlace(result);

4
gtsam/linear/GaussianConditional-inl.h
View File

@ -67,8 +67,8 @@ namespace gtsam {
// Allocate combined conditional, has same keys as firstConditional
Matrix tempCombined;
VerticalBlockView<Matrix> tempBlockView(tempCombined, dims.begin(), dims.end(), 0);
GaussianConditionalOrdered::shared_ptr combinedConditional =
boost::make_shared<GaussianConditionalOrdered>(
GaussianConditional::shared_ptr combinedConditional =
boost::make_shared<GaussianConditional>(
(*firstConditional)->begin(), (*firstConditional)->end(), nFrontals, tempBlockView, Vector::Zero(nRows));
// Resize to correct number of rows

24
gtsam/linear/tests/timeFactorOverhead.cpp
View File

@ -18,7 +18,7 @@
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/EliminationTreeOrdered-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <boost/random.hpp>
#include <boost/timer.hpp>
@ -27,7 +27,7 @@
using namespace gtsam;
using namespace std;
typedef EliminationTreeOrdered<GaussianFactorOrdered> GaussianEliminationTree;
typedef EliminationTree<GaussianFactor> GaussianEliminationTree;
static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0));
@ -56,10 +56,10 @@ int main(int argc, char *argv[]) {
cout.flush();
boost::timer timer;
timer.restart();
vector<GaussianFactorGraphOrdered> blockGfgs;
vector<GaussianFactorGraph> blockGfgs;
blockGfgs.reserve(nTrials);
for(size_t trial=0; trial<nTrials; ++trial) {
blockGfgs.push_back(GaussianFactorGraphOrdered());
blockGfgs.push_back(GaussianFactorGraph());
SharedDiagonal noise = noiseModel::Isotropic::Sigma(blockdim, 1.0);
for(size_t i=0; i<nBlocks; ++i) {
// Generate a random Gaussian factor
@ -70,7 +70,7 @@ int main(int argc, char *argv[]) {
Vector b(blockdim);
for(size_t j=0; j<blockdim; ++j)
b(j) = rg();
blockGfgs[trial].push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(key, A, b, noise)));
blockGfgs[trial].push_back(JacobianFactor::shared_ptr(new JacobianFactor(key, A, b, noise)));
}
}
blockbuild = timer.elapsed();
@ -82,8 +82,8 @@ int main(int argc, char *argv[]) {
timer.restart();
for(size_t trial=0; trial<nTrials; ++trial) {
// cout << "Trial " << trial << endl;
GaussianBayesNetOrdered::shared_ptr gbn(GaussianEliminationTree::Create(blockGfgs[trial])->eliminate(&EliminateQROrdered));
VectorValuesOrdered soln(optimize(*gbn));
GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(blockGfgs[trial])->eliminate(&EliminateQR));
VectorValues soln(optimize(*gbn));
}
blocksolve = timer.elapsed();
cout << blocksolve << " s" << endl;
@ -99,9 +99,9 @@ int main(int argc, char *argv[]) {
cout.flush();
boost::timer timer;
timer.restart();
vector<GaussianFactorGraphOrdered> combGfgs;
vector<GaussianFactorGraph> combGfgs;
for(size_t trial=0; trial<nTrials; ++trial) {
combGfgs.push_back(GaussianFactorGraphOrdered());
combGfgs.push_back(GaussianFactorGraph());
SharedDiagonal noise = noiseModel::Isotropic::Sigma(blockdim, 1.0);
Matrix Acomb(blockdim*nBlocks, vardim);
@ -115,7 +115,7 @@ int main(int argc, char *argv[]) {
for(size_t j=0; j<blockdim; ++j)
bcomb(blockdim*i+j) = rg();
}
combGfgs[trial].push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(key, Acomb, bcomb,
combGfgs[trial].push_back(JacobianFactor::shared_ptr(new JacobianFactor(key, Acomb, bcomb,
noiseModel::Isotropic::Sigma(blockdim*nBlocks, 1.0))));
}
combbuild = timer.elapsed();
@ -126,8 +126,8 @@ int main(int argc, char *argv[]) {
cout.flush();
timer.restart();
for(size_t trial=0; trial<nTrials; ++trial) {
GaussianBayesNetOrdered::shared_ptr gbn(GaussianEliminationTree::Create(combGfgs[trial])->eliminate(&EliminateQROrdered));
VectorValuesOrdered soln(optimize(*gbn));
GaussianBayesNet::shared_ptr gbn(GaussianEliminationTree::Create(combGfgs[trial])->eliminate(&EliminateQR));
VectorValues soln(optimize(*gbn));
}
combsolve = timer.elapsed();
cout << combsolve << " s" << endl;

8
gtsam/linear/tests/timeGaussianFactor.cpp
View File

@ -101,14 +101,14 @@ int main()
b2(7) = -1;
// time eliminate
JacobianFactorOrdered combined(_x2_, Ax2, _l1_, Al1, _x1_, Ax1, b2, noiseModel::Isotropic::Sigma(8,1));
JacobianFactor combined(_x2_, Ax2, _l1_, Al1, _x1_, Ax1, b2, noiseModel::Isotropic::Sigma(8,1));
long timeLog = clock();
int n = 1000000;
GaussianConditionalOrdered::shared_ptr conditional;
JacobianFactorOrdered::shared_ptr factor;
GaussianConditional::shared_ptr conditional;
JacobianFactor::shared_ptr factor;
for(int i = 0; i < n; i++)
conditional = JacobianFactorOrdered(combined).eliminateFirst();
conditional = JacobianFactor(combined).eliminateFirst();
long timeLog2 = clock();
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;

10
gtsam/linear/tests/timeSLAMlike.cpp
View File

@ -31,7 +31,7 @@ using namespace gtsam;
using namespace std;
using namespace boost::lambda;
typedef EliminationTreeOrdered<JacobianFactorOrdered> GaussianEliminationTree;
typedef EliminationTree<JacobianFactor> GaussianEliminationTree;
static boost::variate_generator<boost::mt19937, boost::uniform_real<> > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0));
@ -65,10 +65,10 @@ int main(int argc, char *argv[]) {
cout.flush();
boost::timer timer;
timer.restart();
vector<GaussianFactorGraphOrdered> blockGfgs;
vector<GaussianFactorGraph> blockGfgs;
blockGfgs.reserve(nTrials);
for(size_t trial=0; trial<nTrials; ++trial) {
blockGfgs.push_back(GaussianFactorGraphOrdered());
blockGfgs.push_back(GaussianFactorGraph());
SharedDiagonal noise = noiseModel::Isotropic::Sigma(blockdim, 1.0);
for(int c=0; c<nVars; ++c) {
for(size_t d=0; d<blocksPerVar; ++d) {
@ -103,7 +103,7 @@ int main(int argc, char *argv[]) {
for(size_t j=0; j<blockdim; ++j)
b(j) = rg();
if(!terms.empty())
blockGfgs[trial].push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(terms, b, noise)));
blockGfgs[trial].push_back(JacobianFactor::shared_ptr(new JacobianFactor(terms, b, noise)));
}
}
@ -119,7 +119,7 @@ int main(int argc, char *argv[]) {
timer.restart();
for(size_t trial=0; trial<nTrials; ++trial) {
// cout << "Trial " << trial << endl;
VectorValuesOrdered soln(*GaussianMultifrontalSolver(blockGfgs[trial]).optimize());
VectorValues soln(*GaussianMultifrontalSolver(blockGfgs[trial]).optimize());
}
blocksolve = timer.elapsed();
cout << blocksolve << " s" << endl;

2
gtsam/nonlinear/DoglegOptimizer.h
View File

@ -116,7 +116,7 @@ public:
* @param graph The nonlinear factor graph to optimize
* @param initialValues The initial variable assignments
*/
DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const OrderingOrdered& ordering) :
DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues, const Ordering& ordering) :
NonlinearOptimizer(graph) {
params_.ordering = ordering;
state_ = DoglegState(graph, initialValues, params_); }

6
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -148,13 +148,13 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
{
// Compute steepest descent and Newton's method points
gttic(optimizeGradientSearch);
VectorValuesOrdered dx_u = Rd.optimizeGradientSearch();
VectorValues dx_u = Rd.optimizeGradientSearch();
gttoc(optimizeGradientSearch);
gttic(optimize);
VectorValuesOrdered dx_n = Rd.optimize();
VectorValues dx_n = Rd.optimize();
gttoc(optimize);
gttic(M_error);
const double M_error = Rd.error(VectorValuesOrdered::Zero(dx_u));
const double M_error = Rd.error(VectorValues::Zero(dx_u));
gttoc(M_error);
// Result to return

28
gtsam/nonlinear/ExtendedKalmanFilter-inl.h
View File

@ -29,9 +29,9 @@ namespace gtsam {
/* ************************************************************************* */
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_(
const GaussianFactorGraphOrdered& linearFactorGraph, const OrderingOrdered& ordering,
const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
const Values& linearizationPoints, Key lastKey,
JacobianFactorOrdered::shared_ptr& newPrior) const {
JacobianFactor::shared_ptr& newPrior) const {
// Extract the Index of the provided last key
gtsam::Index lastIndex = ordering.at(lastKey);
@ -39,23 +39,23 @@ namespace gtsam {
// Solve the linear factor graph, converting it into a linear Bayes Network
// P(x0,x1) = P(x0|x1)*P(x1)
GaussianSequentialSolver solver(linearFactorGraph);
GaussianBayesNetOrdered::shared_ptr linearBayesNet = solver.eliminate();
GaussianBayesNet::shared_ptr linearBayesNet = solver.eliminate();
// Extract the current estimate of x1,P1 from the Bayes Network
VectorValuesOrdered result = optimize(*linearBayesNet);
VectorValues result = optimize(*linearBayesNet);
T x = linearizationPoints.at<T>(lastKey).retract(result[lastIndex]);
// Create a Jacobian Factor from the root node of the produced Bayes Net.
// This will act as a prior for the next iteration.
// The linearization point of this prior must be moved to the new estimate of x,
// and the key/index needs to be reset to 0, the first key in the next iteration.
const GaussianConditionalOrdered::shared_ptr& cg = linearBayesNet->back();
const GaussianConditional::shared_ptr& cg = linearBayesNet->back();
assert(cg->nrFrontals() == 1);
assert(cg->nrParents() == 0);
// TODO: Find a way to create the correct Jacobian Factor in a single pass
JacobianFactorOrdered tmpPrior = JacobianFactorOrdered(*cg);
newPrior = JacobianFactorOrdered::shared_ptr(
new JacobianFactorOrdered(
JacobianFactor tmpPrior = JacobianFactor(*cg);
newPrior = JacobianFactor::shared_ptr(
new JacobianFactor(
0,
tmpPrior.getA(tmpPrior.begin()),
tmpPrior.getb()
@ -76,8 +76,8 @@ namespace gtsam {
// Create a Jacobian Prior Factor directly P_initial.
// Since x0 is set to the provided mean, the b vector in the prior will be zero
// TODO Frank asks: is there a reason why noiseModel is not simply P_initial ?
priorFactor_ = JacobianFactorOrdered::shared_ptr(
new JacobianFactorOrdered(0, P_initial->R(), Vector::Zero(x_initial.dim()),
priorFactor_ = JacobianFactor::shared_ptr(
new JacobianFactor(0, P_initial->R(), Vector::Zero(x_initial.dim()),
noiseModel::Unit::Create(P_initial->dim())));
}
@ -95,7 +95,7 @@ namespace gtsam {
Key x1 = motionFactor.key2();
// Create an elimination ordering
OrderingOrdered ordering;
Ordering ordering;
ordering.insert(x0, 0);
ordering.insert(x1, 1);
@ -105,7 +105,7 @@ namespace gtsam {
linearizationPoints.insert(x1, x_); // TODO should this really be x_ ?
// Create a Gaussian Factor Graph
GaussianFactorGraphOrdered linearFactorGraph;
GaussianFactorGraph linearFactorGraph;
// Add in previous posterior as prior on the first state
linearFactorGraph.push_back(priorFactor_);
@ -134,7 +134,7 @@ namespace gtsam {
Key x0 = measurementFactor.key();
// Create an elimination ordering
OrderingOrdered ordering;
Ordering ordering;
ordering.insert(x0, 0);
// Create a set of linearization points
@ -142,7 +142,7 @@ namespace gtsam {
linearizationPoints.insert(x0, x_);
// Create a Gaussian Factor Graph
GaussianFactorGraphOrdered linearFactorGraph;
GaussianFactorGraph linearFactorGraph;
// Add in the prior on the first state
linearFactorGraph.push_back(priorFactor_);

8
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -51,11 +51,11 @@ namespace gtsam {
protected:
T x_; // linearization point
JacobianFactorOrdered::shared_ptr priorFactor_; // density
JacobianFactor::shared_ptr priorFactor_; // density
T solve_(const GaussianFactorGraphOrdered& linearFactorGraph,
const OrderingOrdered& ordering, const Values& linearizationPoints,
Key x, JacobianFactorOrdered::shared_ptr& newPrior) const;
T solve_(const GaussianFactorGraph& linearFactorGraph,
const Ordering& ordering, const Values& linearizationPoints,
Key x, JacobianFactor::shared_ptr& newPrior) const;
public:

64
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -31,9 +31,9 @@ namespace gtsam {
/* ************************************************************************* */
void ISAM2::Impl::AddVariables(
const Values& newTheta, Values& theta, VectorValuesOrdered& delta,
VectorValuesOrdered& deltaNewton, VectorValuesOrdered& RgProd, vector<bool>& replacedKeys,
OrderingOrdered& ordering, const KeyFormatter& keyFormatter) {
const Values& newTheta, Values& theta, VectorValues& delta,
VectorValues& deltaNewton, VectorValues& RgProd, vector<bool>& replacedKeys,
Ordering& ordering, const KeyFormatter& keyFormatter) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
theta.insert(newTheta);
@ -64,10 +64,10 @@ void ISAM2::Impl::AddVariables(
/* ************************************************************************* */
void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Clique::shared_ptr& root,
Values& theta, VariableIndexOrdered& variableIndex,
VectorValuesOrdered& delta, VectorValuesOrdered& deltaNewton, VectorValuesOrdered& RgProd,
std::vector<bool>& replacedKeys, OrderingOrdered& ordering, Base::Nodes& nodes,
GaussianFactorGraphOrdered& linearFactors, FastSet<Key>& fixedVariables) {
Values& theta, VariableIndex& variableIndex,
VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd,
std::vector<bool>& replacedKeys, Ordering& ordering, Base::Nodes& nodes,
GaussianFactorGraph& linearFactors, FastSet<Key>& fixedVariables) {
// Get indices of unused keys
vector<Index> unusedIndices; unusedIndices.reserve(unusedKeys.size());
@ -93,9 +93,9 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Cli
// Copy from the old data structures to new ones, only iterating up to
// the number of used variables, and applying the unusedToEnd permutation
// in order to remove the unused variables.
VectorValuesOrdered newDelta(dims);
VectorValuesOrdered newDeltaNewton(dims);
VectorValuesOrdered newDeltaGradSearch(dims);
VectorValues newDelta(dims);
VectorValues newDeltaNewton(dims);
VectorValues newDeltaGradSearch(dims);
std::vector<bool> newReplacedKeys(replacedKeys.size() - unusedIndices.size());
Base::Nodes newNodes(replacedKeys.size() - unusedIndices.size());
@ -118,7 +118,7 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Cli
// Reorder and remove from ordering, solution, and fixed keys
ordering.permuteInPlace(unusedToEnd);
BOOST_REVERSE_FOREACH(Key key, unusedKeys) {
OrderingOrdered::value_type removed = ordering.pop_back();
Ordering::value_type removed = ordering.pop_back();
assert(removed.first == key);
theta.erase(key);
fixedVariables.erase(key);
@ -131,7 +131,7 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Cli
}
/* ************************************************************************* */
FastSet<Index> ISAM2::Impl::IndicesFromFactors(const OrderingOrdered& ordering, const NonlinearFactorGraph& factors) {
FastSet<Index> ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors) {
FastSet<Index> indices;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) {
BOOST_FOREACH(Key key, factor->keys()) {
@ -142,7 +142,7 @@ FastSet<Index> ISAM2::Impl::IndicesFromFactors(const OrderingOrdered& ordering,
}
/* ************************************************************************* */
FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const VectorValuesOrdered& delta, const OrderingOrdered& ordering,
FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
FastSet<Index> relinKeys;
@ -156,7 +156,7 @@ FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const VectorValuesOrdered&
}
} else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>)) {
const FastMap<char,Vector>& thresholds = boost::get<FastMap<char,Vector> >(relinearizeThreshold);
BOOST_FOREACH(const OrderingOrdered::value_type& key_index, ordering) {
BOOST_FOREACH(const Ordering::value_type& key_index, ordering) {
const Vector& threshold = thresholds.find(Symbol(key_index.first).chr())->second;
Index j = key_index.second;
if(threshold.rows() != delta[j].rows())
@ -170,7 +170,7 @@ FastSet<Index> ISAM2::Impl::CheckRelinearizationFull(const VectorValuesOrdered&
}
/* ************************************************************************* */
void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double threshold, const VectorValuesOrdered& delta, const ISAM2Clique::shared_ptr& clique) {
void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double threshold, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) {
// Check the current clique for relinearization
bool relinearize = false;
@ -191,7 +191,7 @@ void CheckRelinearizationRecursiveDouble(FastSet<Index>& relinKeys, double thres
}
/* ************************************************************************* */
void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const VectorValuesOrdered& delta, const OrderingOrdered& ordering, const ISAM2Clique::shared_ptr& clique) {
void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<char,Vector>& thresholds, const VectorValues& delta, const Ordering& ordering, const ISAM2Clique::shared_ptr& clique) {
// Check the current clique for relinearization
bool relinearize = false;
@ -223,7 +223,7 @@ void CheckRelinearizationRecursiveMap(FastSet<Index>& relinKeys, const FastMap<c
}
/* ************************************************************************* */
FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValuesOrdered& delta, const OrderingOrdered& ordering,
FastSet<Index> ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
FastSet<Index> relinKeys;
@ -260,8 +260,8 @@ void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet<Index>& keys,
}
/* ************************************************************************* */
void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValuesOrdered& delta, const OrderingOrdered& ordering,
const vector<bool>& mask, boost::optional<VectorValuesOrdered&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const Ordering& ordering,
const vector<bool>& mask, boost::optional<VectorValues&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
// If debugging, invalidate if requested, otherwise do not invalidate.
// Invalidating means setting expmapped entries to Inf, to trigger assertions
// if we try to re-use them.
@ -272,7 +272,7 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValuesOrdered& delta,
assert(values.size() == ordering.size());
assert(delta.size() == ordering.size());
Values::iterator key_value;
OrderingOrdered::const_iterator key_index;
Ordering::const_iterator key_index;
for(key_value = values.begin(), key_index = ordering.begin();
key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) {
assert(key_value->key == key_index->first);
@ -297,7 +297,7 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValuesOrdered& delta,
/* ************************************************************************* */
ISAM2::Impl::PartialSolveResult
ISAM2::Impl::PartialSolve(GaussianFactorGraphOrdered& factors,
ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
const FastSet<Index>& keys, const ReorderingMode& reorderingMode, bool useQR) {
const bool debug = ISDEBUG("ISAM2 recalculate");
@ -308,7 +308,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraphOrdered& factors,
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
// Debug check that all variables involved in the factors to be re-eliminated
// are in affectedKeys, since we will use it to select a subset of variables.
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factors) {
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
BOOST_FOREACH(Index key, factor->keys()) {
assert(find(keys.begin(), keys.end(), key) != keys.end());
}
@ -330,7 +330,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraphOrdered& factors,
if(debug) factors.print("Factors to reorder/re-eliminate: ");
gttoc(select_affected_variables);
gttic(variable_index);
VariableIndexOrdered affectedFactorsIndex(factors); // Create a variable index for the factors to be re-eliminated
VariableIndex affectedFactorsIndex(factors); // Create a variable index for the factors to be re-eliminated
if(debug) affectedFactorsIndex.print("affectedFactorsIndex: ");
gttoc(variable_index);
gttic(ccolamd);
@ -371,17 +371,17 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraphOrdered& factors,
if(debug) factors.print("Colamd-ordered affected factors: ");
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
VariableIndexOrdered fromScratchIndex(factors);
VariableIndex fromScratchIndex(factors);
assert(assert_equal(fromScratchIndex, affectedFactorsIndex));
#endif
// eliminate into a Bayes net
gttic(eliminate);
JunctionTreeOrdered<GaussianFactorGraphOrdered, ISAM2::Clique> jt(factors, affectedFactorsIndex);
JunctionTree<GaussianFactorGraph, ISAM2::Clique> jt(factors, affectedFactorsIndex);
if(!useQR)
result.bayesTree = jt.eliminate(EliminatePreferCholeskyOrdered);
result.bayesTree = jt.eliminate(EliminatePreferCholesky);
else
result.bayesTree = jt.eliminate(EliminateQROrdered);
result.bayesTree = jt.eliminate(EliminateQR);
gttoc(eliminate);
gttic(permute_eliminated);
@ -400,7 +400,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraphOrdered& factors,
/* ************************************************************************* */
namespace internal {
inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique, VectorValuesOrdered& result) {
inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique, VectorValues& result) {
// parents are assumed to already be solved and available in result
clique->conditional()->solveInPlace(result);
@ -411,7 +411,7 @@ inline static void optimizeInPlace(const boost::shared_ptr<ISAM2Clique>& clique,
}
/* ************************************************************************* */
size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, VectorValuesOrdered& delta, double wildfireThreshold) {
size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, VectorValues& delta, double wildfireThreshold) {
size_t lastBacksubVariableCount;
@ -439,7 +439,7 @@ size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std:
/* ************************************************************************* */
namespace internal {
void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, const std::vector<bool>& replacedKeys,
const VectorValuesOrdered& grad, VectorValuesOrdered& deltaNewton, VectorValuesOrdered& RgProd, size_t& varsUpdated) {
const VectorValues& grad, VectorValues& deltaNewton, VectorValues& RgProd, size_t& varsUpdated) {
// Check if any frontal or separator keys were recalculated, if so, we need
// update deltas and recurse to children, but if not, we do not need to
@ -478,10 +478,10 @@ void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, const std:
/* ************************************************************************* */
size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector<bool>& replacedKeys,
VectorValuesOrdered& deltaNewton, VectorValuesOrdered& RgProd) {
VectorValues& deltaNewton, VectorValues& RgProd) {
// Get gradient
VectorValuesOrdered grad = *allocateVectorValues(isam);
VectorValues grad = *allocateVectorValues(isam);
gradientAtZero(isam, grad);
// Update variables

30
gtsam/nonlinear/ISAM2-impl.h
View File

@ -49,17 +49,17 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
static void AddVariables(const Values& newTheta, Values& theta, VectorValuesOrdered& delta,
VectorValuesOrdered& deltaNewton, VectorValuesOrdered& RgProd, std::vector<bool>& replacedKeys,
OrderingOrdered& ordering, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta,
VectorValues& deltaNewton, VectorValues& RgProd, std::vector<bool>& replacedKeys,
Ordering& ordering, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
* Remove variables from the ISAM2 system.
*/
static void RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Clique::shared_ptr& root,
Values& theta, VariableIndexOrdered& variableIndex, VectorValuesOrdered& delta, VectorValuesOrdered& deltaNewton,
VectorValuesOrdered& RgProd, std::vector<bool>& replacedKeys, OrderingOrdered& ordering, Base::Nodes& nodes,
GaussianFactorGraphOrdered& linearFactors, FastSet<Key>& fixedVariables);
Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton,
VectorValues& RgProd, std::vector<bool>& replacedKeys, Ordering& ordering, Base::Nodes& nodes,
GaussianFactorGraph& linearFactors, FastSet<Key>& fixedVariables);
/**
* Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol
@ -68,7 +68,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @param factors The factors from which to extract the variables
* @return The set of variables indices from the factors
*/
static FastSet<Index> IndicesFromFactors(const OrderingOrdered& ordering, const NonlinearFactorGraph& factors);
static FastSet<Index> IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors);
/**
* Find the set of variables to be relinearized according to relinearizeThreshold.
@ -79,7 +79,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold
*/
static FastSet<Index> CheckRelinearizationFull(const VectorValuesOrdered& delta, const OrderingOrdered& ordering,
static FastSet<Index> CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
@ -93,7 +93,7 @@ struct GTSAM_EXPORT ISAM2::Impl {
* @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold
*/
static FastSet<Index> CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValuesOrdered& delta, const OrderingOrdered& ordering,
static FastSet<Index> CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
@ -126,9 +126,9 @@ struct GTSAM_EXPORT ISAM2::Impl {
* recalculate its delta.
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
static void ExpmapMasked(Values& values, const VectorValuesOrdered& delta,
const OrderingOrdered& ordering, const std::vector<bool>& mask,
boost::optional<VectorValuesOrdered&> invalidateIfDebug = boost::none,
static void ExpmapMasked(Values& values, const VectorValues& delta,
const Ordering& ordering, const std::vector<bool>& mask,
boost::optional<VectorValues&> invalidateIfDebug = boost::none,
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
@ -145,13 +145,13 @@ struct GTSAM_EXPORT ISAM2::Impl {
* \return The eliminated BayesTree and the permutation to be applied to the
* rest of the ISAM2 data.
*/
static PartialSolveResult PartialSolve(GaussianFactorGraphOrdered& factors, const FastSet<Index>& keys,
static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet<Index>& keys,
const ReorderingMode& reorderingMode, bool useQR);
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, VectorValuesOrdered& delta, double wildfireThreshold);
static size_t UpdateDelta(const boost::shared_ptr<ISAM2Clique>& root, std::vector<bool>& replacedKeys, VectorValues& delta, double wildfireThreshold);
static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector<bool>& replacedKeys,
VectorValuesOrdered& deltaNewton, VectorValuesOrdered& RgProd);
VectorValues& deltaNewton, VectorValues& RgProd);
};

12
gtsam/nonlinear/ISAM2-inl.h
View File

@ -37,7 +37,7 @@ VALUE ISAM2::calculateEstimate(Key key) const {
namespace internal {
template<class CLIQUE>
void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
std::vector<bool>& changed, const std::vector<bool>& replaced, VectorValuesOrdered& delta, int& count) {
std::vector<bool>& changed, const std::vector<bool>& replaced, VectorValues& delta, int& count) {
// if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the
// cliques in the children need to be processed
@ -67,7 +67,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
// Temporary copy of the original values, to check how much they change
std::vector<Vector> originalValues((*clique)->nrFrontals());
GaussianConditionalOrdered::const_iterator it;
GaussianConditional::const_iterator it;
for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
originalValues[it - (*clique)->beginFrontals()] = delta[*it];
}
@ -113,7 +113,7 @@ void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
template<class CLIQUE>
bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double threshold,
std::vector<bool>& changed, const std::vector<bool>& replaced, VectorValuesOrdered& delta, int& count) {
std::vector<bool>& changed, const std::vector<bool>& replaced, VectorValues& delta, int& count) {
// if none of the variables in this clique (frontal and separator!) changed
// significantly, then by the running intersection property, none of the
// cliques in the children need to be processed
@ -143,7 +143,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
// Temporary copy of the original values, to check how much they change
std::vector<Vector> originalValues((*clique)->nrFrontals());
GaussianConditionalOrdered::const_iterator it;
GaussianConditional::const_iterator it;
for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) {
originalValues[it - (*clique)->beginFrontals()] = delta[*it];
}
@ -189,7 +189,7 @@ bool optimizeWildfireNode(const boost::shared_ptr<CLIQUE>& clique, double thresh
/* ************************************************************************* */
template<class CLIQUE>
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const std::vector<bool>& keys, VectorValuesOrdered& delta) {
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const std::vector<bool>& keys, VectorValues& delta) {
std::vector<bool> changed(keys.size(), false);
int count = 0;
// starting from the root, call optimize on each conditional
@ -200,7 +200,7 @@ int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, co
/* ************************************************************************* */
template<class CLIQUE>
int optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, double threshold, const std::vector<bool>& keys, VectorValuesOrdered& delta) {
int optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root, double threshold, const std::vector<bool>& keys, VectorValues& delta) {
std::vector<bool> changed(keys.size(), false);
int count = 0;

109
gtsam/nonlinear/ISAM2.cpp
View File

@ -124,10 +124,10 @@ ISAM2& ISAM2::operator=(const ISAM2& rhs) {
deltaReplacedMask_ = rhs.deltaReplacedMask_;
nonlinearFactors_ = rhs.nonlinearFactors_;
linearFactors_ = GaussianFactorGraphOrdered();
linearFactors_ = GaussianFactorGraph();
linearFactors_.reserve(rhs.linearFactors_.size());
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& linearFactor, rhs.linearFactors_) {
linearFactors_.push_back(linearFactor ? linearFactor->clone() : GaussianFactorOrdered::shared_ptr()); }
BOOST_FOREACH(const GaussianFactor::shared_ptr& linearFactor, rhs.linearFactors_) {
linearFactors_.push_back(linearFactor ? linearFactor->clone() : GaussianFactor::shared_ptr()); }
ordering_ = rhs.ordering_;
params_ = rhs.params_;
@ -150,12 +150,12 @@ FastList<size_t> ISAM2::getAffectedFactors(const FastList<Index>& keys) const {
if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } }
if(debug) cout << endl;
FactorGraphOrdered<NonlinearFactor > allAffected;
FactorGraph<NonlinearFactor > allAffected;
FastList<size_t> indices;
BOOST_FOREACH(const Index key, keys) {
// const list<size_t> l = nonlinearFactors_.factors(key);
// indices.insert(indices.begin(), l.begin(), l.end());
const VariableIndexOrdered::Factors& factors(variableIndex_[key]);
const VariableIndex::Factors& factors(variableIndex_[key]);
BOOST_FOREACH(size_t factor, factors) {
if(debug) cout << "Variable " << key << " affects factor " << factor << endl;
indices.push_back(factor);
@ -172,7 +172,8 @@ FastList<size_t> ISAM2::getAffectedFactors(const FastList<Index>& keys) const {
/* ************************************************************************* */
// retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors)
FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr
FactorGraph<GaussianFactor>::shared_ptr
ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const FastSet<Index>& relinKeys) const {
gttic(getAffectedFactors);
@ -188,7 +189,7 @@ ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const Fas
gttoc(affectedKeysSet);
gttic(check_candidates_and_linearize);
FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr linearized = boost::make_shared<FactorGraphOrdered<GaussianFactorOrdered> >();
FactorGraph<GaussianFactor>::shared_ptr linearized = boost::make_shared<FactorGraph<GaussianFactor> >();
BOOST_FOREACH(size_t idx, candidates) {
bool inside = true;
bool useCachedLinear = params_.cacheLinearizedFactors;
@ -209,7 +210,7 @@ ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const Fas
#endif
linearized->push_back(linearFactors_[idx]);
} else {
GaussianFactorOrdered::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_, ordering_);
GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_, ordering_);
linearized->push_back(linearFactor);
if(params_.cacheLinearizedFactors) {
#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
@ -227,11 +228,12 @@ ISAM2::relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const Fas
/* ************************************************************************* */
// find intermediate (linearized) factors from cache that are passed into the affected area
GaussianFactorGraphOrdered ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) {
static const bool debug = false;
GaussianFactorGraphOrdered cachedBoundary;
GaussianFactorGraph cachedBoundary;
BOOST_FOREACH(sharedClique orphan, orphans) {
// find the last variable that was eliminated
@ -286,14 +288,14 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
// (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques.
gttic(removetop);
Cliques orphans;
BayesNetOrdered<GaussianConditionalOrdered> affectedBayesNet;
BayesNet<GaussianConditional> affectedBayesNet;
this->removeTop(markedKeys, affectedBayesNet, orphans);
gttoc(removetop);
if(debug) affectedBayesNet.print("Removed top: ");
if(debug) orphans.print("Orphans: ");
// FactorGraph<GaussianFactorOrdered> factors(affectedBayesNet);
// FactorGraph<GaussianFactor> factors(affectedBayesNet);
// bug was here: we cannot reuse the original factors, because then the cached factors get messed up
// [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries,
// so this would be correct; however, in the process we also generate new cached_ entries that will be wrong (ie. they don't
@ -316,7 +318,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
gttic(batch);
gttic(add_keys);
BOOST_FOREACH(const OrderingOrdered::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); }
BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); }
gttoc(add_keys);
gttic(reorder);
@ -359,18 +361,18 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
gttoc(reorder);
gttic(linearize);
GaussianFactorGraphOrdered linearized = *nonlinearFactors_.linearize(theta_, ordering_);
GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_, ordering_);
if(params_.cacheLinearizedFactors)
linearFactors_ = linearized;
gttoc(linearize);
gttic(eliminate);
JunctionTreeOrdered<GaussianFactorGraphOrdered, Base::Clique> jt(linearized, variableIndex_);
JunctionTree<GaussianFactorGraph, Base::Clique> jt(linearized, variableIndex_);
sharedClique newRoot;
if(params_.factorization == ISAM2Params::CHOLESKY)
newRoot = jt.eliminate(EliminatePreferCholeskyOrdered);
newRoot = jt.eliminate(EliminatePreferCholesky);
else if(params_.factorization == ISAM2Params::QR)
newRoot = jt.eliminate(EliminateQROrdered);
newRoot = jt.eliminate(EliminateQR);
else assert(false);
if(debug) newRoot->print("Eliminated: ");
gttoc(eliminate);
@ -405,7 +407,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end());
affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), observedKeys.end());
gttic(relinearizeAffected);
GaussianFactorGraphOrdered factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys));
GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys));
if(debug) factors.print("Relinearized factors: ");
gttoc(relinearizeAffected);
@ -433,7 +435,7 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(const FastSet<Index>& mark
gttic(cached);
// add the cached intermediate results from the boundary of the orphans ...
GaussianFactorGraphOrdered cachedBoundary = getCachedBoundaryFactors(orphans);
GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans);
if(debug) cachedBoundary.print("Boundary factors: ");
factors.push_back(cachedBoundary);
gttoc(cached);
@ -737,7 +739,7 @@ ISAM2Result ISAM2::update(
// 7. Linearize new factors
if(params_.cacheLinearizedFactors) {
gttic(linearize);
FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_);
FactorGraph<GaussianFactor>::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_);
linearFactors_.push_back(*linearFactors);
assert(nonlinearFactors_.size() == linearFactors_.size());
gttoc(linearize);
@ -804,7 +806,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeys)
// Keep track of marginal factors - map from clique to the marginal factors
// that should be incorporated into it, passed up from it's children.
multimap<sharedClique, GaussianFactorOrdered::shared_ptr> marginalFactors;
multimap<sharedClique, GaussianFactor::shared_ptr> marginalFactors;
// Remove each variable and its subtrees
BOOST_REVERSE_FOREACH(Index j, indices) {
@ -821,7 +823,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeys)
// Remove either the whole clique or part of it
if(marginalizeEntireClique) {
// Remove the whole clique and its subtree, and keep the marginal factor.
GaussianFactorOrdered::shared_ptr marginalFactor = clique->cachedFactor();
GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor();
// We do not need the marginal factors associated with this clique
// because their information is already incorporated in the new
// marginal factor. So, now associate this marginal factor with the
@ -845,7 +847,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeys)
// subtrees already marginalized out.
// Add child marginals and remove marginalized subtrees
GaussianFactorGraphOrdered graph;
GaussianFactorGraph graph;
FastSet<size_t> factorsInSubtreeRoot;
Cliques subtreesToRemove;
BOOST_FOREACH(const sharedClique& child, clique->children()) {
@ -897,30 +899,30 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeys)
std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), indices.begin(), indices.end(),
std::inserter(cliqueFrontalsToEliminate, cliqueFrontalsToEliminate.end()));
vector<Index> cliqueFrontalsToEliminateV(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end());
pair<GaussianConditionalOrdered::shared_ptr, GaussianFactorGraphOrdered> eliminationResult1 =
pair<GaussianConditional::shared_ptr, GaussianFactorGraph> eliminationResult1 =
graph.eliminate(cliqueFrontalsToEliminateV,
params_.factorization==ISAM2Params::QR ? EliminateQROrdered : EliminatePreferCholeskyOrdered);
params_.factorization==ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky);
// Add the resulting marginal
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& marginal, eliminationResult1.second) {
BOOST_FOREACH(const GaussianFactor::shared_ptr& marginal, eliminationResult1.second) {
if(marginal)
marginalFactors.insert(make_pair(clique, marginal)); }
// Recover the conditional on the remaining subset of frontal variables
// of this clique being martially marginalized.
size_t nToEliminate = std::find(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals(), j) - clique->conditional()->begin() + 1;
GaussianFactorGraphOrdered graph2;
GaussianFactorGraph graph2;
graph2.push_back(clique->conditional()->toFactor());
GaussianFactorGraphOrdered::EliminationResult eliminationResult2 =
GaussianFactorGraph::EliminationResult eliminationResult2 =
params_.factorization == ISAM2Params::QR ?
EliminateQROrdered(graph2, nToEliminate) :
EliminatePreferCholeskyOrdered(graph2, nToEliminate);
GaussianFactorGraphOrdered graph3;
EliminateQR(graph2, nToEliminate) :
EliminatePreferCholesky(graph2, nToEliminate);
GaussianFactorGraph graph3;
graph3.push_back(eliminationResult2.second);
GaussianFactorGraphOrdered::EliminationResult eliminationResult3 =
GaussianFactorGraph::EliminationResult eliminationResult3 =
params_.factorization == ISAM2Params::QR ?
EliminateQROrdered(graph3, clique->conditional()->nrFrontals() - nToEliminate) :
EliminatePreferCholeskyOrdered(graph3, clique->conditional()->nrFrontals() - nToEliminate);
EliminateQR(graph3, clique->conditional()->nrFrontals() - nToEliminate) :
EliminatePreferCholesky(graph3, clique->conditional()->nrFrontals() - nToEliminate);
sharedClique newClique = boost::make_shared<Clique>(make_pair(eliminationResult3.first, clique->cachedFactor()));
// Add the marginalized clique to the BayesTree
@ -936,8 +938,8 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeys)
// At this point we have updated the BayesTree, now update the remaining iSAM2 data structures
// Gather factors to add - the new marginal factors
GaussianFactorGraphOrdered factorsToAdd;
typedef pair<sharedClique, GaussianFactorOrdered::shared_ptr> Clique_Factor;
GaussianFactorGraph factorsToAdd;
typedef pair<sharedClique, GaussianFactor::shared_ptr> Clique_Factor;
BOOST_FOREACH(const Clique_Factor& clique_factor, marginalFactors) {
if(clique_factor.second)
factorsToAdd.push_back(clique_factor.second);
@ -955,7 +957,7 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeys)
BOOST_FOREACH(Index j, indices) {
factorIndicesToRemove.insert(variableIndex_[j].begin(), variableIndex_[j].end()); }
vector<size_t> removedFactorIndices;
SymbolicFactorGraphOrdered removedFactors;
SymbolicFactorGraph removedFactors;
BOOST_FOREACH(size_t i, factorIndicesToRemove) {
removedFactorIndices.push_back(i);
removedFactors.push_back(nonlinearFactors_[i]->symbolic(ordering_));
@ -1011,7 +1013,7 @@ Values ISAM2::calculateEstimate() const {
Values ret(theta_);
gttoc(Copy_Values);
gttic(getDelta);
const VectorValuesOrdered& delta(getDelta());
const VectorValues& delta(getDelta());
gttoc(getDelta);
gttic(Expmap);
vector<bool> mask(ordering_.size(), true);
@ -1023,35 +1025,36 @@ Values ISAM2::calculateEstimate() const {
/* ************************************************************************* */
Matrix ISAM2::marginalCovariance(Index key) const {
return marginalFactor(ordering_[key],
params_.factorization == ISAM2Params::QR ? EliminateQROrdered : EliminatePreferCholeskyOrdered)
params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky)
->information().inverse();
}
/* ************************************************************************* */
Values ISAM2::calculateBestEstimate() const {
VectorValuesOrdered delta(theta_.dims(ordering_));
VectorValues delta(theta_.dims(ordering_));
internal::optimizeInPlace<Base>(this->root(), delta);
return theta_.retract(delta, ordering_);
}
/* ************************************************************************* */
const VectorValuesOrdered& ISAM2::getDelta() const {
const VectorValues& ISAM2::getDelta() const {
if(!deltaUptodate_)
updateDelta();
return delta_;
}
/* ************************************************************************* */
VectorValuesOrdered optimize(const ISAM2& isam) {
VectorValues optimize(const ISAM2& isam) {
gttic(allocateVectorValues);
VectorValuesOrdered delta = *allocateVectorValues(isam);
VectorValues delta = *allocateVectorValues(isam);
gttoc(allocateVectorValues);
optimizeInPlace(isam, delta);
return delta;
}
/* ************************************************************************* */
void optimizeInPlace(const ISAM2& isam, VectorValuesOrdered& delta) {
void optimizeInPlace(const ISAM2& isam, VectorValues& delta) {
// We may need to update the solution calculations
if(!isam.deltaDoglegUptodate_) {
gttic(UpdateDoglegDeltas);
@ -1073,9 +1076,10 @@ void optimizeInPlace(const ISAM2& isam, VectorValuesOrdered& delta) {
}
/* ************************************************************************* */
VectorValuesOrdered optimizeGradientSearch(const ISAM2& isam) {
VectorValues optimizeGradientSearch(const ISAM2& isam) {
gttic(Allocate_VectorValues);
VectorValuesOrdered grad = *allocateVectorValues(isam);
VectorValues grad = *allocateVectorValues(isam);
gttoc(Allocate_VectorValues);
optimizeGradientSearchInPlace(isam, grad);
@ -1084,7 +1088,7 @@ VectorValuesOrdered optimizeGradientSearch(const ISAM2& isam) {
}
/* ************************************************************************* */
void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValuesOrdered& grad) {
void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) {
// We may need to update the solution calcaulations
if(!isam.deltaDoglegUptodate_) {
gttic(UpdateDoglegDeltas);
@ -1119,15 +1123,16 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValuesOrdered& grad)
}
/* ************************************************************************* */
VectorValuesOrdered gradient(const ISAM2& bayesTree, const VectorValuesOrdered& x0) {
return gradient(FactorGraphOrdered<JacobianFactorOrdered>(bayesTree), x0);
VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0) {
return gradient(FactorGraph<JacobianFactor>(bayesTree), x0);
}
/* ************************************************************************* */
static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& root, VectorValuesOrdered& g) {
static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& root, VectorValues& g) {
// Loop through variables in each clique, adding contributions
int variablePosition = 0;
for(GaussianConditionalOrdered::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) {
for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) {
const int dim = root->conditional()->dim(jit);
g[*jit] += root->gradientContribution().segment(variablePosition, dim);
variablePosition += dim;
@ -1141,7 +1146,7 @@ static void gradientAtZeroTreeAdder(const boost::shared_ptr<ISAM2Clique>& root,
}
/* ************************************************************************* */
void gradientAtZero(const ISAM2& bayesTree, VectorValuesOrdered& g) {
void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) {
// Zero-out gradient
g.setZero();

52
gtsam/nonlinear/ISAM2.h
View File

@ -344,13 +344,13 @@ struct GTSAM_EXPORT ISAM2Result {
* Specialized Clique structure for ISAM2, incorporating caching and gradient contribution
* TODO: more documentation
*/
class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBaseOrdered<ISAM2Clique, GaussianConditionalOrdered> {
class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianConditional> {
public:
typedef ISAM2Clique This;
typedef BayesTreeCliqueBaseOrdered<This,GaussianConditionalOrdered> Base;
typedef BayesTreeCliqueBase<This,GaussianConditional> Base;
typedef boost::shared_ptr<This> shared_ptr;
typedef boost::weak_ptr<This> weak_ptr;
typedef GaussianConditionalOrdered ConditionalType;
typedef GaussianConditional ConditionalType;
typedef ConditionalType::shared_ptr sharedConditional;
Base::FactorType::shared_ptr cachedFactor_;
@ -438,7 +438,7 @@ private:
* estimate of all variables.
*
*/
class ISAM2: public BayesTreeOrdered<GaussianConditionalOrdered, ISAM2Clique> {
class ISAM2: public BayesTree<GaussianConditional, ISAM2Clique> {
protected:
@ -446,7 +446,7 @@ protected:
Values theta_;
/** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */
VariableIndexOrdered variableIndex_;
VariableIndex variableIndex_;
/** The linear delta from the last linear solution, an update to the estimate in theta
*
@ -454,10 +454,10 @@ protected:
* until either requested with getDelta() or calculateEstimate(), or needed
* during update() to evaluate whether to relinearize variables.
*/
mutable VectorValuesOrdered delta_;
mutable VectorValues delta_;
mutable VectorValuesOrdered deltaNewton_;
mutable VectorValuesOrdered RgProd_;
mutable VectorValues deltaNewton_;
mutable VectorValues RgProd_;
mutable bool deltaDoglegUptodate_;
/** Indicates whether the current delta is up-to-date, only used
@ -487,11 +487,11 @@ protected:
NonlinearFactorGraph nonlinearFactors_;
/** The current linear factors, which are only updated as needed */
mutable GaussianFactorGraphOrdered linearFactors_;
mutable GaussianFactorGraph linearFactors_;
/** The current elimination ordering Symbols to Index (integer) keys.
* We keep it up to date as we add and reorder variables. */
OrderingOrdered ordering_;
Ordering ordering_;
/** The current parameters */
ISAM2Params params_;
@ -506,7 +506,7 @@ protected:
public:
typedef ISAM2 This; ///< This class
typedef BayesTreeOrdered<GaussianConditionalOrdered,ISAM2Clique> Base; ///< The BayesTree base class
typedef BayesTree<GaussianConditional,ISAM2Clique> Base; ///< The BayesTree base class
/** Create an empty ISAM2 instance */
GTSAM_EXPORT ISAM2(const ISAM2Params& params);
@ -601,16 +601,16 @@ public:
GTSAM_EXPORT Values calculateBestEstimate() const;
/** Access the current delta, computed during the last call to update */
GTSAM_EXPORT const VectorValuesOrdered& getDelta() const;
GTSAM_EXPORT const VectorValues& getDelta() const;
/** Access the set of nonlinear factors */
GTSAM_EXPORT const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; }
/** Access the current ordering */
GTSAM_EXPORT const OrderingOrdered& getOrdering() const { return ordering_; }
GTSAM_EXPORT const Ordering& getOrdering() const { return ordering_; }
/** Access the nonlinear variable index */
GTSAM_EXPORT const VariableIndexOrdered& getVariableIndex() const { return variableIndex_; }
GTSAM_EXPORT const VariableIndex& getVariableIndex() const { return variableIndex_; }
size_t lastAffectedVariableCount;
size_t lastAffectedFactorCount;
@ -629,24 +629,24 @@ public:
private:
GTSAM_EXPORT FastList<size_t> getAffectedFactors(const FastList<Index>& keys) const;
GTSAM_EXPORT FactorGraphOrdered<GaussianFactorOrdered>::shared_ptr relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const FastSet<Index>& relinKeys) const;
GTSAM_EXPORT GaussianFactorGraphOrdered getCachedBoundaryFactors(Cliques& orphans);
GTSAM_EXPORT FactorGraph<GaussianFactor>::shared_ptr relinearizeAffectedFactors(const FastList<Index>& affectedKeys, const FastSet<Index>& relinKeys) const;
GTSAM_EXPORT GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans);
GTSAM_EXPORT boost::shared_ptr<FastSet<Index> > recalculate(const FastSet<Index>& markedKeys, const FastSet<Index>& relinKeys,
const FastVector<Index>& observedKeys, const FastSet<Index>& unusedIndices, const boost::optional<FastMap<Index,int> >& constrainKeys, ISAM2Result& result);
// void linear_update(const GaussianFactorGraph& newFactors);
GTSAM_EXPORT void updateDelta(bool forceFullSolve = false) const;
GTSAM_EXPORT friend void optimizeInPlace(const ISAM2&, VectorValuesOrdered&);
GTSAM_EXPORT friend void optimizeGradientSearchInPlace(const ISAM2&, VectorValuesOrdered&);
GTSAM_EXPORT friend void optimizeInPlace(const ISAM2&, VectorValues&);
GTSAM_EXPORT friend void optimizeGradientSearchInPlace(const ISAM2&, VectorValues&);
}; // ISAM2
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
GTSAM_EXPORT VectorValuesOrdered optimize(const ISAM2& isam);
GTSAM_EXPORT VectorValues optimize(const ISAM2& isam);
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
GTSAM_EXPORT void optimizeInPlace(const ISAM2& isam, VectorValuesOrdered& delta);
GTSAM_EXPORT void optimizeInPlace(const ISAM2& isam, VectorValues& delta);
/// Optimize the BayesTree, starting from the root.
/// @param replaced Needs to contain
@ -661,11 +661,11 @@ GTSAM_EXPORT void optimizeInPlace(const ISAM2& isam, VectorValuesOrdered& delta)
/// @return The number of variables that were solved for
template<class CLIQUE>
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
double threshold, const std::vector<bool>& replaced, VectorValuesOrdered& delta);
double threshold, const std::vector<bool>& replaced, VectorValues& delta);
template<class CLIQUE>
int optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root,
double threshold, const std::vector<bool>& replaced, VectorValuesOrdered& delta);
double threshold, const std::vector<bool>& replaced, VectorValues& delta);
/**
* Optimize along the gradient direction, with a closed-form computation to
@ -692,10 +692,10 @@ int optimizeWildfireNonRecursive(const boost::shared_ptr<CLIQUE>& root,
*
* \f[ \delta x = \hat\alpha g = \frac{-g^T g}{(R g)^T(R g)} \f]
*/
GTSAM_EXPORT VectorValuesOrdered optimizeGradientSearch(const ISAM2& isam);
GTSAM_EXPORT VectorValues optimizeGradientSearch(const ISAM2& isam);
/** In-place version of optimizeGradientSearch requiring pre-allocated VectorValues \c x */
GTSAM_EXPORT void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValuesOrdered& grad);
GTSAM_EXPORT void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad);
/// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
template<class CLIQUE>
@ -712,7 +712,7 @@ int calculate_nnz(const boost::shared_ptr<CLIQUE>& clique);
* @param x0 The center about which to compute the gradient
* @return The gradient as a VectorValues
*/
GTSAM_EXPORT VectorValuesOrdered gradient(const ISAM2& bayesTree, const VectorValuesOrdered& x0);
GTSAM_EXPORT VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0);
/**
* Compute the gradient of the energy function,
@ -725,7 +725,7 @@ GTSAM_EXPORT VectorValuesOrdered gradient(const ISAM2& bayesTree, const VectorVa
* @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
* @return The gradient as a VectorValues
*/
GTSAM_EXPORT void gradientAtZero(const ISAM2& bayesTree, VectorValuesOrdered& g);
GTSAM_EXPORT void gradientAtZero(const ISAM2& bayesTree, VectorValues& g);
} /// namespace gtsam

18
gtsam/nonlinear/Marginals.cpp
View File

@ -68,11 +68,11 @@ Matrix Marginals::marginalInformation(Key variable) const {
Index index = ordering_[variable];
// Compute marginal
GaussianFactorOrdered::shared_ptr marginalFactor;
GaussianFactor::shared_ptr marginalFactor;
if(factorization_ == CHOLESKY)
marginalFactor = bayesTree_.marginalFactor(index, EliminatePreferCholeskyOrdered);
marginalFactor = bayesTree_.marginalFactor(index, EliminatePreferCholesky);
else if(factorization_ == QR)
marginalFactor = bayesTree_.marginalFactor(index, EliminateQROrdered);
marginalFactor = bayesTree_.marginalFactor(index, EliminateQR);
// Get information matrix (only store upper-right triangle)
gttic(AsMatrix);
@ -108,7 +108,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
Matrix info = marginalInformation(variables.front());
std::vector<size_t> dims;
dims.push_back(info.rows());
OrderingOrdered indices;
Ordering indices;
indices.insert(variables.front(), 0);
return JointMarginal(info, dims, indices);
@ -118,12 +118,12 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
for(size_t i=0; i<variables.size(); ++i) { indices[i] = ordering_[variables[i]]; }
// Compute joint marginal factor graph.
GaussianFactorGraphOrdered jointFG;
GaussianFactorGraph jointFG;
if(variables.size() == 2) {
if(factorization_ == CHOLESKY)
jointFG = *bayesTree_.joint(indices[0], indices[1], EliminatePreferCholeskyOrdered);
jointFG = *bayesTree_.joint(indices[0], indices[1], EliminatePreferCholesky);
else if(factorization_ == QR)
jointFG = *bayesTree_.joint(indices[0], indices[1], EliminateQROrdered);
jointFG = *bayesTree_.joint(indices[0], indices[1], EliminateQR);
} else {
if(factorization_ == CHOLESKY)
jointFG = *GaussianSequentialSolver(graph_, false).jointFactorGraph(indices);
@ -133,7 +133,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
// Build map from variable keys to position in factor graph variables,
// which are sorted in index order.
OrderingOrdered variableConversion;
Ordering variableConversion;
{
// First build map from index to key
FastMap<Index,Key> usedIndices;
@ -166,7 +166,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
void JointMarginal::print(const std::string& s, const KeyFormatter& formatter) const {
cout << s << "Joint marginal on keys ";
bool first = true;
BOOST_FOREACH(const OrderingOrdered::value_type& key_index, indices_) {
BOOST_FOREACH(const Ordering::value_type& key_index, indices_) {
if(!first)
cout << ", ";
else

10
gtsam/nonlinear/Marginals.h
View File

@ -44,11 +44,11 @@ public:
protected:
GaussianFactorGraphOrdered graph_;
OrderingOrdered ordering_;
GaussianFactorGraph graph_;
Ordering ordering_;
Values values_;
Factorization factorization_;
GaussianBayesTreeOrdered bayesTree_;
GaussianBayesTree bayesTree_;
public:
@ -88,7 +88,7 @@ protected:
Matrix fullMatrix_;
BlockView blockView_;
OrderingOrdered indices_;
Ordering indices_;
public:
/** A block view of the joint marginal - this stores a reference to the
@ -135,7 +135,7 @@ public:
void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
protected:
JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const OrderingOrdered& indices) :
JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const Ordering& indices) :
fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {}
friend class Marginals;

4
gtsam/nonlinear/NonlinearEquality.h
View File

@ -153,12 +153,12 @@ namespace gtsam {
}
// Linearize is over-written, because base linearization tries to whiten
virtual GaussianFactorOrdered::shared_ptr linearize(const Values& x, const OrderingOrdered& ordering) const {
virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const {
const T& xj = x.at<T>(this->key());
Matrix A;
Vector b = evaluateError(xj, A);
SharedDiagonal model = noiseModel::Constrained::All(b.size());
return GaussianFactorOrdered::shared_ptr(new JacobianFactorOrdered(ordering[this->key()], A, b, model));
return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model));
}
/// @return a deep copy of this factor

4
gtsam/nonlinear/NonlinearFactor.h
View File

@ -130,11 +130,11 @@ public:
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
//virtual IndexFactorOrdered::shared_ptr symbolic(const OrderingOrdered& ordering) const {
//virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
// std::vector<Index> indices(this->size());
// for(size_t j=0; j<this->size(); ++j)
// indices[j] = ordering[this->keys()[j]];
// return IndexFactorOrdered::shared_ptr(new IndexFactorOrdered(indices));
// return IndexFactor::shared_ptr(new IndexFactor(indices));
//}
/**

10
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -221,31 +221,31 @@ Ordering NonlinearFactorGraph::orderingCOLAMDConstrained(const FastMap<Key, int>
}
/* ************************************************************************* */
//SymbolicFactorGraphOrdered::shared_ptr NonlinearFactorGraph::symbolic(const OrderingOrdered& ordering) const {
//SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
// gttic(NonlinearFactorGraph_symbolic_from_Ordering);
//
// // Generate the symbolic factor graph
// SymbolicFactorGraphOrdered::shared_ptr symbolicfg(new SymbolicFactorGraphOrdered);
// SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
// symbolicfg->reserve(this->size());
//
// BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
// if(factor)
// symbolicfg->push_back(factor->symbolic(ordering));
// else
// symbolicfg->push_back(SymbolicFactorGraphOrdered::sharedFactor());
// symbolicfg->push_back(SymbolicFactorGraph::sharedFactor());
// }
//
// return symbolicfg;
//}
/* ************************************************************************* */
//pair<SymbolicFactorGraphOrdered::shared_ptr, OrderingOrdered::shared_ptr> NonlinearFactorGraph::symbolic(
//pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
// const Values& config) const
//{
// gttic(NonlinearFactorGraph_symbolic_from_Values);
//
// // Generate an initial key ordering in iterator order
// OrderingOrdered::shared_ptr ordering(config.orderingArbitrary());
// Ordering::shared_ptr ordering(config.orderingArbitrary());
// return make_pair(symbolic(*ordering), ordering);
//}

6
gtsam/nonlinear/NonlinearISAM.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/ISAMOrdered-inl.h>
#include <gtsam/inference/ISAM-inl.h>
#include <gtsam/nonlinear/Ordering.h>
#include <boost/foreach.hpp>
@ -60,7 +60,7 @@ void NonlinearISAM::update(const NonlinearFactorGraph& newFactors,
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialValues)
ordering_.insert(key_value.key, ordering_.size());
boost::shared_ptr<GaussianFactorGraphOrdered> linearizedNewFactors = newFactors.linearize(linPoint_, ordering_);
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.linearize(linPoint_, ordering_);
// Update ISAM
isam_.update(*linearizedNewFactors);
@ -84,7 +84,7 @@ void NonlinearISAM::reorder_relinearize() {
// Create a linear factor graph at the new linearization point
// TODO: decouple relinearization and reordering to avoid
boost::shared_ptr<GaussianFactorGraphOrdered> gfg = factors_.linearize(newLinPoint, ordering_);
boost::shared_ptr<GaussianFactorGraph> gfg = factors_.linearize(newLinPoint, ordering_);
// Just recreate the whole BayesTree
isam_.update(*gfg);

10
gtsam/nonlinear/NonlinearISAM.h
View File

@ -30,13 +30,13 @@ class GTSAM_EXPORT NonlinearISAM {
protected:
/** The internal iSAM object */
gtsam::GaussianISAMOrdered isam_;
gtsam::GaussianISAM isam_;
/** The current linearization point */
Values linPoint_;
/** The ordering */
gtsam::OrderingOrdered ordering_;
gtsam::Ordering ordering_;
/** The original factors, used when relinearizing */
NonlinearFactorGraph factors_;
@ -72,13 +72,13 @@ public:
// access
/** access the underlying bayes tree */
const GaussianISAMOrdered& bayesTree() const { return isam_; }
const GaussianISAM& bayesTree() const { return isam_; }
/** Return the current linearization point */
const Values& getLinearizationPoint() const { return linPoint_; }
/** Get the ordering */
const gtsam::OrderingOrdered& getOrdering() const { return ordering_; }
const gtsam::Ordering& getOrdering() const { return ordering_; }
/** get underlying nonlinear graph */
const NonlinearFactorGraph& getFactorsUnsafe() const { return factors_; }
@ -110,7 +110,7 @@ public:
void addKey(Key key) { ordering_.push_back(key); }
/** replace the current ordering */
void setOrdering(const OrderingOrdered& new_ordering) { ordering_ = new_ordering; }
void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; }
/// @}

14
gtsam/nonlinear/WhiteNoiseFactor.h
View File

@ -71,7 +71,7 @@ namespace gtsam {
* @f]
* So f = 2 f(x), g = -df(x), G = ddf(x)
*/
static HessianFactorOrdered::shared_ptr linearize(double z, double u, double p,
static HessianFactor::shared_ptr linearize(double z, double u, double p,
Index j1, Index j2) {
double e = u - z, e2 = e * e;
double c = 2 * logSqrt2PI - log(p) + e2 * p;
@ -80,8 +80,8 @@ namespace gtsam {
Matrix G11 = Matrix_(1, 1, p);
Matrix G12 = Matrix_(1, 1, e);
Matrix G22 = Matrix_(1, 1, 0.5 / (p * p));
return HessianFactorOrdered::shared_ptr(
new HessianFactorOrdered(j1, j2, G11, G12, g1, G22, g2, c));
return HessianFactor::shared_ptr(
new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c));
}
/// @name Standard Constructors
@ -144,9 +144,9 @@ namespace gtsam {
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
virtual IndexFactorOrdered::shared_ptr symbolic(const OrderingOrdered& ordering) const {
virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
const Index j1 = ordering[meanKey_], j2 = ordering[precisionKey_];
return IndexFactorOrdered::shared_ptr(new IndexFactorOrdered(j1, j2));
return IndexFactor::shared_ptr(new IndexFactor(j1, j2));
}
/// @}
@ -154,8 +154,8 @@ namespace gtsam {
/// @{
/// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<GaussianFactorOrdered> linearize(const Values& x,
const OrderingOrdered& ordering) const {
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x,
const Ordering& ordering) const {
double u = x.at<LieScalar>(meanKey_);
double p = x.at<LieScalar>(precisionKey_);
Index j1 = ordering[meanKey_];

20
gtsam/nonlinear/summarization.cpp
View File

@ -16,15 +16,15 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
std::pair<GaussianFactorGraphOrdered,OrderingOrdered>
std::pair<GaussianFactorGraph,Ordering>
summarize(const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys, SummarizationMode mode) {
const size_t nrEliminatedKeys = values.size() - saved_keys.size();
// If we aren't eliminating anything, linearize and return
if (!nrEliminatedKeys || saved_keys.empty()) {
OrderingOrdered ordering = *values.orderingArbitrary();
GaussianFactorGraphOrdered linear_graph = *graph.linearize(values, ordering);
Ordering ordering = *values.orderingArbitrary();
GaussianFactorGraph linear_graph = *graph.linearize(values, ordering);
return make_pair(linear_graph, ordering);
}
@ -35,11 +35,11 @@ summarize(const NonlinearFactorGraph& graph, const Values& values,
BOOST_FOREACH(const gtsam::Key& key, saved_keys)
ordering_constraints.insert(make_pair(key, 1));
OrderingOrdered ordering = *graph.orderingCOLAMDConstrained(values, ordering_constraints);
Ordering ordering = *graph.orderingCOLAMDConstrained(values, ordering_constraints);
// Linearize the system
GaussianFactorGraphOrdered full_graph = *graph.linearize(values, ordering);
GaussianFactorGraphOrdered summarized_system;
GaussianFactorGraph full_graph = *graph.linearize(values, ordering);
GaussianFactorGraph summarized_system;
std::vector<Index> indices;
BOOST_FOREACH(const Key& k, saved_keys)
@ -52,11 +52,11 @@ summarize(const NonlinearFactorGraph& graph, const Values& values,
switch (mode) {
case PARTIAL_QR: {
summarized_system.push_back(EliminateQROrdered(full_graph, nrEliminatedKeys).second);
summarized_system.push_back(EliminateQR(full_graph, nrEliminatedKeys).second);
break;
}
case PARTIAL_CHOLESKY: {
summarized_system.push_back(EliminateCholeskyOrdered(full_graph, nrEliminatedKeys).second);
summarized_system.push_back(EliminateCholesky(full_graph, nrEliminatedKeys).second);
break;
}
case SEQUENTIAL_QR: {
@ -77,8 +77,8 @@ summarize(const NonlinearFactorGraph& graph, const Values& values,
NonlinearFactorGraph summarizeAsNonlinearContainer(
const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys, SummarizationMode mode) {
GaussianFactorGraphOrdered summarized_graph;
OrderingOrdered ordering;
GaussianFactorGraph summarized_graph;
Ordering ordering;
boost::tie(summarized_graph, ordering) = summarize(graph, values, saved_keys, mode);
return LinearContainerFactor::convertLinearGraph(summarized_graph, ordering);
}

2
gtsam/nonlinear/summarization.h
View File

@ -40,7 +40,7 @@ typedef enum {
* @param mode controls what elimination technique and requirements to use
* @return a pair of the remaining graph and the ordering used for linearization
*/
std::pair<GaussianFactorGraphOrdered,OrderingOrdered> GTSAM_EXPORT
std::pair<GaussianFactorGraph,Ordering> GTSAM_EXPORT
summarize(const NonlinearFactorGraph& graph, const Values& values,
const KeySet& saved_keys, SummarizationMode mode = PARTIAL_QR);

6
gtsam/slam/AntiFactor.h
View File

@ -94,11 +94,11 @@ namespace gtsam {
* returns a Jacobian instead of a full Hessian), but with the opposite sign. This
* effectively cancels the effect of the original factor on the factor graph.
*/
boost::shared_ptr<GaussianFactorOrdered>
linearize(const Values& c, const OrderingOrdered& ordering) const {
boost::shared_ptr<GaussianFactor>
linearize(const Values& c, const Ordering& ordering) const {
// Generate the linearized factor from the contained nonlinear factor
GaussianFactorOrdered::shared_ptr gaussianFactor = factor_->linearize(c, ordering);
GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c, ordering);
// return the negated version of the factor
return gaussianFactor->negate();

20
gtsam/slam/tests/testAntiFactor.cpp
View File

@ -46,7 +46,7 @@ TEST( AntiFactor, NegativeHessian)
values->insert(2, pose2);
// Define an elimination ordering
OrderingOrdered::shared_ptr ordering(new OrderingOrdered());
Ordering::shared_ptr ordering(new Ordering());
ordering->insert(1, 0);
ordering->insert(2, 1);
@ -55,17 +55,17 @@ TEST( AntiFactor, NegativeHessian)
BetweenFactor<Pose3>::shared_ptr originalFactor(new BetweenFactor<Pose3>(1, 2, z, sigma));
// Linearize it into a Jacobian Factor
GaussianFactorOrdered::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering);
GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering);
// Convert it to a Hessian Factor
HessianFactorOrdered::shared_ptr originalHessian = HessianFactorOrdered::shared_ptr(new HessianFactorOrdered(*originalJacobian));
HessianFactor::shared_ptr originalHessian = HessianFactor::shared_ptr(new HessianFactor(*originalJacobian));
// Create the AntiFactor version of the original nonlinear factor
AntiFactor::shared_ptr antiFactor(new AntiFactor(originalFactor));
// Linearize the AntiFactor into a Hessian Factor
GaussianFactorOrdered::shared_ptr antiGaussian = antiFactor->linearize(*values, *ordering);
HessianFactorOrdered::shared_ptr antiHessian = boost::dynamic_pointer_cast<HessianFactorOrdered>(antiGaussian);
GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(*values, *ordering);
HessianFactor::shared_ptr antiHessian = boost::dynamic_pointer_cast<HessianFactor>(antiGaussian);
// Compare Hessian blocks
@ -107,14 +107,14 @@ TEST( AntiFactor, EquivalentBayesNet)
values->insert(2, pose2);
// Define an elimination ordering
OrderingOrdered::shared_ptr ordering = graph->orderingCOLAMD(*values);
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
// Eliminate into a BayesNet
GaussianSequentialSolver solver1(*graph->linearize(*values, *ordering));
GaussianBayesNetOrdered::shared_ptr expectedBayesNet = solver1.eliminate();
GaussianBayesNet::shared_ptr expectedBayesNet = solver1.eliminate();
// Back-substitute to find the optimal deltas
VectorValuesOrdered expectedDeltas = optimize(*expectedBayesNet);
VectorValues expectedDeltas = optimize(*expectedBayesNet);
// Add an additional factor between Pose1 and Pose2
BetweenFactor<Pose3>::shared_ptr f1(new BetweenFactor<Pose3>(1, 2, z, sigma));
@ -126,10 +126,10 @@ TEST( AntiFactor, EquivalentBayesNet)
// Again, Eliminate into a BayesNet
GaussianSequentialSolver solver2(*graph->linearize(*values, *ordering));
GaussianBayesNetOrdered::shared_ptr actualBayesNet = solver2.eliminate();
GaussianBayesNet::shared_ptr actualBayesNet = solver2.eliminate();
// Back-substitute to find the optimal deltas
VectorValuesOrdered actualDeltas = optimize(*actualBayesNet);
VectorValues actualDeltas = optimize(*actualBayesNet);
// Verify the BayesNets are identical
CHECK(assert_equal(*expectedBayesNet, *actualBayesNet, 1e-5));

14
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -149,8 +149,8 @@ static vector<GeneralCamera> genCameraVariableCalibration() {
return X ;
}
static boost::shared_ptr<OrderingOrdered> getOrdering(const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
boost::shared_ptr<OrderingOrdered> ordering(new OrderingOrdered);
static boost::shared_ptr<Ordering> getOrdering(const vector<GeneralCamera>& cameras, const vector<Point3>& landmarks) {
boost::shared_ptr<Ordering> ordering(new Ordering);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ;
return ordering ;
@ -190,7 +190,7 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
graph.addCameraConstraint(0, cameras[0]);
// Create an ordering of the variables
OrderingOrdered ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
@ -233,7 +233,7 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
graph.addCameraConstraint(0, cameras[0]);
const double reproj_error = 1e-5;
OrderingOrdered ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -274,7 +274,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const double reproj_error = 1e-5 ;
OrderingOrdered ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -331,7 +331,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const double reproj_error = 1e-5 ;
OrderingOrdered ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -375,7 +375,7 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
const double reproj_error = 1e-5 ;
OrderingOrdered ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);

14
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -152,8 +152,8 @@ static vector<GeneralCamera> genCameraVariableCalibration() {
return cameras ;
}
static boost::shared_ptr<OrderingOrdered> getOrdering(const std::vector<GeneralCamera>& cameras, const std::vector<Point3>& landmarks) {
boost::shared_ptr<OrderingOrdered> ordering(new OrderingOrdered);
static boost::shared_ptr<Ordering> getOrdering(const std::vector<GeneralCamera>& cameras, const std::vector<Point3>& landmarks) {
boost::shared_ptr<Ordering> ordering(new Ordering);
for ( size_t i = 0 ; i < landmarks.size() ; ++i ) ordering->push_back(L(i)) ;
for ( size_t i = 0 ; i < cameras.size() ; ++i ) ordering->push_back(X(i)) ;
return ordering ;
@ -192,7 +192,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_defaultK ) {
graph.addCameraConstraint(0, cameras[0]);
// Create an ordering of the variables
OrderingOrdered ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * 1e-5 * nMeasurements);
@ -235,7 +235,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_SingleMeasurementError ) {
graph.addCameraConstraint(0, cameras[0]);
const double reproj_error = 1e-5;
OrderingOrdered ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -276,7 +276,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixCameras ) {
const double reproj_error = 1e-5 ;
OrderingOrdered ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -329,7 +329,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_FixLandmarks ) {
const double reproj_error = 1e-5 ;
OrderingOrdered ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);
@ -373,7 +373,7 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
const double reproj_error = 1e-5 ;
OrderingOrdered ordering = *getOrdering(cameras,landmarks);
Ordering ordering = *getOrdering(cameras,landmarks);
LevenbergMarquardtOptimizer optimizer(graph, values, ordering);
Values final = optimizer.optimize();
EXPECT(optimizer.error() < 0.5 * reproj_error * nMeasurements);

6
gtsam/symbolic/tests/testSymbolicBayesNet.cpp
View File

@ -38,7 +38,7 @@ static SymbolicConditional::shared_ptr
L(new SymbolicConditional(_L_, _B_));
/* ************************************************************************* */
TEST( SymbolicBayesNetOrdered, equals )
TEST( SymbolicBayesNet, equals )
{
SymbolicBayesNet f1;
f1.push_back(B);
@ -51,7 +51,7 @@ TEST( SymbolicBayesNetOrdered, equals )
}
/* ************************************************************************* */
TEST( SymbolicBayesNetOrdered, combine )
TEST( SymbolicBayesNet, combine )
{
SymbolicConditional::shared_ptr
A(new SymbolicConditional(_A_,_B_,_C_)),
@ -79,7 +79,7 @@ TEST( SymbolicBayesNetOrdered, combine )
}
/* ************************************************************************* */
TEST(SymbolicBayesNetOrdered, saveGraph) {
TEST(SymbolicBayesNet, saveGraph) {
SymbolicBayesNet bn;
bn += SymbolicConditional(_A_, _B_);
std::vector<Index> keys;

20
gtsam/symbolic/tests/testSymbolicBayesTree.cpp
View File

@ -67,7 +67,7 @@ namespace {
}
/* ************************************************************************* */
TEST(SymbolicBayesTreeOrdered, clear)
TEST(SymbolicBayesTree, clear)
{
SymbolicBayesTree bayesTree = asiaBayesTree;
bayesTree.clear();
@ -85,7 +85,7 @@ Bayes Tree for testing conversion to a forest of orphans needed for incremental.
D|C F|E
*/
/* ************************************************************************* */
TEST( BayesTreeOrdered, removePath )
TEST( BayesTree, removePath )
{
const Key _A_=A(0), _B_=B(0), _C_=C(0), _D_=D(0), _E_=E(0), _F_=F(0);
@ -133,7 +133,7 @@ TEST( BayesTreeOrdered, removePath )
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, removePath2 )
TEST( BayesTree, removePath2 )
{
SymbolicBayesTree bayesTree = asiaBayesTree;
@ -153,7 +153,7 @@ TEST( BayesTreeOrdered, removePath2 )
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, removePath3 )
TEST( BayesTree, removePath3 )
{
SymbolicBayesTree bayesTree = asiaBayesTree;
@ -185,7 +185,7 @@ void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, SymbolicBayes
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, shortcutCheck )
TEST( BayesTree, shortcutCheck )
{
const Key _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0;
SymbolicFactorGraph chain = list_of
@ -234,7 +234,7 @@ TEST( BayesTreeOrdered, shortcutCheck )
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, removeTop )
TEST( BayesTree, removeTop )
{
SymbolicBayesTree bayesTree = asiaBayesTree;
@ -268,7 +268,7 @@ TEST( BayesTreeOrdered, removeTop )
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, removeTop2 )
TEST( BayesTree, removeTop2 )
{
SymbolicBayesTree bayesTree = asiaBayesTree;
@ -294,7 +294,7 @@ TEST( BayesTreeOrdered, removeTop2 )
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, removeTop3 )
TEST( BayesTree, removeTop3 )
{
SymbolicFactorGraph graph = list_of
(SymbolicFactor(L(5)))
@ -318,7 +318,7 @@ TEST( BayesTreeOrdered, removeTop3 )
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, removeTop4 )
TEST( BayesTree, removeTop4 )
{
SymbolicFactorGraph graph = list_of
(SymbolicFactor(L(5)))
@ -342,7 +342,7 @@ TEST( BayesTreeOrdered, removeTop4 )
}
/* ************************************************************************* */
TEST( BayesTreeOrdered, removeTop5 )
TEST( BayesTree, removeTop5 )
{
// Remove top called with variables that are not in the Bayes tree
SymbolicFactorGraph graph = list_of

2
gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
View File

@ -71,7 +71,7 @@ public:
/* ************************************************************************* */
TEST(EliminationTreeOrdered, Create)
TEST(EliminationTree, Create)
{
SymbolicEliminationTree expected =
EliminationTreeTester::buildHardcodedTree(simpleTestGraph1);

14
gtsam/symbolic/tests/testSymbolicFactor.cpp
View File

@ -34,17 +34,17 @@ using namespace boost::assign;
#ifdef TRACK_ELIMINATE
TEST(SymbolicFactor, eliminate) {
vector<Index> keys; keys += 2, 3, 4, 6, 7, 9, 10, 11;
IndexFactorOrdered actual(keys.begin(), keys.end());
BayesNetOrdered<IndexConditionalOrdered> fragment = *actual.eliminate(3);
IndexFactor actual(keys.begin(), keys.end());
BayesNet<IndexConditional> fragment = *actual.eliminate(3);
IndexFactorOrdered expected(keys.begin()+3, keys.end());
IndexConditionalOrdered::shared_ptr expected0 = IndexConditionalOrdered::FromRange(keys.begin(), keys.end(), 1);
IndexConditionalOrdered::shared_ptr expected1 = IndexConditionalOrdered::FromRange(keys.begin()+1, keys.end(), 1);
IndexConditionalOrdered::shared_ptr expected2 = IndexConditionalOrdered::FromRange(keys.begin()+2, keys.end(), 1);
IndexFactor expected(keys.begin()+3, keys.end());
IndexConditional::shared_ptr expected0 = IndexConditional::FromRange(keys.begin(), keys.end(), 1);
IndexConditional::shared_ptr expected1 = IndexConditional::FromRange(keys.begin()+1, keys.end(), 1);
IndexConditional::shared_ptr expected2 = IndexConditional::FromRange(keys.begin()+2, keys.end(), 1);
CHECK(assert_equal(fragment.size(), size_t(3)));
CHECK(assert_equal(expected, actual));
BayesNetOrdered<IndexConditionalOrdered>::const_iterator fragmentCond = fragment.begin();
BayesNet<IndexConditional>::const_iterator fragmentCond = fragment.begin();
CHECK(assert_equal(**fragmentCond++, *expected0));
CHECK(assert_equal(**fragmentCond++, *expected1));
CHECK(assert_equal(**fragmentCond++, *expected2));

34
gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
View File

@ -29,7 +29,7 @@ using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */
TEST(SymbolicFactorGraphOrdered, eliminateFullSequential)
TEST(SymbolicFactorGraph, eliminateFullSequential)
{
// Test with simpleTestGraph1
Ordering order;
@ -43,7 +43,7 @@ TEST(SymbolicFactorGraphOrdered, eliminateFullSequential)
}
/* ************************************************************************* */
TEST(SymbolicFactorGraphOrdered, eliminatePartialSequential)
TEST(SymbolicFactorGraph, eliminatePartialSequential)
{
// Eliminate 0 and 1
const Ordering order = list_of(0)(1);
@ -67,7 +67,7 @@ TEST(SymbolicFactorGraphOrdered, eliminatePartialSequential)
}
/* ************************************************************************* */
TEST(SymbolicFactorGraphOrdered, eliminateFullMultifrontal)
TEST(SymbolicFactorGraph, eliminateFullMultifrontal)
{
Ordering ordering; ordering += 0,1,2,3;
SymbolicBayesTree actual1 =
@ -80,7 +80,7 @@ TEST(SymbolicFactorGraphOrdered, eliminateFullMultifrontal)
}
/* ************************************************************************* */
TEST(SymbolicFactorGraphOrdered, eliminatePartialMultifrontal)
TEST(SymbolicFactorGraph, eliminatePartialMultifrontal)
{
SymbolicBayesTree expectedBayesTree;
SymbolicConditional::shared_ptr root = boost::make_shared<SymbolicConditional>(
@ -104,7 +104,7 @@ TEST(SymbolicFactorGraphOrdered, eliminatePartialMultifrontal)
}
/* ************************************************************************* */
TEST(SymbolicFactorGraphOrdered, marginalMultifrontalBayesNet)
TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet)
{
SymbolicBayesNet expectedBayesNet = list_of
(SymbolicConditional(0, 1, 2))
@ -118,7 +118,7 @@ TEST(SymbolicFactorGraphOrdered, marginalMultifrontalBayesNet)
}
/* ************************************************************************* */
TEST(SymbolicFactorGraphOrdered, eliminate_disconnected_graph) {
TEST(SymbolicFactorGraph, eliminate_disconnected_graph) {
SymbolicFactorGraph fg;
fg.push_factor(0, 1);
fg.push_factor(0, 2);
@ -141,10 +141,10 @@ TEST(SymbolicFactorGraphOrdered, eliminate_disconnected_graph) {
}
/* ************************************************************************* */
//TEST(SymbolicFactorGraphOrdered, marginals)
//TEST(SymbolicFactorGraph, marginals)
//{
// // Create factor graph
// SymbolicFactorGraphOrdered fg;
// SymbolicFactorGraph fg;
// fg.push_factor(0, 1);
// fg.push_factor(0, 2);
// fg.push_factor(1, 4);
@ -176,12 +176,12 @@ TEST(SymbolicFactorGraphOrdered, eliminate_disconnected_graph) {
// EXPECT( assert_equal(expectedBN,*actualBN));
//
// // jointFactorGraph
// SymbolicFactorGraphOrdered::shared_ptr actualFG = solver.jointFactorGraph(js);
// SymbolicFactorGraphOrdered expectedFG;
// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js);
// SymbolicFactorGraph expectedFG;
// expectedFG.push_factor(0, 4);
// expectedFG.push_factor(4, 3);
// expectedFG.push_factor(3);
// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraphOrdered)(*actualFG)));
// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG)));
// }
//
// {
@ -198,12 +198,12 @@ TEST(SymbolicFactorGraphOrdered, eliminate_disconnected_graph) {
// EXPECT( assert_equal(expectedBN,*actualBN));
//
// // jointFactorGraph
// SymbolicFactorGraphOrdered::shared_ptr actualFG = solver.jointFactorGraph(js);
// SymbolicFactorGraphOrdered expectedFG;
// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js);
// SymbolicFactorGraph expectedFG;
// expectedFG.push_factor(0, 3, 2);
// expectedFG.push_factor(3, 2);
// expectedFG.push_factor(2);
// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraphOrdered)(*actualFG)));
// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG)));
// }
//
// {
@ -223,7 +223,7 @@ TEST(SymbolicFactorGraphOrdered, eliminate_disconnected_graph) {
//}
/* ************************************************************************* */
TEST( SymbolicFactorGraphOrdered, constructFromBayesNet )
TEST( SymbolicFactorGraph, constructFromBayesNet )
{
// create expected factor graph
SymbolicFactorGraph expected;
@ -244,7 +244,7 @@ TEST( SymbolicFactorGraphOrdered, constructFromBayesNet )
}
/* ************************************************************************* */
TEST( SymbolicFactorGraphOrdered, constructFromBayesTree )
TEST( SymbolicFactorGraph, constructFromBayesTree )
{
// create expected factor graph
SymbolicFactorGraph expected;
@ -259,7 +259,7 @@ TEST( SymbolicFactorGraphOrdered, constructFromBayesTree )
}
/* ************************************************************************* */
TEST( SymbolicFactorGraphOrdered, push_back )
TEST( SymbolicFactorGraph, push_back )
{
// Create two factor graphs and expected combined graph
SymbolicFactorGraph fg1, fg2, expected;

2
gtsam/symbolic/tests/testSymbolicJunctionTree.cpp
View File

@ -38,7 +38,7 @@ using namespace std;
* 2 3
* 0 1 : 2
****************************************************************************/
TEST( JunctionTreeOrdered, constructor )
TEST( JunctionTree, constructor )
{
Ordering order; order += 0, 1, 2, 3;

44
gtsam_unstable/linear/bayesTreeOperations.cpp
View File

@ -14,7 +14,7 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
std::set<Index> keysToIndices(const KeySet& keys, const OrderingOrdered& ordering) {
std::set<Index> keysToIndices(const KeySet& keys, const Ordering& ordering) {
std::set<Index> result;
BOOST_FOREACH(const Key& key, keys)
result.insert(ordering[key]);
@ -22,22 +22,22 @@ std::set<Index> keysToIndices(const KeySet& keys, const OrderingOrdered& orderin
}
/* ************************************************************************* */
GaussianFactorGraphOrdered splitFactors(const GaussianFactorGraphOrdered& fullgraph) {
GaussianFactorGraphOrdered result;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, fullgraph) {
GaussianFactorGraphOrdered split = splitFactor(factor);
GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph) {
GaussianFactorGraph result;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fullgraph) {
GaussianFactorGraph split = splitFactor(factor);
result.push_back(split);
}
return result;
}
/* ************************************************************************* */
GaussianFactorGraphOrdered splitFactor(const GaussianFactorOrdered::shared_ptr& factor) {
GaussianFactorGraphOrdered result;
GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor) {
GaussianFactorGraph result;
if (!factor) return result;
// Needs to be a jacobian factor - just pass along hessians
JacobianFactorOrdered::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor);
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
if (!jf) {
result.push_back(factor);
return result;
@ -45,7 +45,7 @@ GaussianFactorGraphOrdered splitFactor(const GaussianFactorOrdered::shared_ptr&
// Loop over variables and strip off factors using split conditionals
// Assumes upper triangular structure
JacobianFactorOrdered::const_iterator rowIt, colIt;
JacobianFactor::const_iterator rowIt, colIt;
const size_t totalRows = jf->rows();
size_t rowsRemaining = totalRows;
for (rowIt = jf->begin(); rowIt != jf->end() && rowsRemaining > 0; ++rowIt) {
@ -80,10 +80,10 @@ GaussianFactorGraphOrdered splitFactor(const GaussianFactorOrdered::shared_ptr&
}
/* ************************************************************************* */
GaussianFactorGraphOrdered removePriors(const GaussianFactorGraphOrdered& fullgraph) {
GaussianFactorGraphOrdered result;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, fullgraph) {
JacobianFactorOrdered::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactorOrdered>(factor);
GaussianFactorGraph removePriors(const GaussianFactorGraph& fullgraph) {
GaussianFactorGraph result;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fullgraph) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(factor);
if (factor && (!jf || jf->size() > 1))
result.push_back(factor);
}
@ -91,8 +91,8 @@ GaussianFactorGraphOrdered removePriors(const GaussianFactorGraphOrdered& fullgr
}
/* ************************************************************************* */
void findCliques(const GaussianBayesTreeOrdered::sharedClique& current_clique,
std::set<GaussianConditionalOrdered::shared_ptr>& result) {
void findCliques(const GaussianBayesTree::sharedClique& current_clique,
std::set<GaussianConditional::shared_ptr>& result) {
// Add the current clique
result.insert(current_clique->conditional());
@ -102,12 +102,12 @@ void findCliques(const GaussianBayesTreeOrdered::sharedClique& current_clique,
}
/* ************************************************************************* */
std::set<GaussianConditionalOrdered::shared_ptr> findAffectedCliqueConditionals(
const GaussianBayesTreeOrdered& bayesTree, const std::set<Index>& savedIndices) {
std::set<GaussianConditionalOrdered::shared_ptr> affected_cliques;
std::set<GaussianConditional::shared_ptr> findAffectedCliqueConditionals(
const GaussianBayesTree& bayesTree, const std::set<Index>& savedIndices) {
std::set<GaussianConditional::shared_ptr> affected_cliques;
// FIXME: track previously found keys more efficiently
BOOST_FOREACH(const Index& index, savedIndices) {
GaussianBayesTreeOrdered::sharedClique clique = bayesTree.nodes()[index];
GaussianBayesTree::sharedClique clique = bayesTree.nodes()[index];
// add path back to root to affected set
findCliques(clique, affected_cliques);
@ -116,9 +116,9 @@ std::set<GaussianConditionalOrdered::shared_ptr> findAffectedCliqueConditionals(
}
/* ************************************************************************* */
std::deque<GaussianBayesTreeOrdered::sharedClique>
findPathCliques(const GaussianBayesTreeOrdered::sharedClique& initial) {
std::deque<GaussianBayesTreeOrdered::sharedClique> result, parents;
std::deque<GaussianBayesTree::sharedClique>
findPathCliques(const GaussianBayesTree::sharedClique& initial) {
std::deque<GaussianBayesTree::sharedClique> result, parents;
if (initial->isRoot())
return result;
result.push_back(initial->parent());

28
gtsam_unstable/linear/bayesTreeOperations.h
View File

@ -19,7 +19,7 @@ namespace gtsam {
// Managing orderings
/** Converts sets of keys to indices by way of orderings */
GTSAM_UNSTABLE_EXPORT std::set<Index> keysToIndices(const KeySet& keys, const OrderingOrdered& ordering);
GTSAM_UNSTABLE_EXPORT std::set<Index> keysToIndices(const KeySet& keys, const Ordering& ordering);
// Linear Graph Operations
@ -27,16 +27,16 @@ GTSAM_UNSTABLE_EXPORT std::set<Index> keysToIndices(const KeySet& keys, const Or
* Given a graph, splits each factor into factors where the dimension is
* that of the first variable.
*/
GTSAM_UNSTABLE_EXPORT GaussianFactorGraphOrdered splitFactors(const GaussianFactorGraphOrdered& fullgraph);
GTSAM_UNSTABLE_EXPORT GaussianFactorGraph splitFactors(const GaussianFactorGraph& fullgraph);
/**
* Splits a factor into factors where the dimension is
* that of the first variable.
*/
GTSAM_UNSTABLE_EXPORT GaussianFactorGraphOrdered splitFactor(const GaussianFactorOrdered::shared_ptr& factor);
GTSAM_UNSTABLE_EXPORT GaussianFactorGraph splitFactor(const GaussianFactor::shared_ptr& factor);
/** Removes prior jacobian factors from the graph */
GTSAM_UNSTABLE_EXPORT GaussianFactorGraphOrdered removePriors(const GaussianFactorGraphOrdered& fullgraph);
GTSAM_UNSTABLE_EXPORT GaussianFactorGraph removePriors(const GaussianFactorGraph& fullgraph);
// Bayes Tree / Conditional operations
@ -46,8 +46,8 @@ GTSAM_UNSTABLE_EXPORT GaussianFactorGraphOrdered removePriors(const GaussianFact
*
* @return the set of conditionals extracted from cliques.
*/
GTSAM_UNSTABLE_EXPORT std::set<GaussianConditionalOrdered::shared_ptr> findAffectedCliqueConditionals(
const GaussianBayesTreeOrdered& bayesTree, const std::set<Index>& savedIndices);
GTSAM_UNSTABLE_EXPORT std::set<GaussianConditional::shared_ptr> findAffectedCliqueConditionals(
const GaussianBayesTree& bayesTree, const std::set<Index>& savedIndices);
/**
* Recursively traverses from a given clique in a Bayes Tree and collects all of the conditionals
@ -56,15 +56,15 @@ GTSAM_UNSTABLE_EXPORT std::set<GaussianConditionalOrdered::shared_ptr> findAffec
* Note the use of a set of shared_ptr: this will sort/filter on unique *pointer* locations,
* which ensures unique cliques, but the order of the cliques is meaningless
*/
GTSAM_UNSTABLE_EXPORT void findCliqueConditionals(const GaussianBayesTreeOrdered::sharedClique& current_clique,
std::set<GaussianConditionalOrdered::shared_ptr>& result);
GTSAM_UNSTABLE_EXPORT void findCliqueConditionals(const GaussianBayesTree::sharedClique& current_clique,
std::set<GaussianConditional::shared_ptr>& result);
/**
* Given a clique, returns a sequence of clique parents to the root, not including the
* given clique.
*/
GTSAM_UNSTABLE_EXPORT std::deque<GaussianBayesTreeOrdered::sharedClique>
findPathCliques(const GaussianBayesTreeOrdered::sharedClique& initial);
GTSAM_UNSTABLE_EXPORT std::deque<GaussianBayesTree::sharedClique>
findPathCliques(const GaussianBayesTree::sharedClique& initial);
/**
* Liquefies a BayesTree into a GaussianFactorGraph recursively, given a
@ -73,10 +73,10 @@ findPathCliques(const GaussianBayesTreeOrdered::sharedClique& initial);
* @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors
*/
template <class BAYESTREE>
GaussianFactorGraphOrdered liquefy(const typename BAYESTREE::sharedClique& root, bool splitConditionals = false) {
GaussianFactorGraphOrdered result;
GaussianFactorGraph liquefy(const typename BAYESTREE::sharedClique& root, bool splitConditionals = false) {
GaussianFactorGraph result;
if (root && root->conditional()) {
GaussianConditionalOrdered::shared_ptr conditional = root->conditional();
GaussianConditional::shared_ptr conditional = root->conditional();
if (!splitConditionals)
result.push_back(conditional->toFactor());
else
@ -93,7 +93,7 @@ GaussianFactorGraphOrdered liquefy(const typename BAYESTREE::sharedClique& root,
* @param splitConditionals flag enables spliting multi-frontal conditionals into separate factors
*/
template <class BAYESTREE>
GaussianFactorGraphOrdered liquefy(const BAYESTREE& bayesTree, bool splitConditionals = false) {
GaussianFactorGraph liquefy(const BAYESTREE& bayesTree, bool splitConditionals = false) {
return liquefy<BAYESTREE>(bayesTree.root(), splitConditionals);
}

30
gtsam_unstable/linear/conditioning.cpp
View File

@ -13,8 +13,8 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
GaussianConditionalOrdered::shared_ptr conditionDensity(const GaussianConditionalOrdered::shared_ptr& initConditional,
const std::set<Index>& saved_indices, const VectorValuesOrdered& solution) {
GaussianConditional::shared_ptr conditionDensity(const GaussianConditional::shared_ptr& initConditional,
const std::set<Index>& saved_indices, const VectorValues& solution) {
const bool verbose = false;
if (!initConditional)
@ -41,7 +41,7 @@ GaussianConditionalOrdered::shared_ptr conditionDensity(const GaussianConditiona
// If none of the frontal variables are to be saved, return empty pointer
if (frontalsToRemove.size() == initConditional->nrFrontals())
return GaussianConditionalOrdered::shared_ptr();
return GaussianConditional::shared_ptr();
// Collect dimensions of the new conditional
if (verbose) cout << " Collecting dimensions" << endl;
@ -54,7 +54,7 @@ GaussianConditionalOrdered::shared_ptr conditionDensity(const GaussianConditiona
vector<size_t> newIdxToOldIdx; // Access to arrays, maps from new var to old var
const vector<Index>& oldIndices = initConditional->keys();
const size_t oldNrFrontals = initConditional->nrFrontals();
GaussianConditionalOrdered::const_iterator varIt = initConditional->beginFrontals();
GaussianConditional::const_iterator varIt = initConditional->beginFrontals();
size_t oldIdx = 0;
for (; varIt != initConditional->endFrontals(); ++varIt) {
size_t varDim = initConditional->dim(varIt);
@ -99,7 +99,7 @@ GaussianConditionalOrdered::shared_ptr conditionDensity(const GaussianConditiona
size_t oldColOffset = oldColOffsets.at(newfrontalIdx);
// get R block, sliced by row
Eigen::Block<GaussianConditionalOrdered::rsd_type::constBlock> rblock =
Eigen::Block<GaussianConditional::rsd_type::constBlock> rblock =
initConditional->get_R().block(oldColOffset, oldColOffset, dim, oldRNrCols-oldColOffset);
if (verbose) cout << " rblock\n" << rblock << endl;
@ -142,7 +142,7 @@ GaussianConditionalOrdered::shared_ptr conditionDensity(const GaussianConditiona
}
// add parents (looping over original block structure), while updating rhs
GaussianConditionalOrdered::const_iterator oldParent = initConditional->beginParents();
GaussianConditional::const_iterator oldParent = initConditional->beginParents();
for (; oldParent != initConditional->endParents(); ++oldParent) {
Index parentKey = *oldParent;
size_t parentDim = initConditional->dim(oldParent);
@ -169,26 +169,26 @@ GaussianConditionalOrdered::shared_ptr conditionDensity(const GaussianConditiona
// Construct a new conditional
if (verbose) cout << "backsubSummarize() Complete!" << endl;
GaussianConditionalOrdered::rsd_type matrices(full_matrix, newDims.begin(), newDims.end());
return GaussianConditionalOrdered::shared_ptr(new
GaussianConditionalOrdered(newIndices.begin(), newIndices.end(), newNrFrontals, matrices, sigmas));
GaussianConditional::rsd_type matrices(full_matrix, newDims.begin(), newDims.end());
return GaussianConditional::shared_ptr(new
GaussianConditional(newIndices.begin(), newIndices.end(), newNrFrontals, matrices, sigmas));
}
/* ************************************************************************* */
GaussianFactorGraphOrdered conditionDensity(const GaussianBayesTreeOrdered& bayesTree,
GaussianFactorGraph conditionDensity(const GaussianBayesTree& bayesTree,
const std::set<Index>& saved_indices) {
const bool verbose = false;
VectorValuesOrdered solution = optimize(bayesTree);
VectorValues solution = optimize(bayesTree);
// FIXME: set of conditionals does not manage possibility of solving out whole separators
std::set<GaussianConditionalOrdered::shared_ptr> affected_cliques = findAffectedCliqueConditionals(bayesTree, saved_indices);
std::set<GaussianConditional::shared_ptr> affected_cliques = findAffectedCliqueConditionals(bayesTree, saved_indices);
// Summarize conditionals separately
GaussianFactorGraphOrdered summarized_graph;
BOOST_FOREACH(const GaussianConditionalOrdered::shared_ptr& conditional, affected_cliques) {
GaussianFactorGraph summarized_graph;
BOOST_FOREACH(const GaussianConditional::shared_ptr& conditional, affected_cliques) {
if (verbose) conditional->print("Initial conditional");
GaussianConditionalOrdered::shared_ptr reducedConditional = conditionDensity(conditional, saved_indices, solution);
GaussianConditional::shared_ptr reducedConditional = conditionDensity(conditional, saved_indices, solution);
if (reducedConditional) {
if (verbose) reducedConditional->print("Final conditional");
summarized_graph.push_back(reducedConditional->toFactor());

6
gtsam_unstable/linear/conditioning.h
View File

@ -27,15 +27,15 @@ namespace gtsam {
* @param saved_indices is the set of indices that should appear in the result
* @param solution is a full solution for the system
*/
GTSAM_UNSTABLE_EXPORT gtsam::GaussianConditionalOrdered::shared_ptr conditionDensity(const gtsam::GaussianConditionalOrdered::shared_ptr& initConditional,
const std::set<gtsam::Index>& saved_indices, const gtsam::VectorValuesOrdered& solution);
GTSAM_UNSTABLE_EXPORT gtsam::GaussianConditional::shared_ptr conditionDensity(const gtsam::GaussianConditional::shared_ptr& initConditional,
const std::set<gtsam::Index>& saved_indices, const gtsam::VectorValues& solution);
/**
* Backsubstitution-based conditioning for a complete Bayes Tree - reduces
* conditionals by solving out variables to eliminate. Traverses the tree to
* add the correct dummy factors whenever a separator is eliminated.
*/
GTSAM_UNSTABLE_EXPORT gtsam::GaussianFactorGraphOrdered conditionDensity(const gtsam::GaussianBayesTreeOrdered& bayesTree,
GTSAM_UNSTABLE_EXPORT gtsam::GaussianFactorGraph conditionDensity(const gtsam::GaussianBayesTree& bayesTree,
const std::set<gtsam::Index>& saved_indices);

36
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -189,7 +189,7 @@ void BatchFixedLagSmoother::eraseKeys(const std::set<Key>& keys) {
void BatchFixedLagSmoother::reorder(const std::set<Key>& marginalizeKeys) {
// Calculate a variable index
VariableIndexOrdered variableIndex(*factors_.symbolic(ordering_), ordering_.size());
VariableIndex variableIndex(*factors_.symbolic(ordering_), ordering_.size());
// COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
int group0 = 0;
@ -236,7 +236,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
// Use a custom optimization loop so the linearization points can be controlled
double previousError;
VectorValuesOrdered newDelta;
VectorValues newDelta;
do {
previousError = result.error;
@ -244,13 +244,13 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
gttic(optimizer_iteration);
{
// Linearize graph around the linearization point
GaussianFactorGraphOrdered linearFactorGraph = *factors_.linearize(theta_, ordering_);
GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_);
// Keep increasing lambda until we make make progress
while(true) {
// Add prior factors at the current solution
gttic(damp);
GaussianFactorGraphOrdered dampedFactorGraph(linearFactorGraph);
GaussianFactorGraph dampedFactorGraph(linearFactorGraph);
dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size());
{
// for each of the variables, add a prior at the current solution
@ -260,7 +260,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
Matrix A = eye(dim);
Vector b = delta_[j];
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
GaussianFactorOrdered::shared_ptr prior(new JacobianFactorOrdered(j, A, b, model));
GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model));
dampedFactorGraph.push_back(prior);
}
}
@ -269,7 +269,7 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
gttic(solve);
// Solve Damped Gaussian Factor Graph
newDelta = GaussianJunctionTreeOrdered(dampedFactorGraph).optimize(parameters_.getEliminationFunction());
newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters_.getEliminationFunction());
// update the evalpoint with the new delta
evalpoint = theta_.retract(newDelta, ordering_);
gttoc(solve);
@ -334,10 +334,10 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
// Calculate marginal factors on the remaining variables (after marginalizing 'marginalizeKeys')
// Note: It is assumed the ordering already has these keys first
// Create the linear factor graph
GaussianFactorGraphOrdered linearFactorGraph = *factors_.linearize(theta_, ordering_);
GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_);
// Create a variable index
VariableIndexOrdered variableIndex(linearFactorGraph, ordering_.size());
VariableIndex variableIndex(linearFactorGraph, ordering_.size());
// Use the variable Index to mark the factors that will be marginalized
std::set<size_t> removedFactorSlots;
@ -364,7 +364,7 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
// Add the marginal factor variables to the separator
NonlinearFactorGraph marginalFactors;
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactorOrdered::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction());
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction());
if(gaussianFactor->size() > 0) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_));
marginalFactors.push_back(marginalFactor);
@ -409,7 +409,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_pt
}
/* ************************************************************************* */
void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering) {
void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering) {
std::cout << "f(";
BOOST_FOREACH(Index index, factor->keys()) {
std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]";
@ -426,15 +426,15 @@ void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph
}
/* ************************************************************************* */
void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraphOrdered& graph, const OrderingOrdered& ordering, const std::string& label) {
void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label) {
std::cout << label << std::endl;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, graph) {
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) {
PrintSymbolicFactor(factor, ordering);
}
}
/* ************************************************************************* */
std::vector<Index> BatchFixedLagSmoother::EliminationForest::ComputeParents(const VariableIndexOrdered& structure) {
std::vector<Index> BatchFixedLagSmoother::EliminationForest::ComputeParents(const VariableIndex& structure) {
// Number of factors and variables
const size_t m = structure.nFactors();
const size_t n = structure.size();
@ -465,7 +465,7 @@ std::vector<Index> BatchFixedLagSmoother::EliminationForest::ComputeParents(cons
}
/* ************************************************************************* */
std::vector<BatchFixedLagSmoother::EliminationForest::shared_ptr> BatchFixedLagSmoother::EliminationForest::Create(const GaussianFactorGraphOrdered& factorGraph, const VariableIndexOrdered& structure) {
std::vector<BatchFixedLagSmoother::EliminationForest::shared_ptr> BatchFixedLagSmoother::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) {
// Compute the tree structure
std::vector<Index> parents(ComputeParents(structure));
@ -484,7 +484,7 @@ std::vector<BatchFixedLagSmoother::EliminationForest::shared_ptr> BatchFixedLagS
}
// Hang factors in right places
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factorGraph) {
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) {
if(factor && factor->size() > 0) {
Index j = *std::min_element(factor->begin(), factor->end());
if(j < structure.size())
@ -496,10 +496,10 @@ std::vector<BatchFixedLagSmoother::EliminationForest::shared_ptr> BatchFixedLagS
}
/* ************************************************************************* */
GaussianFactorOrdered::shared_ptr BatchFixedLagSmoother::EliminationForest::eliminateRecursive(GaussianFactorGraphOrdered::Eliminate function) {
GaussianFactor::shared_ptr BatchFixedLagSmoother::EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) {
// Create the list of factors to be eliminated, initially empty, and reserve space
GaussianFactorGraphOrdered factors;
GaussianFactorGraph factors;
factors.reserve(this->factors_.size() + this->subTrees_.size());
// Add all factors associated with the current node
@ -510,7 +510,7 @@ GaussianFactorOrdered::shared_ptr BatchFixedLagSmoother::EliminationForest::elim
factors.push_back(child->eliminateRecursive(function));
// Combine all factors (from this node and from subtrees) into a joint factor
GaussianFactorGraphOrdered::EliminationResult eliminated(function(factors, 1));
GaussianFactorGraph::EliminationResult eliminated(function(factors, 1));
return eliminated.second;
}

24
gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
View File

@ -92,12 +92,12 @@ public:
}
/** Access the current ordering */
const OrderingOrdered& getOrdering() const {
const Ordering& getOrdering() const {
return ordering_;
}
/** Access the current set of deltas to the linearization point */
const VectorValuesOrdered& getDelta() const {
const VectorValues& getDelta() const {
return delta_;
}
@ -124,10 +124,10 @@ protected:
Values linearKeys_;
/** The current ordering */
OrderingOrdered ordering_;
Ordering ordering_;
/** The current set of linear deltas */
VectorValuesOrdered delta_;
VectorValues delta_;
/** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/
std::queue<size_t> availableSlots_;
@ -162,9 +162,9 @@ protected:
typedef boost::shared_ptr<EliminationForest> shared_ptr; ///< Shared pointer to this class
private:
typedef FastList<GaussianFactorOrdered::shared_ptr> Factors;
typedef FastList<GaussianFactor::shared_ptr> Factors;
typedef FastList<shared_ptr> SubTrees;
typedef std::vector<GaussianConditionalOrdered::shared_ptr> Conditionals;
typedef std::vector<GaussianConditional::shared_ptr> Conditionals;
Index key_; ///< index associated with root
Factors factors_; ///< factors associated with root
@ -177,10 +177,10 @@ protected:
* Static internal function to build a vector of parent pointers using the
* algorithm of Gilbert et al., 2001, BIT.
*/
static std::vector<Index> ComputeParents(const VariableIndexOrdered& structure);
static std::vector<Index> ComputeParents(const VariableIndex& structure);
/** add a factor, for Create use only */
void add(const GaussianFactorOrdered::shared_ptr& factor) { factors_.push_back(factor); }
void add(const GaussianFactor::shared_ptr& factor) { factors_.push_back(factor); }
/** add a subtree, for Create use only */
void add(const shared_ptr& child) { subTrees_.push_back(child); }
@ -197,10 +197,10 @@ protected:
const Factors& factors() const { return factors_; }
/** Create an elimination tree from a factor graph */
static std::vector<shared_ptr> Create(const GaussianFactorGraphOrdered& factorGraph, const VariableIndexOrdered& structure);
static std::vector<shared_ptr> Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure);
/** Recursive routine that eliminates the factors arranged in an elimination tree */
GaussianFactorOrdered::shared_ptr eliminateRecursive(GaussianFactorGraphOrdered::Eliminate function);
GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function);
/** Recursive function that helps find the top of each tree */
static void removeChildrenIndices(std::set<Index>& indices, const EliminationForest::shared_ptr& tree);
@ -210,9 +210,9 @@ private:
/** Private methods for printing debug information */
static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor);
static void PrintSymbolicFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering);
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering);
static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label);
static void PrintSymbolicGraph(const GaussianFactorGraphOrdered& graph, const OrderingOrdered& ordering, const std::string& label);
static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const Ordering& ordering, const std::string& label);
}; // BatchFixedLagSmoother
} /// namespace gtsam

54
gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp
View File

@ -128,8 +128,8 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFa
// Perform an optional optimization on the to-be-sent-to-the-smoother factors
if(relin_) {
// Create ordering and delta
OrderingOrdered ordering = *graph.orderingCOLAMD(values);
VectorValuesOrdered delta = values.zeroVectors(ordering);
Ordering ordering = *graph.orderingCOLAMD(values);
VectorValues delta = values.zeroVectors(ordering);
// Optimize this graph using a modified version of L-M
optimize(graph, values, ordering, delta, separatorValues, parameters_);
// Update filter theta and delta
@ -162,8 +162,8 @@ void ConcurrentBatchFilter::synchronize(const NonlinearFactorGraph& summarizedFa
// Generate separate orderings that place the filter keys or the smoother keys first
// TODO: This is convenient, but it recalculates the variable index each time
OrderingOrdered filterOrdering = *graph.orderingCOLAMDConstrained(values, filterConstraints);
OrderingOrdered smootherOrdering = *graph.orderingCOLAMDConstrained(values, smootherConstraints);
Ordering filterOrdering = *graph.orderingCOLAMDConstrained(values, filterConstraints);
Ordering smootherOrdering = *graph.orderingCOLAMDConstrained(values, smootherConstraints);
// Extract the set of filter keys and smoother keys
std::set<Key> filterKeys;
@ -276,7 +276,7 @@ void ConcurrentBatchFilter::removeFactors(const std::vector<size_t>& slots) {
gttic(remove_factors);
// For each factor slot to delete...
SymbolicFactorGraphOrdered factors;
SymbolicFactorGraph factors;
BOOST_FOREACH(size_t slot, slots) {
// Create a symbolic version for the variable index
factors.push_back(factors_.at(slot)->symbolic(ordering_));
@ -295,7 +295,7 @@ void ConcurrentBatchFilter::removeFactors(const std::vector<size_t>& slots) {
void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysToMove) {
// Calculate the variable index
VariableIndexOrdered variableIndex(*factors_.symbolic(ordering_), ordering_.size());
VariableIndex variableIndex(*factors_.symbolic(ordering_), ordering_.size());
// COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
int group0 = 0;
@ -320,8 +320,8 @@ void ConcurrentBatchFilter::reorder(const boost::optional<FastList<Key> >& keysT
}
/* ************************************************************************* */
ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values& theta, const OrderingOrdered& ordering,
VectorValuesOrdered& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters) {
ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering,
VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters) {
// Create output result structure
Result result;
@ -344,7 +344,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac
// Use a custom optimization loop so the linearization points can be controlled
double previousError;
VectorValuesOrdered newDelta;
VectorValues newDelta;
do {
previousError = result.error;
@ -352,13 +352,13 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac
gttic(optimizer_iteration);
{
// Linearize graph around the linearization point
GaussianFactorGraphOrdered linearFactorGraph = *factors.linearize(theta, ordering);
GaussianFactorGraph linearFactorGraph = *factors.linearize(theta, ordering);
// Keep increasing lambda until we make make progress
while(true) {
// Add prior factors at the current solution
gttic(damp);
GaussianFactorGraphOrdered dampedFactorGraph(linearFactorGraph);
GaussianFactorGraph dampedFactorGraph(linearFactorGraph);
dampedFactorGraph.reserve(linearFactorGraph.size() + delta.size());
{
// for each of the variables, add a prior at the current solution
@ -366,7 +366,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac
Matrix A = lambda * eye(delta[j].size());
Vector b = lambda * delta[j];
SharedDiagonal model = noiseModel::Unit::Create(delta[j].size());
GaussianFactorOrdered::shared_ptr prior(new JacobianFactorOrdered(j, A, b, model));
GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model));
dampedFactorGraph.push_back(prior);
}
}
@ -375,7 +375,7 @@ ConcurrentBatchFilter::Result ConcurrentBatchFilter::optimize(const NonlinearFac
gttic(solve);
// Solve Damped Gaussian Factor Graph
newDelta = GaussianJunctionTreeOrdered(dampedFactorGraph).optimize(parameters.getEliminationFunction());
newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters.getEliminationFunction());
// update the evalpoint with the new delta
evalpoint = theta.retract(newDelta, ordering);
gttoc(solve);
@ -442,10 +442,10 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
// Note: It is assumed the ordering already has these keys first
// Create the linear factor graph
GaussianFactorGraphOrdered linearFactorGraph = *factors_.linearize(theta_, ordering_);
GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_);
// Calculate the variable index
VariableIndexOrdered variableIndex(linearFactorGraph, ordering_.size());
VariableIndex variableIndex(linearFactorGraph, ordering_.size());
// Use the variable Index to mark the factors that will be marginalized
std::set<size_t> removedFactorSlots;
@ -472,7 +472,7 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
// Add the marginal factor variables to the separator
NonlinearFactorGraph marginalFactors;
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactorOrdered::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction());
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction());
if(gaussianFactor->size() > 0) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_));
marginalFactors.push_back(marginalFactor);
@ -545,16 +545,16 @@ void ConcurrentBatchFilter::marginalize(const FastList<Key>& keysToMove) {
/* ************************************************************************* */
NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGraph& graph, const Values& values,
const OrderingOrdered& ordering, const std::set<Key>& marginalizeKeys, const GaussianFactorGraphOrdered::Eliminate& function) {
const Ordering& ordering, const std::set<Key>& marginalizeKeys, const GaussianFactorGraph::Eliminate& function) {
// Calculate marginal factors on the remaining variables (after marginalizing 'marginalizeKeys')
// Note: It is assumed the ordering already has these keys first
// Create the linear factor graph
GaussianFactorGraphOrdered linearFactorGraph = *graph.linearize(values, ordering);
GaussianFactorGraph linearFactorGraph = *graph.linearize(values, ordering);
// Construct a variable index
VariableIndexOrdered variableIndex(linearFactorGraph, ordering.size());
VariableIndex variableIndex(linearFactorGraph, ordering.size());
// Construct an elimination tree to perform sparse elimination
std::vector<EliminationForest::shared_ptr> forest( EliminationForest::Create(linearFactorGraph, variableIndex) );
@ -574,7 +574,7 @@ NonlinearFactorGraph ConcurrentBatchFilter::marginalize(const NonlinearFactorGra
// Add the marginal factor variables to the separator
NonlinearFactorGraph marginalFactors;
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactorOrdered::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(function);
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(function);
if(gaussianFactor->size() > 0) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering, values));
marginalFactors.push_back(marginalFactor);
@ -604,7 +604,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p
}
/* ************************************************************************* */
void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering,
void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering,
const std::string& indent, const KeyFormatter& keyFormatter) {
std::cout << indent;
if(factor) {
@ -619,7 +619,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactorOrdered::share
}
/* ************************************************************************* */
std::vector<Index> ConcurrentBatchFilter::EliminationForest::ComputeParents(const VariableIndexOrdered& structure) {
std::vector<Index> ConcurrentBatchFilter::EliminationForest::ComputeParents(const VariableIndex& structure) {
// Number of factors and variables
const size_t m = structure.nFactors();
const size_t n = structure.size();
@ -650,7 +650,7 @@ std::vector<Index> ConcurrentBatchFilter::EliminationForest::ComputeParents(cons
}
/* ************************************************************************* */
std::vector<ConcurrentBatchFilter::EliminationForest::shared_ptr> ConcurrentBatchFilter::EliminationForest::Create(const GaussianFactorGraphOrdered& factorGraph, const VariableIndexOrdered& structure) {
std::vector<ConcurrentBatchFilter::EliminationForest::shared_ptr> ConcurrentBatchFilter::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) {
// Compute the tree structure
std::vector<Index> parents(ComputeParents(structure));
@ -669,7 +669,7 @@ std::vector<ConcurrentBatchFilter::EliminationForest::shared_ptr> ConcurrentBatc
}
// Hang factors in right places
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factorGraph) {
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) {
if(factor && factor->size() > 0) {
Index j = *std::min_element(factor->begin(), factor->end());
if(j < structure.size())
@ -681,10 +681,10 @@ std::vector<ConcurrentBatchFilter::EliminationForest::shared_ptr> ConcurrentBatc
}
/* ************************************************************************* */
GaussianFactorOrdered::shared_ptr ConcurrentBatchFilter::EliminationForest::eliminateRecursive(GaussianFactorGraphOrdered::Eliminate function) {
GaussianFactor::shared_ptr ConcurrentBatchFilter::EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) {
// Create the list of factors to be eliminated, initially empty, and reserve space
GaussianFactorGraphOrdered factors;
GaussianFactorGraph factors;
factors.reserve(this->factors_.size() + this->subTrees_.size());
// Add all factors associated with the current node
@ -695,7 +695,7 @@ GaussianFactorOrdered::shared_ptr ConcurrentBatchFilter::EliminationForest::elim
factors.push_back(child->eliminateRecursive(function));
// Combine all factors (from this node and from subtrees) into a joint factor
GaussianFactorGraphOrdered::EliminationResult eliminated(function(factors, 1));
GaussianFactorGraph::EliminationResult eliminated(function(factors, 1));
return eliminated.second;
}

28
gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
View File

@ -76,12 +76,12 @@ public:
}
/** Access the current ordering */
const OrderingOrdered& getOrdering() const {
const Ordering& getOrdering() const {
return ordering_;
}
/** Access the current set of deltas to the linearization point */
const VectorValuesOrdered& getDelta() const {
const VectorValues& getDelta() const {
return delta_;
}
@ -125,8 +125,8 @@ protected:
bool relin_;
NonlinearFactorGraph factors_; ///< The set of all factors currently in the filter
Values theta_; ///< Current linearization point of all variables in the filter
OrderingOrdered ordering_; ///< The current ordering used to calculate the linear deltas
VectorValuesOrdered delta_; ///< The current set of linear deltas from the linearization point
Ordering ordering_; ///< The current ordering used to calculate the linear deltas
VectorValues delta_; ///< The current set of linear deltas from the linearization point
std::queue<size_t> availableSlots_; ///< The set of available factor graph slots caused by deleting factors
Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization.
std::vector<size_t> smootherSummarizationSlots_; ///< The slots in factor graph that correspond to the current smoother summarization factors
@ -194,8 +194,8 @@ private:
void reorder(const boost::optional<FastList<Key> >& keysToMove = boost::none);
/** Use a modified version of L-M to update the linearization point and delta */
static Result optimize(const NonlinearFactorGraph& factors, Values& theta, const OrderingOrdered& ordering,
VectorValuesOrdered& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters);
static Result optimize(const NonlinearFactorGraph& factors, Values& theta, const Ordering& ordering,
VectorValues& delta, const Values& linearValues, const LevenbergMarquardtParams& parameters);
/** Marginalize out the set of requested variables from the filter, caching them for the smoother
* This effectively moves the separator.
@ -206,14 +206,14 @@ private:
* This effectively moves the separator.
*/
static NonlinearFactorGraph marginalize(const NonlinearFactorGraph& graph, const Values& values,
const OrderingOrdered& ordering, const std::set<Key>& marginalizeKeys, const GaussianFactorGraphOrdered::Eliminate& function = EliminateQROrdered);
const Ordering& ordering, const std::set<Key>& marginalizeKeys, const GaussianFactorGraph::Eliminate& function = EliminateQR);
/** Print just the nonlinear keys in a nonlinear factor */
static void PrintNonlinearFactor(const NonlinearFactor::shared_ptr& factor,
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/** Print just the nonlinear keys in a linear factor */
static void PrintLinearFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering,
static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering,
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
// A custom elimination tree that supports forests and partial elimination
@ -222,9 +222,9 @@ private:
typedef boost::shared_ptr<EliminationForest> shared_ptr; ///< Shared pointer to this class
private:
typedef FastList<GaussianFactorOrdered::shared_ptr> Factors;
typedef FastList<GaussianFactor::shared_ptr> Factors;
typedef FastList<shared_ptr> SubTrees;
typedef std::vector<GaussianConditionalOrdered::shared_ptr> Conditionals;
typedef std::vector<GaussianConditional::shared_ptr> Conditionals;
Index key_; ///< index associated with root
Factors factors_; ///< factors associated with root
@ -237,10 +237,10 @@ private:
* Static internal function to build a vector of parent pointers using the
* algorithm of Gilbert et al., 2001, BIT.
*/
static std::vector<Index> ComputeParents(const VariableIndexOrdered& structure);
static std::vector<Index> ComputeParents(const VariableIndex& structure);
/** add a factor, for Create use only */
void add(const GaussianFactorOrdered::shared_ptr& factor) { factors_.push_back(factor); }
void add(const GaussianFactor::shared_ptr& factor) { factors_.push_back(factor); }
/** add a subtree, for Create use only */
void add(const shared_ptr& child) { subTrees_.push_back(child); }
@ -257,10 +257,10 @@ private:
const Factors& factors() const { return factors_; }
/** Create an elimination tree from a factor graph */
static std::vector<shared_ptr> Create(const GaussianFactorGraphOrdered& factorGraph, const VariableIndexOrdered& structure);
static std::vector<shared_ptr> Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure);
/** Recursive routine that eliminates the factors arranged in an elimination tree */
GaussianFactorOrdered::shared_ptr eliminateRecursive(GaussianFactorGraphOrdered::Eliminate function);
GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function);
/** Recursive function that helps find the top of each tree */
static void removeChildrenIndices(std::set<Index>& indices, const EliminationForest::shared_ptr& tree);

32
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp
View File

@ -217,7 +217,7 @@ void ConcurrentBatchSmoother::removeFactors(const std::vector<size_t>& slots) {
gttic(remove_factors);
// For each factor slot to delete...
SymbolicFactorGraphOrdered factors;
SymbolicFactorGraph factors;
BOOST_FOREACH(size_t slot, slots) {
// Create a symbolic version for the variable index
factors.push_back(factors_.at(slot)->symbolic(ordering_));
@ -236,7 +236,7 @@ void ConcurrentBatchSmoother::removeFactors(const std::vector<size_t>& slots) {
void ConcurrentBatchSmoother::reorder() {
// Recalculate the variable index
variableIndex_ = VariableIndexOrdered(*factors_.symbolic(ordering_));
variableIndex_ = VariableIndex(*factors_.symbolic(ordering_));
// Initialize all variables to group0
std::vector<int> cmember(variableIndex_.size(), 0);
@ -281,7 +281,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
// Use a custom optimization loop so the linearization points can be controlled
double previousError;
VectorValuesOrdered newDelta;
VectorValues newDelta;
do {
previousError = result.error;
@ -289,13 +289,13 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
gttic(optimizer_iteration);
{
// Linearize graph around the linearization point
GaussianFactorGraphOrdered linearFactorGraph = *factors_.linearize(theta_, ordering_);
GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_);
// Keep increasing lambda until we make make progress
while(true) {
// Add prior factors at the current solution
gttic(damp);
GaussianFactorGraphOrdered dampedFactorGraph(linearFactorGraph);
GaussianFactorGraph dampedFactorGraph(linearFactorGraph);
dampedFactorGraph.reserve(linearFactorGraph.size() + delta_.size());
{
// for each of the variables, add a prior at the current solution
@ -303,7 +303,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
Matrix A = lambda * eye(delta_[j].size());
Vector b = lambda * delta_[j];
SharedDiagonal model = noiseModel::Unit::Create(delta_[j].size());
GaussianFactorOrdered::shared_ptr prior(new JacobianFactorOrdered(j, A, b, model));
GaussianFactor::shared_ptr prior(new JacobianFactor(j, A, b, model));
dampedFactorGraph.push_back(prior);
}
}
@ -312,7 +312,7 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() {
gttic(solve);
// Solve Damped Gaussian Factor Graph
newDelta = GaussianJunctionTreeOrdered(dampedFactorGraph).optimize(parameters_.getEliminationFunction());
newDelta = GaussianJunctionTree(dampedFactorGraph).optimize(parameters_.getEliminationFunction());
// update the evalpoint with the new delta
evalpoint = theta_.retract(newDelta, ordering_);
gttoc(solve);
@ -382,7 +382,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
reorder();
// Create the linear factor graph
GaussianFactorGraphOrdered linearFactorGraph = *factors_.linearize(theta_, ordering_);
GaussianFactorGraph linearFactorGraph = *factors_.linearize(theta_, ordering_);
// Construct an elimination tree to perform sparse elimination
std::vector<EliminationForest::shared_ptr> forest( EliminationForest::Create(linearFactorGraph, variableIndex_) );
@ -404,7 +404,7 @@ void ConcurrentBatchSmoother::updateSmootherSummarization() {
// Eliminate each top-most key, returning a Gaussian Factor on some of the remaining variables
// Convert the marginal factors into Linear Container Factors and store
BOOST_FOREACH(Index index, indicesToEliminate) {
GaussianFactorOrdered::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction());
GaussianFactor::shared_ptr gaussianFactor = forest.at(index)->eliminateRecursive(parameters_.getEliminationFunction());
if(gaussianFactor->size() > 0) {
LinearContainerFactor::shared_ptr marginalFactor(new LinearContainerFactor(gaussianFactor, ordering_, theta_));
smootherSummarization_.push_back(marginalFactor);
@ -432,7 +432,7 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared
}
/* ************************************************************************* */
void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering, const std::string& indent, const KeyFormatter& keyFormatter) {
void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering, const std::string& indent, const KeyFormatter& keyFormatter) {
std::cout << indent;
if(factor) {
std::cout << "g( ";
@ -446,7 +446,7 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactorOrdered::sha
}
/* ************************************************************************* */
std::vector<Index> ConcurrentBatchSmoother::EliminationForest::ComputeParents(const VariableIndexOrdered& structure) {
std::vector<Index> ConcurrentBatchSmoother::EliminationForest::ComputeParents(const VariableIndex& structure) {
// Number of factors and variables
const size_t m = structure.nFactors();
const size_t n = structure.size();
@ -477,7 +477,7 @@ std::vector<Index> ConcurrentBatchSmoother::EliminationForest::ComputeParents(co
}
/* ************************************************************************* */
std::vector<ConcurrentBatchSmoother::EliminationForest::shared_ptr> ConcurrentBatchSmoother::EliminationForest::Create(const GaussianFactorGraphOrdered& factorGraph, const VariableIndexOrdered& structure) {
std::vector<ConcurrentBatchSmoother::EliminationForest::shared_ptr> ConcurrentBatchSmoother::EliminationForest::Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure) {
// Compute the tree structure
std::vector<Index> parents(ComputeParents(structure));
@ -496,7 +496,7 @@ std::vector<ConcurrentBatchSmoother::EliminationForest::shared_ptr> ConcurrentBa
}
// Hang factors in right places
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, factorGraph) {
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorGraph) {
if(factor && factor->size() > 0) {
Index j = *std::min_element(factor->begin(), factor->end());
if(j < structure.size())
@ -508,10 +508,10 @@ std::vector<ConcurrentBatchSmoother::EliminationForest::shared_ptr> ConcurrentBa
}
/* ************************************************************************* */
GaussianFactorOrdered::shared_ptr ConcurrentBatchSmoother::EliminationForest::eliminateRecursive(GaussianFactorGraphOrdered::Eliminate function) {
GaussianFactor::shared_ptr ConcurrentBatchSmoother::EliminationForest::eliminateRecursive(GaussianFactorGraph::Eliminate function) {
// Create the list of factors to be eliminated, initially empty, and reserve space
GaussianFactorGraphOrdered factors;
GaussianFactorGraph factors;
factors.reserve(this->factors_.size() + this->subTrees_.size());
// Add all factors associated with the current node
@ -522,7 +522,7 @@ GaussianFactorOrdered::shared_ptr ConcurrentBatchSmoother::EliminationForest::el
factors.push_back(child->eliminateRecursive(function));
// Combine all factors (from this node and from subtrees) into a joint factor
GaussianFactorGraphOrdered::EliminationResult eliminated(function(factors, 1));
GaussianFactorGraph::EliminationResult eliminated(function(factors, 1));
return eliminated.second;
}

24
gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h
View File

@ -76,12 +76,12 @@ public:
}
/** Access the current ordering */
const OrderingOrdered& getOrdering() const {
const Ordering& getOrdering() const {
return ordering_;
}
/** Access the current set of deltas to the linearization point */
const VectorValuesOrdered& getDelta() const {
const VectorValues& getDelta() const {
return delta_;
}
@ -126,9 +126,9 @@ protected:
LevenbergMarquardtParams parameters_; ///< LM parameters
NonlinearFactorGraph factors_; ///< The set of all factors currently in the smoother
Values theta_; ///< Current linearization point of all variables in the smoother
OrderingOrdered ordering_; ///< The current ordering used to calculate the linear deltas
VectorValuesOrdered delta_; ///< The current set of linear deltas from the linearization point
VariableIndexOrdered variableIndex_; ///< The current variable index, which allows efficient factor lookup by variable
Ordering ordering_; ///< The current ordering used to calculate the linear deltas
VectorValues delta_; ///< The current set of linear deltas from the linearization point
VariableIndex variableIndex_; ///< The current variable index, which allows efficient factor lookup by variable
std::queue<size_t> availableSlots_; ///< The set of available factor graph slots caused by deleting factors
Values separatorValues_; ///< The linearization points of the separator variables. These should not be updated during optimization.
std::vector<size_t> filterSummarizationSlots_; ///< The slots in factor graph that correspond to the current filter summarization factors
@ -197,7 +197,7 @@ private:
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/** Print just the nonlinear keys in a linear factor */
static void PrintLinearFactor(const GaussianFactorOrdered::shared_ptr& factor, const OrderingOrdered& ordering,
static void PrintLinearFactor(const GaussianFactor::shared_ptr& factor, const Ordering& ordering,
const std::string& indent = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter);
// A custom elimination tree that supports forests and partial elimination
@ -206,9 +206,9 @@ private:
typedef boost::shared_ptr<EliminationForest> shared_ptr; ///< Shared pointer to this class
private:
typedef FastList<GaussianFactorOrdered::shared_ptr> Factors;
typedef FastList<GaussianFactor::shared_ptr> Factors;
typedef FastList<shared_ptr> SubTrees;
typedef std::vector<GaussianConditionalOrdered::shared_ptr> Conditionals;
typedef std::vector<GaussianConditional::shared_ptr> Conditionals;
Index key_; ///< index associated with root
Factors factors_; ///< factors associated with root
@ -221,10 +221,10 @@ private:
* Static internal function to build a vector of parent pointers using the
* algorithm of Gilbert et al., 2001, BIT.
*/
static std::vector<Index> ComputeParents(const VariableIndexOrdered& structure);
static std::vector<Index> ComputeParents(const VariableIndex& structure);
/** add a factor, for Create use only */
void add(const GaussianFactorOrdered::shared_ptr& factor) { factors_.push_back(factor); }
void add(const GaussianFactor::shared_ptr& factor) { factors_.push_back(factor); }
/** add a subtree, for Create use only */
void add(const shared_ptr& child) { subTrees_.push_back(child); }
@ -241,10 +241,10 @@ private:
const Factors& factors() const { return factors_; }
/** Create an elimination tree from a factor graph */
static std::vector<shared_ptr> Create(const GaussianFactorGraphOrdered& factorGraph, const VariableIndexOrdered& structure);
static std::vector<shared_ptr> Create(const GaussianFactorGraph& factorGraph, const VariableIndex& structure);
/** Recursive routine that eliminates the factors arranged in an elimination tree */
GaussianFactorOrdered::shared_ptr eliminateRecursive(GaussianFactorGraphOrdered::Eliminate function);
GaussianFactor::shared_ptr eliminateRecursive(GaussianFactorGraph::Eliminate function);
/** Recursive function that helps find the top of each tree */
static void removeChildrenIndices(std::set<Index>& indices, const EliminationForest::shared_ptr& tree);

10
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -25,7 +25,7 @@
namespace gtsam {
/* ************************************************************************* */
void recursiveMarkAffectedKeys(const Index& index, const ISAM2Clique::shared_ptr& clique, const OrderingOrdered& ordering, std::set<Key>& additionalKeys) {
void recursiveMarkAffectedKeys(const Index& index, const ISAM2Clique::shared_ptr& clique, const Ordering& ordering, std::set<Key>& additionalKeys) {
// Check if the separator keys of the current clique contain the specified key
if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), index) != clique->conditional()->endParents()) {
@ -184,7 +184,7 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, const s
}
/* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactorOrdered::shared_ptr& factor, const gtsam::OrderingOrdered& ordering) {
void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering) {
std::cout << "f(";
BOOST_FOREACH(Index index, factor->keys()) {
std::cout << " " << index << "[" << gtsam::DefaultKeyFormatter(ordering.key(index)) << "]";
@ -193,9 +193,9 @@ void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactorOrdere
}
/* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraphOrdered& graph, const gtsam::OrderingOrdered& ordering, const std::string& label) {
void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const std::string& label) {
std::cout << label << std::endl;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, graph) {
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) {
PrintSymbolicFactor(factor, ordering);
}
}
@ -210,7 +210,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, co
}
/* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::OrderingOrdered& ordering, const std::string indent) {
void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string indent) {
// Print the current clique
std::cout << indent << "P( ";

6
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
View File

@ -93,10 +93,10 @@ protected:
private:
/** Private methods for printing debug information */
static void PrintKeySet(const std::set<Key>& keys, const std::string& label = "Keys:");
static void PrintSymbolicFactor(const GaussianFactorOrdered::shared_ptr& factor, const gtsam::OrderingOrdered& ordering);
static void PrintSymbolicGraph(const GaussianFactorGraphOrdered& graph, const gtsam::OrderingOrdered& ordering, const std::string& label = "Factor Graph:");
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor, const gtsam::Ordering& ordering);
static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const gtsam::Ordering& ordering, const std::string& label = "Factor Graph:");
static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:");
static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::OrderingOrdered& ordering, const std::string indent = "");
static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const gtsam::Ordering& ordering, const std::string indent = "");
}; // IncrementalFixedLagSmoother

28
gtsam_unstable/nonlinear/LinearizedFactor.cpp
View File

@ -22,7 +22,7 @@
namespace gtsam {
/* ************************************************************************* */
LinearizedGaussianFactor::LinearizedGaussianFactor(const GaussianFactorOrdered::shared_ptr& gaussian, const OrderingOrdered& ordering, const Values& lin_points) {
LinearizedGaussianFactor::LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points) {
// Extract the keys and linearization points
BOOST_FOREACH(const Index& idx, gaussian->keys()) {
// find full symbol
@ -48,8 +48,8 @@ LinearizedJacobianFactor::LinearizedJacobianFactor() : Ab_(matrix_) {
}
/* ************************************************************************* */
LinearizedJacobianFactor::LinearizedJacobianFactor(const JacobianFactorOrdered::shared_ptr& jacobian,
const OrderingOrdered& ordering, const Values& lin_points)
LinearizedJacobianFactor::LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian,
const Ordering& ordering, const Values& lin_points)
: Base(jacobian, ordering, lin_points), Ab_(matrix_) {
// Get the Ab matrix from the Jacobian factor, with any covariance baked in
@ -58,7 +58,7 @@ LinearizedJacobianFactor::LinearizedJacobianFactor(const JacobianFactorOrdered::
// Create the dims array
size_t *dims = (size_t *)alloca(sizeof(size_t) * (jacobian->size() + 1));
size_t index = 0;
for(JacobianFactorOrdered::const_iterator iter = jacobian->begin(); iter != jacobian->end(); ++iter) {
for(JacobianFactor::const_iterator iter = jacobian->begin(); iter != jacobian->end(); ++iter) {
dims[index++] = jacobian->getDim(iter);
}
dims[index] = 1;
@ -109,8 +109,8 @@ double LinearizedJacobianFactor::error(const Values& c) const {
}
/* ************************************************************************* */
boost::shared_ptr<GaussianFactorOrdered>
LinearizedJacobianFactor::linearize(const Values& c, const OrderingOrdered& ordering) const {
boost::shared_ptr<GaussianFactor>
LinearizedJacobianFactor::linearize(const Values& c, const Ordering& ordering) const {
// Create the 'terms' data structure for the Jacobian constructor
std::vector<std::pair<Index, Matrix> > terms;
@ -121,7 +121,7 @@ LinearizedJacobianFactor::linearize(const Values& c, const OrderingOrdered& orde
// compute rhs
Vector b = -error_vector(c);
return boost::shared_ptr<GaussianFactorOrdered>(new JacobianFactorOrdered(terms, b, noiseModel::Unit::Create(dim())));
return boost::shared_ptr<GaussianFactor>(new JacobianFactor(terms, b, noiseModel::Unit::Create(dim())));
}
/* ************************************************************************* */
@ -150,8 +150,8 @@ LinearizedHessianFactor::LinearizedHessianFactor() : info_(matrix_) {
}
/* ************************************************************************* */
LinearizedHessianFactor::LinearizedHessianFactor(const HessianFactorOrdered::shared_ptr& hessian,
const OrderingOrdered& ordering, const Values& lin_points)
LinearizedHessianFactor::LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian,
const Ordering& ordering, const Values& lin_points)
: Base(hessian, ordering, lin_points), info_(matrix_) {
// Copy the augmented matrix holding G, g, and f
@ -160,7 +160,7 @@ LinearizedHessianFactor::LinearizedHessianFactor(const HessianFactorOrdered::sha
// Create the dims array
size_t *dims = (size_t*)alloca(sizeof(size_t)*(hessian->size() + 1));
size_t index = 0;
for(HessianFactorOrdered::const_iterator iter = hessian->begin(); iter != hessian->end(); ++iter) {
for(HessianFactor::const_iterator iter = hessian->begin(); iter != hessian->end(); ++iter) {
dims[index++] = hessian->getDim(iter);
}
dims[index] = 1;
@ -227,8 +227,8 @@ double LinearizedHessianFactor::error(const Values& c) const {
}
/* ************************************************************************* */
boost::shared_ptr<GaussianFactorOrdered>
LinearizedHessianFactor::linearize(const Values& c, const OrderingOrdered& ordering) const {
boost::shared_ptr<GaussianFactor>
LinearizedHessianFactor::linearize(const Values& c, const Ordering& ordering) const {
// Use the ordering to convert the keys into indices;
std::vector<Index> js;
@ -274,8 +274,8 @@ LinearizedHessianFactor::linearize(const Values& c, const OrderingOrdered& order
}
// Create a Hessian Factor from the modified info matrix
//return boost::shared_ptr<GaussianFactorOrdered>(new HessianFactorOrdered(js, newInfo));
return boost::shared_ptr<GaussianFactorOrdered>(new HessianFactorOrdered(js, Gs, gs, f));
//return boost::shared_ptr<GaussianFactor>(new HessianFactor(js, newInfo));
return boost::shared_ptr<GaussianFactor>(new HessianFactor(js, Gs, gs, f));
}
} // \namespace aspn

18
gtsam_unstable/nonlinear/LinearizedFactor.h
View File

@ -54,7 +54,7 @@ public:
* @param ordering: The full ordering used to linearize this factor
* @param lin_points: The linearization points for, at least, the variables used by this factor
*/
LinearizedGaussianFactor(const GaussianFactorOrdered::shared_ptr& gaussian, const OrderingOrdered& ordering, const Values& lin_points);
LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Ordering& ordering, const Values& lin_points);
virtual ~LinearizedGaussianFactor() {};
@ -114,8 +114,8 @@ public:
* @param ordering: The ordering used to linearize this factor
* @param lin_points: The linearization points for, at least, the variables used by this factor
*/
LinearizedJacobianFactor(const JacobianFactorOrdered::shared_ptr& jacobian,
const OrderingOrdered& ordering, const Values& lin_points);
LinearizedJacobianFactor(const JacobianFactor::shared_ptr& jacobian,
const Ordering& ordering, const Values& lin_points);
virtual ~LinearizedJacobianFactor() {}
@ -148,8 +148,8 @@ public:
* Reimplemented from NoiseModelFactor to directly copy out the
* matrices and only update the RHS b with an updated residual
*/
boost::shared_ptr<GaussianFactorOrdered> linearize(
const Values& c, const OrderingOrdered& ordering) const;
boost::shared_ptr<GaussianFactor> linearize(
const Values& c, const Ordering& ordering) const;
/** (A*x-b) */
Vector error_vector(const Values& c) const;
@ -207,8 +207,8 @@ public:
* @param ordering: The ordering used to linearize this factor
* @param lin_points: The linearization points for, at least, the variables used by this factor
*/
LinearizedHessianFactor(const HessianFactorOrdered::shared_ptr& hessian,
const OrderingOrdered& ordering, const Values& lin_points);
LinearizedHessianFactor(const HessianFactor::shared_ptr& hessian,
const Ordering& ordering, const Values& lin_points);
virtual ~LinearizedHessianFactor() {}
@ -270,8 +270,8 @@ public:
* Reimplemented from NoiseModelFactor to directly copy out the
* matrices and only update the RHS b with an updated residual
*/
boost::shared_ptr<GaussianFactorOrdered> linearize(
const Values& c, const OrderingOrdered& ordering) const;
boost::shared_ptr<GaussianFactor> linearize(
const Values& c, const Ordering& ordering) const;
private:
/** Serialization function */

8
gtsam_unstable/nonlinear/summarization.cpp
View File

@ -15,15 +15,15 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
GaussianFactorGraphOrdered::shared_ptr summarizeGraphSequential(
const GaussianFactorGraphOrdered& full_graph, const std::vector<Index>& indices, bool useQR) {
GaussianFactorGraph::shared_ptr summarizeGraphSequential(
const GaussianFactorGraph& full_graph, const std::vector<Index>& indices, bool useQR) {
GaussianSequentialSolver solver(full_graph, useQR);
return solver.jointFactorGraph(indices);
}
/* ************************************************************************* */
GaussianFactorGraphOrdered::shared_ptr summarizeGraphSequential(
const GaussianFactorGraphOrdered& full_graph, const OrderingOrdered& ordering, const KeySet& saved_keys, bool useQR) {
GaussianFactorGraph::shared_ptr summarizeGraphSequential(
const GaussianFactorGraph& full_graph, const Ordering& ordering, const KeySet& saved_keys, bool useQR) {
std::vector<Index> indices;
BOOST_FOREACH(const Key& k, saved_keys)
indices.push_back(ordering[k]);

8
gtsam_unstable/nonlinear/summarization.h
View File

@ -21,12 +21,12 @@ namespace gtsam {
* Summarization function that eliminates a set of variables (does not convert to Jacobians)
* NOTE: uses sequential solver - requires fully constrained system
*/
GaussianFactorGraphOrdered::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential(
const GaussianFactorGraphOrdered& full_graph, const std::vector<Index>& indices, bool useQR = false);
GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential(
const GaussianFactorGraph& full_graph, const std::vector<Index>& indices, bool useQR = false);
/** Summarization that also converts keys to indices */
GaussianFactorGraphOrdered::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential(
const GaussianFactorGraphOrdered& full_graph, const OrderingOrdered& ordering,
GaussianFactorGraph::shared_ptr GTSAM_UNSTABLE_EXPORT summarizeGraphSequential(
const GaussianFactorGraph& full_graph, const Ordering& ordering,
const KeySet& saved_keys, bool useQR = false);
} // \namespace gtsam

42
gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp
View File

@ -33,7 +33,7 @@ TEST( LinearizedFactor, equals_jacobian )
// Create a Between Factor from a Point3. This is actually a linear factor.
Key x1(1);
Key x2(2);
OrderingOrdered ordering;
Ordering ordering;
ordering.push_back(x1);
ordering.push_back(x2);
Values values;
@ -46,7 +46,7 @@ TEST( LinearizedFactor, equals_jacobian )
// Create two identical factors and make sure they're equal
JacobianFactorOrdered::shared_ptr jf = boost::static_pointer_cast<JacobianFactorOrdered>(betweenFactor.linearize(values, ordering));
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
LinearizedJacobianFactor jacobian1(jf, ordering, values);
LinearizedJacobianFactor jacobian2(jf, ordering, values);
@ -59,7 +59,7 @@ TEST( LinearizedFactor, clone_jacobian )
// Create a Between Factor from a Point3. This is actually a linear factor.
Key x1(1);
Key x2(2);
OrderingOrdered ordering;
Ordering ordering;
ordering.push_back(x1);
ordering.push_back(x2);
Values values;
@ -71,13 +71,13 @@ TEST( LinearizedFactor, clone_jacobian )
BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
// Create one factor that is a clone of the other and make sure they're equal
JacobianFactorOrdered::shared_ptr jf = boost::static_pointer_cast<JacobianFactorOrdered>(betweenFactor.linearize(values, ordering));
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
LinearizedJacobianFactor jacobian1(jf, ordering, values);
LinearizedJacobianFactor::shared_ptr jacobian2 = boost::static_pointer_cast<LinearizedJacobianFactor>(jacobian1.clone());
CHECK(assert_equal(jacobian1, *jacobian2));
JacobianFactorOrdered::shared_ptr jf1 = boost::static_pointer_cast<JacobianFactorOrdered>(jacobian1.linearize(values, ordering));
JacobianFactorOrdered::shared_ptr jf2 = boost::static_pointer_cast<JacobianFactorOrdered>(jacobian2->linearize(values, ordering));
JacobianFactor::shared_ptr jf1 = boost::static_pointer_cast<JacobianFactor>(jacobian1.linearize(values, ordering));
JacobianFactor::shared_ptr jf2 = boost::static_pointer_cast<JacobianFactor>(jacobian2->linearize(values, ordering));
CHECK(assert_equal(*jf1, *jf2));
Matrix information1 = jf1->augmentedInformation();
@ -91,7 +91,7 @@ TEST( LinearizedFactor, add_jacobian )
// Create a Between Factor from a Point3. This is actually a linear factor.
Key x1(1);
Key x2(2);
OrderingOrdered ordering;
Ordering ordering;
ordering.push_back(x1);
ordering.push_back(x2);
Values values;
@ -103,7 +103,7 @@ TEST( LinearizedFactor, add_jacobian )
BetweenFactor<Point3> betweenFactor(x1, x2, measured, model);
// Create two factor graphs, one using 'push_back' and one using 'add' and make sure they're equal
JacobianFactorOrdered::shared_ptr jf = boost::static_pointer_cast<JacobianFactorOrdered>(betweenFactor.linearize(values, ordering));
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
LinearizedJacobianFactor::shared_ptr jacobian(new LinearizedJacobianFactor(jf, ordering, values));
NonlinearFactorGraph graph1; graph1.push_back(jacobian);
NonlinearFactorGraph graph2; graph2.add(*jacobian);
@ -120,7 +120,7 @@ TEST( LinearizedFactor, add_jacobian )
// // Create a Between Factor from a Point3. This is actually a linear factor.
// Key key1(1);
// Key key2(2);
// OrderingOrdered ordering;
// Ordering ordering;
// ordering.push_back(key1);
// ordering.push_back(key2);
// Values values;
@ -175,7 +175,7 @@ TEST( LinearizedFactor, equals_hessian )
// Create a Between Factor from a Point3. This is actually a linear factor.
Key x1(1);
Key x2(2);
OrderingOrdered ordering;
Ordering ordering;
ordering.push_back(x1);
ordering.push_back(x2);
Values values;
@ -188,8 +188,8 @@ TEST( LinearizedFactor, equals_hessian )
// Create two identical factors and make sure they're equal
JacobianFactorOrdered::shared_ptr jf = boost::static_pointer_cast<JacobianFactorOrdered>(betweenFactor.linearize(values, ordering));
HessianFactorOrdered::shared_ptr hf(new HessianFactorOrdered(*jf));
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
HessianFactor::shared_ptr hf(new HessianFactor(*jf));
LinearizedHessianFactor hessian1(hf, ordering, values);
LinearizedHessianFactor hessian2(hf, ordering, values);
@ -202,7 +202,7 @@ TEST( LinearizedFactor, clone_hessian )
// Create a Between Factor from a Point3. This is actually a linear factor.
Key x1(1);
Key x2(2);
OrderingOrdered ordering;
Ordering ordering;
ordering.push_back(x1);
ordering.push_back(x2);
Values values;
@ -215,8 +215,8 @@ TEST( LinearizedFactor, clone_hessian )
// Create two identical factors and make sure they're equal
JacobianFactorOrdered::shared_ptr jf = boost::static_pointer_cast<JacobianFactorOrdered>(betweenFactor.linearize(values, ordering));
HessianFactorOrdered::shared_ptr hf(new HessianFactorOrdered(*jf));
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
HessianFactor::shared_ptr hf(new HessianFactor(*jf));
LinearizedHessianFactor hessian1(hf, ordering, values);
LinearizedHessianFactor::shared_ptr hessian2 = boost::static_pointer_cast<LinearizedHessianFactor>(hessian1.clone());
@ -229,7 +229,7 @@ TEST( LinearizedFactor, add_hessian )
// Create a Between Factor from a Point3. This is actually a linear factor.
Key x1(1);
Key x2(2);
OrderingOrdered ordering;
Ordering ordering;
ordering.push_back(x1);
ordering.push_back(x2);
Values values;
@ -242,8 +242,8 @@ TEST( LinearizedFactor, add_hessian )
// Create two identical factors and make sure they're equal
JacobianFactorOrdered::shared_ptr jf = boost::static_pointer_cast<JacobianFactorOrdered>(betweenFactor.linearize(values, ordering));
HessianFactorOrdered::shared_ptr hf(new HessianFactorOrdered(*jf));
JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
HessianFactor::shared_ptr hf(new HessianFactor(*jf));
LinearizedHessianFactor::shared_ptr hessian(new LinearizedHessianFactor(hf, ordering, values));
NonlinearFactorGraph graph1; graph1.push_back(hessian);
NonlinearFactorGraph graph2; graph2.add(*hessian);
@ -257,7 +257,7 @@ TEST( LinearizedFactor, add_hessian )
// // Create a Between Factor from a Point3. This is actually a linear factor.
// Key key1(1);
// Key key2(2);
// OrderingOrdered ordering;
// Ordering ordering;
// ordering.push_back(key1);
// ordering.push_back(key2);
// Values values;
@ -271,7 +271,7 @@ TEST( LinearizedFactor, add_hessian )
//
// // Create a linearized hessian factor
// JacobianFactor::shared_ptr jf = boost::static_pointer_cast<JacobianFactor>(betweenFactor.linearize(values, ordering));
// HessianFactorOrdered::shared_ptr hf(new HessianFactorOrdered(*jf));
// HessianFactor::shared_ptr hf(new HessianFactor(*jf));
// LinearizedHessianFactor hessian(hf, ordering, values);
//
//
@ -294,7 +294,7 @@ TEST( LinearizedFactor, add_hessian )
// EXPECT_DOUBLES_EQUAL(expected_error, actual_error, 1e-9 );
//
// // Check that the linearized factors are identical
// GaussianFactor::shared_ptr expected_factor = HessianFactorOrdered::shared_ptr(new HessianFactorOrdered(*betweenFactor.linearize(linpoint, ordering)));
// GaussianFactor::shared_ptr expected_factor = HessianFactor::shared_ptr(new HessianFactor(*betweenFactor.linearize(linpoint, ordering)));
// GaussianFactor::shared_ptr actual_factor = hessian.linearize(linpoint, ordering);
// CHECK(assert_equal(*expected_factor, *actual_factor));
// }

10
gtsam_unstable/slam/DummyFactor.cpp
View File

@ -37,11 +37,11 @@ bool DummyFactor::equals(const NonlinearFactor& f, double tol) const {
}
/* ************************************************************************* */
boost::shared_ptr<GaussianFactorOrdered>
DummyFactor::linearize(const Values& c, const OrderingOrdered& ordering) const {
boost::shared_ptr<GaussianFactor>
DummyFactor::linearize(const Values& c, const Ordering& ordering) const {
// Only linearize if the factor is active
if (!this->active(c))
return boost::shared_ptr<JacobianFactorOrdered>();
return boost::shared_ptr<JacobianFactor>();
// Fill in terms with zero matrices
std::vector<std::pair<Index, Matrix> > terms(this->size());
@ -51,8 +51,8 @@ DummyFactor::linearize(const Values& c, const OrderingOrdered& ordering) const {
}
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(rowDim_);
return GaussianFactorOrdered::shared_ptr(
new JacobianFactorOrdered(terms, zero(rowDim_), model));
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, zero(rowDim_), model));
}
/* ************************************************************************* */

4
gtsam_unstable/slam/DummyFactor.h
View File

@ -54,8 +54,8 @@ public:
virtual size_t dim() const { return rowDim_; }
/** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactorOrdered>
linearize(const Values& c, const OrderingOrdered& ordering) const;
virtual boost::shared_ptr<GaussianFactor>
linearize(const Values& c, const Ordering& ordering) const;
/**
* Creates a shared_ptr clone of the factor - needs to be specialized to allow

6
gtsam_unstable/slam/tests/testDummyFactor.cpp
View File

@ -41,14 +41,14 @@ TEST( testDummyFactor, basics ) {
// errors - all zeros
DOUBLES_EQUAL(0.0, dummyfactor.error(c), tol);
OrderingOrdered ordering;
Ordering ordering;
ordering += key1, key2;
// linearization: all zeros
GaussianFactorOrdered::shared_ptr actLinearization = dummyfactor.linearize(c, ordering);
GaussianFactor::shared_ptr actLinearization = dummyfactor.linearize(c, ordering);
CHECK(actLinearization);
noiseModel::Diagonal::shared_ptr model3 = noiseModel::Unit::Create(3);
GaussianFactorOrdered::shared_ptr expLinearization(new JacobianFactorOrdered(
GaussianFactor::shared_ptr expLinearization(new JacobianFactor(
ordering[key1], zeros(3,3), ordering[key2], zeros(3,3), zero(3), model3));
EXPECT(assert_equal(*expLinearization, *actLinearization, tol));
}

22
tests/testBoundingConstraint.cpp
View File

@ -116,10 +116,10 @@ TEST( testBoundingConstraint, unary_linearization_inactive) {
Point2 pt1(2.0, 3.0);
Values config1;
config1.insert(key, pt1);
OrderingOrdered ordering;
Ordering ordering;
ordering += key;
GaussianFactorOrdered::shared_ptr actual1 = constraint1.linearize(config1, ordering);
GaussianFactorOrdered::shared_ptr actual2 = constraint2.linearize(config1, ordering);
GaussianFactor::shared_ptr actual1 = constraint1.linearize(config1, ordering);
GaussianFactor::shared_ptr actual2 = constraint2.linearize(config1, ordering);
EXPECT(!actual1);
EXPECT(!actual2);
}
@ -129,14 +129,14 @@ TEST( testBoundingConstraint, unary_linearization_active) {
Point2 pt2(-2.0, -3.0);
Values config2;
config2.insert(key, pt2);
OrderingOrdered ordering;
Ordering ordering;
ordering += key;
GaussianFactorOrdered::shared_ptr actual1 = constraint1.linearize(config2, ordering);
GaussianFactorOrdered::shared_ptr actual2 = constraint2.linearize(config2, ordering);
JacobianFactorOrdered expected1(ordering[key], Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1);
JacobianFactorOrdered expected2(ordering[key], Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1);
EXPECT(assert_equal((const GaussianFactorOrdered&)expected1, *actual1, tol));
EXPECT(assert_equal((const GaussianFactorOrdered&)expected2, *actual2, tol));
GaussianFactor::shared_ptr actual1 = constraint1.linearize(config2, ordering);
GaussianFactor::shared_ptr actual2 = constraint2.linearize(config2, ordering);
JacobianFactor expected1(ordering[key], Matrix_(1, 2, 1.0, 0.0), repeat(1, 3.0), hard_model1);
JacobianFactor expected2(ordering[key], Matrix_(1, 2, 0.0, 1.0), repeat(1, 5.0), hard_model1);
EXPECT(assert_equal((const GaussianFactor&)expected1, *actual1, tol));
EXPECT(assert_equal((const GaussianFactor&)expected2, *actual2, tol));
}
/* ************************************************************************* */
@ -199,7 +199,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
Values config1;
config1.insert(key1, pt1);
config1.insert(key2, pt1);
OrderingOrdered ordering; ordering += key1, key2;
Ordering ordering; ordering += key1, key2;
EXPECT(!rangeBound.active(config1));
EXPECT(assert_equal(zero(1), rangeBound.unwhitenedError(config1)));
EXPECT(!rangeBound.linearize(config1, ordering));

144
tests/testDoglegOptimizer.cpp
View File

@ -46,28 +46,28 @@ using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
double computeError(const GaussianBayesNetOrdered& gbn, const LieVector& values) {
double computeError(const GaussianBayesNet& gbn, const LieVector& values) {
// Convert Vector to VectorValues
VectorValuesOrdered vv = *allocateVectorValues(gbn);
VectorValues vv = *allocateVectorValues(gbn);
internal::writeVectorValuesSlices(values, vv,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size()));
// Convert to factor graph
GaussianFactorGraphOrdered gfg(gbn);
GaussianFactorGraph gfg(gbn);
return gfg.error(vv);
}
/* ************************************************************************* */
double computeErrorBt(const BayesTreeOrdered<GaussianConditionalOrdered>& gbt, const LieVector& values) {
double computeErrorBt(const BayesTree<GaussianConditional>& gbt, const LieVector& values) {
// Convert Vector to VectorValues
VectorValuesOrdered vv = *allocateVectorValues(gbt);
VectorValues vv = *allocateVectorValues(gbt);
internal::writeVectorValuesSlices(values, vv,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size()));
// Convert to factor graph
GaussianFactorGraphOrdered gfg(gbt);
GaussianFactorGraph gfg(gbt);
return gfg.error(vv);
}
@ -75,58 +75,58 @@ double computeErrorBt(const BayesTreeOrdered<GaussianConditionalOrdered>& gbt, c
TEST(DoglegOptimizer, ComputeSteepestDescentPoint) {
// Create an arbitrary Bayes Net
GaussianBayesNetOrdered gbn;
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
GaussianBayesNet gbn;
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2)));
// Compute the Hessian numerically
Matrix hessian = numericalHessian(
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
LieVector(VectorValuesOrdered::Zero(*allocateVectorValues(gbn)).asVector()));
LieVector(VectorValues::Zero(*allocateVectorValues(gbn)).asVector()));
// Compute the gradient numerically
VectorValuesOrdered gradientValues = *allocateVectorValues(gbn);
VectorValues gradientValues = *allocateVectorValues(gbn);
Vector gradient = numericalGradient(
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
LieVector(VectorValuesOrdered::Zero(gradientValues).asVector()));
LieVector(VectorValues::Zero(gradientValues).asVector()));
internal::writeVectorValuesSlices(gradient, gradientValues,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraphOrdered(gbn).augmentedHessian();
Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();
LONGS_EQUAL(11, augmentedHessian.cols());
VectorValuesOrdered denseMatrixGradient = *allocateVectorValues(gbn);
VectorValues denseMatrixGradient = *allocateVectorValues(gbn);
internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
// Compute the steepest descent point
double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0);
VectorValuesOrdered expected = gradientValues; scal(step, expected);
VectorValues expected = gradientValues; scal(step, expected);
// Compute the steepest descent point with the dogleg function
VectorValuesOrdered actual = optimizeGradientSearch(gbn);
VectorValues actual = optimizeGradientSearch(gbn);
// Check that points agree
EXPECT(assert_equal(expected, actual, 1e-5));
// Check that point causes a decrease in error
double origError = GaussianFactorGraphOrdered(gbn).error(VectorValuesOrdered::Zero(*allocateVectorValues(gbn)));
double newError = GaussianFactorGraphOrdered(gbn).error(actual);
double origError = GaussianFactorGraph(gbn).error(VectorValues::Zero(*allocateVectorValues(gbn)));
double newError = GaussianFactorGraph(gbn).error(actual);
EXPECT(newError < origError);
}
@ -134,9 +134,9 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) {
TEST(DoglegOptimizer, BT_BN_equivalency) {
// Create an arbitrary Bayes Tree
BayesTreeOrdered<GaussianConditionalOrdered> bt;
bt.insert(BayesTreeOrdered<GaussianConditionalOrdered>::sharedClique(new BayesTreeOrdered<GaussianConditionalOrdered>::Clique(
GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
BayesTree<GaussianConditional> bt;
bt.insert(BayesTree<GaussianConditional>::sharedClique(new BayesTree<GaussianConditional>::Clique(
GaussianConditional::shared_ptr(new GaussianConditional(
boost::assign::pair_list_of
(2, Matrix_(6,2,
31.0,32.0,
@ -160,8 +160,8 @@ TEST(DoglegOptimizer, BT_BN_equivalency) {
51.0,52.0,
0.0,54.0)),
3, Vector_(6, 29.0,30.0,39.0,40.0,49.0,50.0), ones(6))))));
bt.insert(BayesTreeOrdered<GaussianConditionalOrdered>::sharedClique(new BayesTreeOrdered<GaussianConditionalOrdered>::Clique(
GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
bt.insert(BayesTree<GaussianConditional>::sharedClique(new BayesTree<GaussianConditional>::Clique(
GaussianConditional::shared_ptr(new GaussianConditional(
boost::assign::pair_list_of
(0, Matrix_(4,2,
3.0,4.0,
@ -191,26 +191,26 @@ TEST(DoglegOptimizer, BT_BN_equivalency) {
2, Vector_(4, 1.0,2.0,15.0,16.0), ones(4))))));
// Create an arbitrary Bayes Net
GaussianBayesNetOrdered gbn;
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
GaussianBayesNet gbn;
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2)));
GaussianFactorGraphOrdered expected(gbn);
GaussianFactorGraphOrdered actual(bt);
GaussianFactorGraph expected(gbn);
GaussianFactorGraph actual(bt);
EXPECT(assert_equal(expected.augmentedHessian(), actual.augmentedHessian()));
}
@ -219,9 +219,9 @@ TEST(DoglegOptimizer, BT_BN_equivalency) {
TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {
// Create an arbitrary Bayes Tree
BayesTreeOrdered<GaussianConditionalOrdered> bt;
bt.insert(BayesTreeOrdered<GaussianConditionalOrdered>::sharedClique(new BayesTreeOrdered<GaussianConditionalOrdered>::Clique(
GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
BayesTree<GaussianConditional> bt;
bt.insert(BayesTree<GaussianConditional>::sharedClique(new BayesTree<GaussianConditional>::Clique(
GaussianConditional::shared_ptr(new GaussianConditional(
boost::assign::pair_list_of
(2, Matrix_(6,2,
31.0,32.0,
@ -245,8 +245,8 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {
51.0,52.0,
0.0,54.0)),
3, Vector_(6, 29.0,30.0,39.0,40.0,49.0,50.0), ones(6))))));
bt.insert(BayesTreeOrdered<GaussianConditionalOrdered>::sharedClique(new BayesTreeOrdered<GaussianConditionalOrdered>::Clique(
GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
bt.insert(BayesTree<GaussianConditional>::sharedClique(new BayesTree<GaussianConditional>::Clique(
GaussianConditional::shared_ptr(new GaussianConditional(
boost::assign::pair_list_of
(0, Matrix_(4,2,
3.0,4.0,
@ -278,30 +278,30 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {
// Compute the Hessian numerically
Matrix hessian = numericalHessian(
boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
LieVector(VectorValuesOrdered::Zero(*allocateVectorValues(bt)).asVector()));
LieVector(VectorValues::Zero(*allocateVectorValues(bt)).asVector()));
// Compute the gradient numerically
VectorValuesOrdered gradientValues = *allocateVectorValues(bt);
VectorValues gradientValues = *allocateVectorValues(bt);
Vector gradient = numericalGradient(
boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
LieVector(VectorValuesOrdered::Zero(gradientValues).asVector()));
LieVector(VectorValues::Zero(gradientValues).asVector()));
internal::writeVectorValuesSlices(gradient, gradientValues,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraphOrdered(bt).augmentedHessian();
Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();
LONGS_EQUAL(11, augmentedHessian.cols());
VectorValuesOrdered denseMatrixGradient = *allocateVectorValues(bt);
VectorValues denseMatrixGradient = *allocateVectorValues(bt);
internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient,
boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));
// Compute the steepest descent point
double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0);
VectorValuesOrdered expected = gradientValues; scal(step, expected);
VectorValues expected = gradientValues; scal(step, expected);
// Known steepest descent point from Bayes' net version
VectorValuesOrdered expectedFromBN(5,2);
VectorValues expectedFromBN(5,2);
expectedFromBN[0] = Vector_(2, 0.000129034, 0.000688183);
expectedFromBN[1] = Vector_(2, 0.0109679, 0.0253767);
expectedFromBN[2] = Vector_(2, 0.0680441, 0.114496);
@ -309,90 +309,90 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {
expectedFromBN[4] = Vector_(2, 0.300134, 0.423233);
// Compute the steepest descent point with the dogleg function
VectorValuesOrdered actual = optimizeGradientSearch(bt);
VectorValues actual = optimizeGradientSearch(bt);
// Check that points agree
EXPECT(assert_equal(expected, actual, 1e-5));
EXPECT(assert_equal(expectedFromBN, actual, 1e-5));
// Check that point causes a decrease in error
double origError = GaussianFactorGraphOrdered(bt).error(VectorValuesOrdered::Zero(*allocateVectorValues(bt)));
double newError = GaussianFactorGraphOrdered(bt).error(actual);
double origError = GaussianFactorGraph(bt).error(VectorValues::Zero(*allocateVectorValues(bt)));
double newError = GaussianFactorGraph(bt).error(actual);
EXPECT(newError < origError);
}
/* ************************************************************************* */
TEST(DoglegOptimizer, ComputeBlend) {
// Create an arbitrary Bayes Net
GaussianBayesNetOrdered gbn;
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
GaussianBayesNet gbn;
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2)));
// Compute steepest descent point
VectorValuesOrdered xu = optimizeGradientSearch(gbn);
VectorValues xu = optimizeGradientSearch(gbn);
// Compute Newton's method point
VectorValuesOrdered xn = optimize(gbn);
VectorValues xn = optimize(gbn);
// The Newton's method point should be more "adventurous", i.e. larger, than the steepest descent point
EXPECT(xu.asVector().norm() < xn.asVector().norm());
// Compute blend
double Delta = 1.5;
VectorValuesOrdered xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn);
VectorValues xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn);
DOUBLES_EQUAL(Delta, xb.asVector().norm(), 1e-10);
}
/* ************************************************************************* */
TEST(DoglegOptimizer, ComputeDoglegPoint) {
// Create an arbitrary Bayes Net
GaussianBayesNetOrdered gbn;
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
GaussianBayesNet gbn;
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2)));
gbn += GaussianConditionalOrdered::shared_ptr(new GaussianConditionalOrdered(
gbn += GaussianConditional::shared_ptr(new GaussianConditional(
4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2)));
// Compute dogleg point for different deltas
double Delta1 = 0.5; // Less than steepest descent
VectorValuesOrdered actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, optimizeGradientSearch(gbn), optimize(gbn));
VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, optimizeGradientSearch(gbn), optimize(gbn));
DOUBLES_EQUAL(Delta1, actual1.asVector().norm(), 1e-5);
double Delta2 = 1.5; // Between steepest descent and Newton's method
VectorValuesOrdered expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
VectorValuesOrdered actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
DOUBLES_EQUAL(Delta2, actual2.asVector().norm(), 1e-5);
EXPECT(assert_equal(expected2, actual2));
double Delta3 = 5.0; // Larger than Newton's method point
VectorValuesOrdered expected3 = optimize(gbn);
VectorValuesOrdered actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, optimizeGradientSearch(gbn), optimize(gbn));
VectorValues expected3 = optimize(gbn);
VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, optimizeGradientSearch(gbn), optimize(gbn));
EXPECT(assert_equal(expected3, actual3));
}
@ -408,16 +408,16 @@ TEST(DoglegOptimizer, Iterate) {
config->insert(X(1), x0);
// ordering
boost::shared_ptr<OrderingOrdered> ord(new OrderingOrdered());
boost::shared_ptr<Ordering> ord(new Ordering());
ord->push_back(X(1));
double Delta = 1.0;
for(size_t it=0; it<10; ++it) {
GaussianSequentialSolver solver(*fg->linearize(*config, *ord));
GaussianBayesNetOrdered gbn = *solver.eliminate();
GaussianBayesNet gbn = *solver.eliminate();
// Iterate assumes that linear error = nonlinear error at the linearization point, and this should be true
double nonlinearError = fg->error(*config);
double linearError = GaussianFactorGraphOrdered(gbn).error(VectorValuesOrdered::Zero(*allocateVectorValues(gbn)));
double linearError = GaussianFactorGraph(gbn).error(VectorValues::Zero(*allocateVectorValues(gbn)));
DOUBLES_EQUAL(nonlinearError, linearError, 1e-5);
// cout << "it " << it << ", Delta = " << Delta << ", error = " << fg->error(*config) << endl;
DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, gbn, *fg, *config, *ord, fg->error(*config));

12
tests/testExtendedKalmanFilter.cpp
View File

@ -192,7 +192,7 @@ public:
* Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z
* Hence b = z - h(x1,x2) = - error_vector(x)
*/
boost::shared_ptr<GaussianFactorOrdered> linearize(const Values& c, const OrderingOrdered& ordering) const {
boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
const X1& x1 = c.at<X1>(key1());
const X2& x2 = c.at<X2>(key2());
Matrix A1, A2;
@ -201,7 +201,7 @@ public:
SharedDiagonal constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
return JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(var1, A1, var2,
return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, var2,
A2, b, constrained));
}
// "Whiten" the system before converting to a Gaussian Factor
@ -209,7 +209,7 @@ public:
A1 = Qinvsqrt*A1;
A2 = Qinvsqrt*A2;
b = Qinvsqrt*b;
return GaussianFactorOrdered::shared_ptr(new JacobianFactorOrdered(var1, A1, var2,
return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2,
A2, b, noiseModel::Unit::Create(b.size())));
}
@ -329,7 +329,7 @@ public:
* Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z
* Hence b = z - h(x1) = - error_vector(x)
*/
boost::shared_ptr<GaussianFactorOrdered> linearize(const Values& c, const OrderingOrdered& ordering) const {
boost::shared_ptr<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
const X& x1 = c.at<X>(key());
Matrix A1;
Vector b = -evaluateError(x1, A1);
@ -337,13 +337,13 @@ public:
SharedDiagonal constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained.get() != NULL) {
return JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(var1, A1, b, constrained));
return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, b, constrained));
}
// "Whiten" the system before converting to a Gaussian Factor
Matrix Rinvsqrt = RInvSqrt(x1);
A1 = Rinvsqrt*A1;
b = Rinvsqrt*b;
return GaussianFactorOrdered::shared_ptr(new JacobianFactorOrdered(var1, A1, b, noiseModel::Unit::Create(b.size())));
return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, b, noiseModel::Unit::Create(b.size())));
}
/** vector of errors */

164
tests/testGaussianBayesTree.cpp
View File

@ -52,45 +52,45 @@ C4 x3 : x4
C5 x2 : x3
C6 x1 : x2
**************************************************************************** */
TEST( GaussianBayesTreeOrdered, linear_smoother_shortcuts )
TEST( GaussianBayesTree, linear_smoother_shortcuts )
{
// Create smoother with 7 nodes
OrderingOrdered ordering;
GaussianFactorGraphOrdered smoother;
Ordering ordering;
GaussianFactorGraph smoother;
boost::tie(smoother, ordering) = createSmoother(7);
GaussianBayesTreeOrdered bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
// Create the Bayes tree
LONGS_EQUAL(6, bayesTree.size());
// Check the conditional P(Root|Root)
GaussianBayesNetOrdered empty;
GaussianBayesTreeOrdered::sharedClique R = bayesTree.root();
GaussianBayesNetOrdered actual1 = R->shortcut(R, EliminateCholeskyOrdered);
GaussianBayesNet empty;
GaussianBayesTree::sharedClique R = bayesTree.root();
GaussianBayesNet actual1 = R->shortcut(R, EliminateCholesky);
EXPECT(assert_equal(empty,actual1,tol));
// Check the conditional P(C2|Root)
GaussianBayesTreeOrdered::sharedClique C2 = bayesTree[ordering[X(5)]];
GaussianBayesNetOrdered actual2 = C2->shortcut(R, EliminateCholeskyOrdered);
GaussianBayesTree::sharedClique C2 = bayesTree[ordering[X(5)]];
GaussianBayesNet actual2 = C2->shortcut(R, EliminateCholesky);
EXPECT(assert_equal(empty,actual2,tol));
// Check the conditional P(C3|Root)
double sigma3 = 0.61808;
Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022);
GaussianBayesNetOrdered expected3;
GaussianBayesNet expected3;
push_front(expected3,ordering[X(5)], zero(2), eye(2)/sigma3, ordering[X(6)], A56/sigma3, ones(2));
GaussianBayesTreeOrdered::sharedClique C3 = bayesTree[ordering[X(4)]];
GaussianBayesNetOrdered actual3 = C3->shortcut(R, EliminateCholeskyOrdered);
GaussianBayesTree::sharedClique C3 = bayesTree[ordering[X(4)]];
GaussianBayesNet actual3 = C3->shortcut(R, EliminateCholesky);
EXPECT(assert_equal(expected3,actual3,tol));
// Check the conditional P(C4|Root)
double sigma4 = 0.661968;
Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067);
GaussianBayesNetOrdered expected4;
GaussianBayesNet expected4;
push_front(expected4, ordering[X(4)], zero(2), eye(2)/sigma4, ordering[X(6)], A46/sigma4, ones(2));
GaussianBayesTreeOrdered::sharedClique C4 = bayesTree[ordering[X(3)]];
GaussianBayesNetOrdered actual4 = C4->shortcut(R, EliminateCholeskyOrdered);
GaussianBayesTree::sharedClique C4 = bayesTree[ordering[X(3)]];
GaussianBayesNet actual4 = C4->shortcut(R, EliminateCholesky);
EXPECT(assert_equal(expected4,actual4,tol));
}
@ -113,18 +113,18 @@ TEST( GaussianBayesTreeOrdered, linear_smoother_shortcuts )
C4 x7 : x6
************************************************************************* */
TEST( GaussianBayesTreeOrdered, balanced_smoother_marginals )
TEST( GaussianBayesTree, balanced_smoother_marginals )
{
// Create smoother with 7 nodes
OrderingOrdered ordering;
Ordering ordering;
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first;
GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// Create the Bayes tree
GaussianBayesTreeOrdered bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
VectorValuesOrdered expectedSolution(VectorValuesOrdered::Zero(7,2));
VectorValuesOrdered actualSolution = optimize(bayesTree);
VectorValues expectedSolution(VectorValues::Zero(7,2));
VectorValues actualSolution = optimize(bayesTree);
EXPECT(assert_equal(expectedSolution,actualSolution,tol));
LONGS_EQUAL(4,bayesTree.size());
@ -132,73 +132,73 @@ TEST( GaussianBayesTreeOrdered, balanced_smoother_marginals )
double tol=1e-5;
// Check marginal on x1
GaussianBayesNetOrdered expected1 = simpleGaussian(ordering[X(1)], zero(2), sigmax1);
GaussianBayesNetOrdered actual1 = *bayesTree.marginalBayesNet(ordering[X(1)], EliminateCholeskyOrdered);
GaussianBayesNet expected1 = simpleGaussian(ordering[X(1)], zero(2), sigmax1);
GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering[X(1)], EliminateCholesky);
Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1);
Matrix actualCovarianceX1;
GaussianFactorOrdered::shared_ptr m = bayesTree.marginalFactor(ordering[X(1)], EliminateCholeskyOrdered);
actualCovarianceX1 = bayesTree.marginalFactor(ordering[X(1)], EliminateCholeskyOrdered)->information().inverse();
GaussianFactor::shared_ptr m = bayesTree.marginalFactor(ordering[X(1)], EliminateCholesky);
actualCovarianceX1 = bayesTree.marginalFactor(ordering[X(1)], EliminateCholesky)->information().inverse();
EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
EXPECT(assert_equal(expected1,actual1,tol));
// Check marginal on x2
double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
GaussianBayesNetOrdered expected2 = simpleGaussian(ordering[X(2)], zero(2), sigx2);
GaussianBayesNetOrdered actual2 = *bayesTree.marginalBayesNet(ordering[X(2)], EliminateCholeskyOrdered);
GaussianBayesNet expected2 = simpleGaussian(ordering[X(2)], zero(2), sigx2);
GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering[X(2)], EliminateCholesky);
Matrix expectedCovarianceX2 = eye(2,2) * (sigx2 * sigx2);
Matrix actualCovarianceX2;
actualCovarianceX2 = bayesTree.marginalFactor(ordering[X(2)], EliminateCholeskyOrdered)->information().inverse();
actualCovarianceX2 = bayesTree.marginalFactor(ordering[X(2)], EliminateCholesky)->information().inverse();
EXPECT(assert_equal(expectedCovarianceX2, actualCovarianceX2, tol));
EXPECT(assert_equal(expected2,actual2,tol));
// Check marginal on x3
GaussianBayesNetOrdered expected3 = simpleGaussian(ordering[X(3)], zero(2), sigmax3);
GaussianBayesNetOrdered actual3 = *bayesTree.marginalBayesNet(ordering[X(3)], EliminateCholeskyOrdered);
GaussianBayesNet expected3 = simpleGaussian(ordering[X(3)], zero(2), sigmax3);
GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering[X(3)], EliminateCholesky);
Matrix expectedCovarianceX3 = eye(2,2) * (sigmax3 * sigmax3);
Matrix actualCovarianceX3;
actualCovarianceX3 = bayesTree.marginalFactor(ordering[X(3)], EliminateCholeskyOrdered)->information().inverse();
actualCovarianceX3 = bayesTree.marginalFactor(ordering[X(3)], EliminateCholesky)->information().inverse();
EXPECT(assert_equal(expectedCovarianceX3, actualCovarianceX3, tol));
EXPECT(assert_equal(expected3,actual3,tol));
// Check marginal on x4
GaussianBayesNetOrdered expected4 = simpleGaussian(ordering[X(4)], zero(2), sigmax4);
GaussianBayesNetOrdered actual4 = *bayesTree.marginalBayesNet(ordering[X(4)], EliminateCholeskyOrdered);
GaussianBayesNet expected4 = simpleGaussian(ordering[X(4)], zero(2), sigmax4);
GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering[X(4)], EliminateCholesky);
Matrix expectedCovarianceX4 = eye(2,2) * (sigmax4 * sigmax4);
Matrix actualCovarianceX4;
actualCovarianceX4 = bayesTree.marginalFactor(ordering[X(4)], EliminateCholeskyOrdered)->information().inverse();
actualCovarianceX4 = bayesTree.marginalFactor(ordering[X(4)], EliminateCholesky)->information().inverse();
EXPECT(assert_equal(expectedCovarianceX4, actualCovarianceX4, tol));
EXPECT(assert_equal(expected4,actual4,tol));
// Check marginal on x7 (should be equal to x1)
GaussianBayesNetOrdered expected7 = simpleGaussian(ordering[X(7)], zero(2), sigmax7);
GaussianBayesNetOrdered actual7 = *bayesTree.marginalBayesNet(ordering[X(7)], EliminateCholeskyOrdered);
GaussianBayesNet expected7 = simpleGaussian(ordering[X(7)], zero(2), sigmax7);
GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering[X(7)], EliminateCholesky);
Matrix expectedCovarianceX7 = eye(2,2) * (sigmax7 * sigmax7);
Matrix actualCovarianceX7;
actualCovarianceX7 = bayesTree.marginalFactor(ordering[X(7)], EliminateCholeskyOrdered)->information().inverse();
actualCovarianceX7 = bayesTree.marginalFactor(ordering[X(7)], EliminateCholesky)->information().inverse();
EXPECT(assert_equal(expectedCovarianceX7, actualCovarianceX7, tol));
EXPECT(assert_equal(expected7,actual7,tol));
}
/* ************************************************************************* */
TEST( GaussianBayesTreeOrdered, balanced_smoother_shortcuts )
TEST( GaussianBayesTree, balanced_smoother_shortcuts )
{
// Create smoother with 7 nodes
OrderingOrdered ordering;
Ordering ordering;
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first;
GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// Create the Bayes tree
GaussianBayesTreeOrdered bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
// Check the conditional P(Root|Root)
GaussianBayesNetOrdered empty;
GaussianBayesTreeOrdered::sharedClique R = bayesTree.root();
GaussianBayesNetOrdered actual1 = R->shortcut(R, EliminateCholeskyOrdered);
GaussianBayesNet empty;
GaussianBayesTree::sharedClique R = bayesTree.root();
GaussianBayesNet actual1 = R->shortcut(R, EliminateCholesky);
EXPECT(assert_equal(empty,actual1,tol));
// Check the conditional P(C2|Root)
GaussianBayesTreeOrdered::sharedClique C2 = bayesTree[ordering[X(3)]];
GaussianBayesNetOrdered actual2 = C2->shortcut(R, EliminateCholeskyOrdered);
GaussianBayesTree::sharedClique C2 = bayesTree[ordering[X(3)]];
GaussianBayesNet actual2 = C2->shortcut(R, EliminateCholesky);
EXPECT(assert_equal(empty,actual2,tol));
// Check the conditional P(C3|Root), which should be equal to P(x2|x4)
@ -206,11 +206,11 @@ TEST( GaussianBayesTreeOrdered, balanced_smoother_shortcuts )
* p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering[X(2)]]->conditional()
* We don't know yet how to take it out.
*/
// GaussianConditionalOrdered::shared_ptr p_x2_x4 = bayesTree[ordering[X(2)]]->conditional();
// GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[X(2)]]->conditional();
// p_x2_x4->print("Conditional p_x2_x4: ");
// GaussianBayesNetOrdered expected3(p_x2_x4);
// GaussianBayesNet expected3(p_x2_x4);
// GaussianISAM::sharedClique C3 = isamTree[ordering[X(1)]];
// GaussianBayesNetOrdered actual3 = GaussianISAM::shortcut(C3,R);
// GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
// EXPECT(assert_equal(expected3,actual3,tol));
}
@ -218,96 +218,96 @@ TEST( GaussianBayesTreeOrdered, balanced_smoother_shortcuts )
//TEST( BayesTree, balanced_smoother_clique_marginals )
//{
// // Create smoother with 7 nodes
// OrderingOrdered ordering;
// Ordering ordering;
// ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
// GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first;
// GaussianFactorGraph smoother = createSmoother(7, ordering).first;
//
// // Create the Bayes tree
// GaussianBayesNetOrdered chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate();
// GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate();
// GaussianISAM bayesTree(chordalBayesNet);
//
// // Check the clique marginal P(C3)
// double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED!
// GaussianBayesNetOrdered expected = simpleGaussian(ordering[X(2)],zero(2),sigmax2_alt);
// GaussianBayesNet expected = simpleGaussian(ordering[X(2)],zero(2),sigmax2_alt);
// push_front(expected,ordering[X(1)], zero(2), eye(2)*sqrt(2), ordering[X(2)], -eye(2)*sqrt(2)/2, ones(2));
// GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[X(1)]];
// GaussianFactorGraphOrdered marginal = C3->marginal(R);
// GaussianFactorGraph marginal = C3->marginal(R);
// GaussianVariableIndex varIndex(marginal);
// Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size()));
// Permutation toFrontInverse(*toFront.inverse());
// varIndex.permute(toFront);
// BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, marginal) {
// BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, marginal) {
// factor->permuteWithInverse(toFrontInverse); }
// GaussianBayesNetOrdered actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex);
// GaussianBayesNet actual = *inference::EliminateUntil(marginal, C3->keys().size(), varIndex);
// actual.permuteWithInverse(toFront);
// EXPECT(assert_equal(expected,actual,tol));
//}
/* ************************************************************************* */
TEST( GaussianBayesTreeOrdered, balanced_smoother_joint )
TEST( GaussianBayesTree, balanced_smoother_joint )
{
// Create smoother with 7 nodes
OrderingOrdered ordering;
Ordering ordering;
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraphOrdered smoother = createSmoother(7, ordering).first;
GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// Create the Bayes tree, expected to look like:
// x5 x6 x4
// x3 x2 : x4
// x1 : x2
// x7 : x6
GaussianBayesTreeOrdered bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
GaussianBayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
// Conditional density elements reused by both tests
const Vector sigma = ones(2);
const Matrix I = eye(2), A = -0.00429185*I;
// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
GaussianBayesNetOrdered expected1;
GaussianBayesNet expected1;
// Why does the sign get flipped on the prior?
GaussianConditionalOrdered::shared_ptr
parent1(new GaussianConditionalOrdered(ordering[X(7)], zero(2), -1*I/sigmax7, ones(2)));
GaussianConditional::shared_ptr
parent1(new GaussianConditional(ordering[X(7)], zero(2), -1*I/sigmax7, ones(2)));
expected1.push_front(parent1);
push_front(expected1,ordering[X(1)], zero(2), I/sigmax7, ordering[X(7)], A/sigmax7, sigma);
GaussianBayesNetOrdered actual1 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(7)], EliminateCholeskyOrdered);
GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(7)], EliminateCholesky);
EXPECT(assert_equal(expected1,actual1,tol));
// // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
// GaussianBayesNetOrdered expected2;
// GaussianConditionalOrdered::shared_ptr
// parent2(new GaussianConditionalOrdered(ordering[X(1)], zero(2), -1*I/sigmax1, ones(2)));
// GaussianBayesNet expected2;
// GaussianConditional::shared_ptr
// parent2(new GaussianConditional(ordering[X(1)], zero(2), -1*I/sigmax1, ones(2)));
// expected2.push_front(parent2);
// push_front(expected2,ordering[X(7)], zero(2), I/sigmax1, ordering[X(1)], A/sigmax1, sigma);
// GaussianBayesNetOrdered actual2 = *bayesTree.jointBayesNet(ordering[X(7)],ordering[X(1)]);
// GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering[X(7)],ordering[X(1)]);
// EXPECT(assert_equal(expected2,actual2,tol));
// Check the joint density P(x1,x4), i.e. with a root variable
GaussianBayesNetOrdered expected3;
GaussianConditionalOrdered::shared_ptr
parent3(new GaussianConditionalOrdered(ordering[X(4)], zero(2), I/sigmax4, ones(2)));
GaussianBayesNet expected3;
GaussianConditional::shared_ptr
parent3(new GaussianConditional(ordering[X(4)], zero(2), I/sigmax4, ones(2)));
expected3.push_front(parent3);
double sig14 = 0.784465;
Matrix A14 = -0.0769231*I;
push_front(expected3,ordering[X(1)], zero(2), I/sig14, ordering[X(4)], A14/sig14, sigma);
GaussianBayesNetOrdered actual3 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(4)], EliminateCholeskyOrdered);
GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering[X(1)],ordering[X(4)], EliminateCholesky);
EXPECT(assert_equal(expected3,actual3,tol));
// // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
// GaussianBayesNetOrdered expected4;
// GaussianConditionalOrdered::shared_ptr
// parent4(new GaussianConditionalOrdered(ordering[X(1)], zero(2), -1.0*I/sigmax1, ones(2)));
// GaussianBayesNet expected4;
// GaussianConditional::shared_ptr
// parent4(new GaussianConditional(ordering[X(1)], zero(2), -1.0*I/sigmax1, ones(2)));
// expected4.push_front(parent4);
// double sig41 = 0.668096;
// Matrix A41 = -0.055794*I;
// push_front(expected4,ordering[X(4)], zero(2), I/sig41, ordering[X(1)], A41/sig41, sigma);
// GaussianBayesNetOrdered actual4 = *bayesTree.jointBayesNet(ordering[X(4)],ordering[X(1)]);
// GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering[X(4)],ordering[X(1)]);
// EXPECT(assert_equal(expected4,actual4,tol));
}
/* ************************************************************************* */
TEST(GaussianBayesTreeOrdered, simpleMarginal)
TEST(GaussianBayesTree, simpleMarginal)
{
GaussianFactorGraphOrdered gfg;
GaussianFactorGraph gfg;
Matrix A12 = Rot2::fromDegrees(45.0).matrix();
@ -322,7 +322,7 @@ TEST(GaussianBayesTreeOrdered, simpleMarginal)
}
/* ************************************************************************* */
TEST(GaussianBayesTreeOrdered, shortcut_overlapping_separator)
TEST(GaussianBayesTree, shortcut_overlapping_separator)
{
// Test computing shortcuts when the separator overlaps. This previously
// would have highlighted a problem where information was duplicated.
@ -332,7 +332,7 @@ TEST(GaussianBayesTreeOrdered, shortcut_overlapping_separator)
// f(3,4,5)
// f(5,6)
// f(6,7)
GaussianFactorGraphOrdered fg;
GaussianFactorGraph fg;
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
fg.add(1, Matrix_(1,1, 1.0), 3, Matrix_(1,1, 2.0), 5, Matrix_(1,1, 3.0), Vector_(1, 4.0), model);
fg.add(1, Matrix_(1,1, 5.0), Vector_(1, 6.0), model);
@ -347,9 +347,9 @@ TEST(GaussianBayesTreeOrdered, shortcut_overlapping_separator)
// c(5|6)
// c(1,2|5)
// c(3,4|5)
GaussianBayesTreeOrdered bt = *GaussianMultifrontalSolver(fg).eliminate();
GaussianBayesTree bt = *GaussianMultifrontalSolver(fg).eliminate();
GaussianFactorGraphOrdered joint = *bt.joint(1,2, EliminateQROrdered);
GaussianFactorGraph joint = *bt.joint(1,2, EliminateQR);
Matrix expectedJointJ = (Matrix(2,3) <<
0, 11, 12,

274
tests/testGaussianFactorGraphB.cpp
View File

@ -47,19 +47,19 @@ using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, equals ) {
TEST( GaussianFactorGraph, equals ) {
OrderingOrdered ordering; ordering += X(1),X(2),L(1);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering);
GaussianFactorGraphOrdered fg2 = createGaussianFactorGraph(ordering);
Ordering ordering; ordering += X(1),X(2),L(1);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianFactorGraph fg2 = createGaussianFactorGraph(ordering);
EXPECT(fg.equals(fg2));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, error ) {
OrderingOrdered ordering; ordering += X(1),X(2),L(1);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering);
VectorValuesOrdered cfg = createZeroDelta(ordering);
TEST( GaussianFactorGraph, error ) {
Ordering ordering; ordering += X(1),X(2),L(1);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
VectorValues cfg = createZeroDelta(ordering);
// note the error is the same as in testNonlinearFactorGraph as a
// zero delta config in the linear graph is equivalent to noisy in
@ -69,71 +69,71 @@ TEST( GaussianFactorGraphOrdered, error ) {
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, eliminateOne_x1 )
TEST( GaussianFactorGraph, eliminateOne_x1 )
{
OrderingOrdered ordering; ordering += X(1),L(1),X(2);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering);
Ordering ordering; ordering += X(1),L(1),X(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditionalOrdered::shared_ptr conditional;
GaussianFactorGraphOrdered remaining;
boost::tie(conditional,remaining) = fg.eliminateOne(0, EliminateQROrdered);
GaussianConditional::shared_ptr conditional;
GaussianFactorGraph remaining;
boost::tie(conditional,remaining) = fg.eliminateOne(0, EliminateQR);
// create expected Conditional Gaussian
Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
GaussianConditionalOrdered expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
EXPECT(assert_equal(expected,*conditional,tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, eliminateOne_x2 )
TEST( GaussianFactorGraph, eliminateOne_x2 )
{
OrderingOrdered ordering; ordering += X(2),L(1),X(1);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering);
GaussianConditionalOrdered::shared_ptr actual = fg.eliminateOne(0, EliminateQROrdered).first;
Ordering ordering; ordering += X(2),L(1),X(1);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditional::shared_ptr actual = fg.eliminateOne(0, EliminateQR).first;
// create expected Conditional Gaussian
double sig = 0.0894427;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
GaussianConditionalOrdered expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma);
GaussianConditional expected(ordering[X(2)],d,R11,ordering[L(1)],S12,ordering[X(1)],S13,sigma);
EXPECT(assert_equal(expected,*actual,tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, eliminateOne_l1 )
TEST( GaussianFactorGraph, eliminateOne_l1 )
{
OrderingOrdered ordering; ordering += L(1),X(1),X(2);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering);
GaussianConditionalOrdered::shared_ptr actual = fg.eliminateOne(0, EliminateQROrdered).first;
Ordering ordering; ordering += L(1),X(1),X(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditional::shared_ptr actual = fg.eliminateOne(0, EliminateQR).first;
// create expected Conditional Gaussian
double sig = sqrt(2.0)/10.;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
GaussianConditionalOrdered expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
EXPECT(assert_equal(expected,*actual,tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, eliminateOne_x1_fast )
TEST( GaussianFactorGraph, eliminateOne_x1_fast )
{
OrderingOrdered ordering; ordering += X(1),L(1),X(2);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering);
GaussianConditionalOrdered::shared_ptr conditional;
GaussianFactorGraphOrdered remaining;
boost::tie(conditional,remaining) = fg.eliminateOne(ordering[X(1)], EliminateQROrdered);
Ordering ordering; ordering += X(1),L(1),X(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditional::shared_ptr conditional;
GaussianFactorGraph remaining;
boost::tie(conditional,remaining) = fg.eliminateOne(ordering[X(1)], EliminateQR);
// create expected Conditional Gaussian
Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I;
Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2);
GaussianConditionalOrdered expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
GaussianConditional expected(ordering[X(1)],15*d,R11,ordering[L(1)],S12,ordering[X(2)],S13,sigma);
// Create expected remaining new factor
JacobianFactorOrdered expectedFactor(1, Matrix_(4,2,
JacobianFactor expectedFactor(1, Matrix_(4,2,
4.714045207910318, 0.,
0., 4.714045207910318,
0., 0.,
@ -146,52 +146,52 @@ TEST( GaussianFactorGraphOrdered, eliminateOne_x1_fast )
Vector_(4, -0.707106781186547, 0.942809041582063, 0.707106781186547, -1.414213562373094), noiseModel::Unit::Create(4));
EXPECT(assert_equal(expected,*conditional,tol));
EXPECT(assert_equal((const GaussianFactorOrdered&)expectedFactor,*remaining.back(),tol));
EXPECT(assert_equal((const GaussianFactor&)expectedFactor,*remaining.back(),tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, eliminateOne_x2_fast )
TEST( GaussianFactorGraph, eliminateOne_x2_fast )
{
OrderingOrdered ordering; ordering += X(1),L(1),X(2);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering);
GaussianConditionalOrdered::shared_ptr actual = fg.eliminateOne(ordering[X(2)], EliminateQROrdered).first;
Ordering ordering; ordering += X(1),L(1),X(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditional::shared_ptr actual = fg.eliminateOne(ordering[X(2)], EliminateQR).first;
// create expected Conditional Gaussian
double sig = 0.0894427;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I;
Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2);
GaussianConditionalOrdered expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma);
GaussianConditional expected(ordering[X(2)],d,R11,ordering[X(1)],S13,ordering[L(1)],S12,sigma);
EXPECT(assert_equal(expected,*actual,tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, eliminateOne_l1_fast )
TEST( GaussianFactorGraph, eliminateOne_l1_fast )
{
OrderingOrdered ordering; ordering += X(1),L(1),X(2);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering);
GaussianConditionalOrdered::shared_ptr actual = fg.eliminateOne(ordering[L(1)], EliminateQROrdered).first;
Ordering ordering; ordering += X(1),L(1),X(2);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
GaussianConditional::shared_ptr actual = fg.eliminateOne(ordering[L(1)], EliminateQR).first;
// create expected Conditional Gaussian
double sig = sqrt(2.0)/10.;
Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I;
Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2);
GaussianConditionalOrdered expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
GaussianConditional expected(ordering[L(1)],d,R11,ordering[X(1)],S12,ordering[X(2)],S13,sigma);
EXPECT(assert_equal(expected,*actual,tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, eliminateAll )
TEST( GaussianFactorGraph, eliminateAll )
{
// create expected Chordal bayes Net
Matrix I = eye(2);
OrderingOrdered ordering;
Ordering ordering;
ordering += X(2),L(1),X(1);
Vector d1 = Vector_(2, -0.1,-0.1);
GaussianBayesNetOrdered expected = simpleGaussian(ordering[X(1)],d1,0.1);
GaussianBayesNet expected = simpleGaussian(ordering[X(1)],d1,0.1);
double sig1 = 0.149071;
Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2);
@ -202,108 +202,108 @@ TEST( GaussianFactorGraphOrdered, eliminateAll )
push_front(expected,ordering[X(2)],d3, I/sig2,ordering[L(1)], (-0.2)*I/sig2, ordering[X(1)], (-0.8)*I/sig2, sigma3);
// Check one ordering
GaussianFactorGraphOrdered fg1 = createGaussianFactorGraph(ordering);
GaussianBayesNetOrdered actual = *GaussianSequentialSolver(fg1).eliminate();
GaussianFactorGraph fg1 = createGaussianFactorGraph(ordering);
GaussianBayesNet actual = *GaussianSequentialSolver(fg1).eliminate();
EXPECT(assert_equal(expected,actual,tol));
GaussianBayesNetOrdered actualQR = *GaussianSequentialSolver(fg1, true).eliminate();
GaussianBayesNet actualQR = *GaussianSequentialSolver(fg1, true).eliminate();
EXPECT(assert_equal(expected,actualQR,tol));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, copying )
TEST( GaussianFactorGraph, copying )
{
// Create a graph
OrderingOrdered ordering; ordering += X(2),L(1),X(1);
GaussianFactorGraphOrdered actual = createGaussianFactorGraph(ordering);
Ordering ordering; ordering += X(2),L(1),X(1);
GaussianFactorGraph actual = createGaussianFactorGraph(ordering);
// Copy the graph !
GaussianFactorGraphOrdered copy = actual;
GaussianFactorGraph copy = actual;
// now eliminate the copy
GaussianBayesNetOrdered actual1 = *GaussianSequentialSolver(copy).eliminate();
GaussianBayesNet actual1 = *GaussianSequentialSolver(copy).eliminate();
// Create the same graph, but not by copying
GaussianFactorGraphOrdered expected = createGaussianFactorGraph(ordering);
GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
// and check that original is still the same graph
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, CONSTRUCTOR_GaussianBayesNet )
TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
{
OrderingOrdered ord;
Ordering ord;
ord += X(2),L(1),X(1);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ord);
GaussianFactorGraph fg = createGaussianFactorGraph(ord);
// render with a given ordering
GaussianBayesNetOrdered CBN = *GaussianSequentialSolver(fg).eliminate();
GaussianBayesNet CBN = *GaussianSequentialSolver(fg).eliminate();
// True GaussianFactorGraph
GaussianFactorGraphOrdered fg2(CBN);
GaussianBayesNetOrdered CBN2 = *GaussianSequentialSolver(fg2).eliminate();
GaussianFactorGraph fg2(CBN);
GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate();
EXPECT(assert_equal(CBN,CBN2));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, getOrdering)
TEST( GaussianFactorGraph, getOrdering)
{
OrderingOrdered original; original += L(1),X(1),X(2);
FactorGraphOrdered<IndexFactorOrdered> symbolic(createGaussianFactorGraph(original));
Permutation perm(*inference::PermutationCOLAMD(VariableIndexOrdered(symbolic)));
OrderingOrdered actual = original; actual.permuteInPlace(perm);
OrderingOrdered expected; expected += L(1),X(2),X(1);
Ordering original; original += L(1),X(1),X(2);
FactorGraph<IndexFactor> symbolic(createGaussianFactorGraph(original));
Permutation perm(*inference::PermutationCOLAMD(VariableIndex(symbolic)));
Ordering actual = original; actual.permuteInPlace(perm);
Ordering expected; expected += L(1),X(2),X(1);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, optimize_Cholesky )
TEST( GaussianFactorGraph, optimize_Cholesky )
{
// create an ordering
OrderingOrdered ord; ord += X(2),L(1),X(1);
Ordering ord; ord += X(2),L(1),X(1);
// create a graph
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ord);
GaussianFactorGraph fg = createGaussianFactorGraph(ord);
// optimize the graph
VectorValuesOrdered actual = *GaussianSequentialSolver(fg, false).optimize();
VectorValues actual = *GaussianSequentialSolver(fg, false).optimize();
// verify
VectorValuesOrdered expected = createCorrectDelta(ord);
VectorValues expected = createCorrectDelta(ord);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, optimize_QR )
TEST( GaussianFactorGraph, optimize_QR )
{
// create an ordering
OrderingOrdered ord; ord += X(2),L(1),X(1);
Ordering ord; ord += X(2),L(1),X(1);
// create a graph
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ord);
GaussianFactorGraph fg = createGaussianFactorGraph(ord);
// optimize the graph
VectorValuesOrdered actual = *GaussianSequentialSolver(fg, true).optimize();
VectorValues actual = *GaussianSequentialSolver(fg, true).optimize();
// verify
VectorValuesOrdered expected = createCorrectDelta(ord);
VectorValues expected = createCorrectDelta(ord);
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, combine)
TEST( GaussianFactorGraph, combine)
{
// create an ordering
OrderingOrdered ord; ord += X(2),L(1),X(1);
Ordering ord; ord += X(2),L(1),X(1);
// create a test graph
GaussianFactorGraphOrdered fg1 = createGaussianFactorGraph(ord);
GaussianFactorGraph fg1 = createGaussianFactorGraph(ord);
// create another factor graph
GaussianFactorGraphOrdered fg2 = createGaussianFactorGraph(ord);
GaussianFactorGraph fg2 = createGaussianFactorGraph(ord);
// get sizes
size_t size1 = fg1.size();
@ -316,23 +316,23 @@ TEST( GaussianFactorGraphOrdered, combine)
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, combine2)
TEST( GaussianFactorGraph, combine2)
{
// create an ordering
OrderingOrdered ord; ord += X(2),L(1),X(1);
Ordering ord; ord += X(2),L(1),X(1);
// create a test graph
GaussianFactorGraphOrdered fg1 = createGaussianFactorGraph(ord);
GaussianFactorGraph fg1 = createGaussianFactorGraph(ord);
// create another factor graph
GaussianFactorGraphOrdered fg2 = createGaussianFactorGraph(ord);
GaussianFactorGraph fg2 = createGaussianFactorGraph(ord);
// get sizes
size_t size1 = fg1.size();
size_t size2 = fg2.size();
// combine them
GaussianFactorGraphOrdered fg3 = GaussianFactorGraphOrdered::combine2(fg1, fg2);
GaussianFactorGraph fg3 = GaussianFactorGraph::combine2(fg1, fg2);
EXPECT(size1+size2 == fg3.size());
}
@ -346,31 +346,31 @@ void print(vector<int> v) {
}
/* ************************************************************************* */
TEST(GaussianFactorGraphOrdered, createSmoother)
TEST(GaussianFactorGraph, createSmoother)
{
GaussianFactorGraphOrdered fg1 = createSmoother(2).first;
GaussianFactorGraph fg1 = createSmoother(2).first;
LONGS_EQUAL(3,fg1.size());
GaussianFactorGraphOrdered fg2 = createSmoother(3).first;
GaussianFactorGraph fg2 = createSmoother(3).first;
LONGS_EQUAL(5,fg2.size());
}
/* ************************************************************************* */
double error(const VectorValuesOrdered& x) {
double error(const VectorValues& x) {
// create an ordering
OrderingOrdered ord; ord += X(2),L(1),X(1);
Ordering ord; ord += X(2),L(1),X(1);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ord);
GaussianFactorGraph fg = createGaussianFactorGraph(ord);
return fg.error(x);
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, multiplication )
TEST( GaussianFactorGraph, multiplication )
{
// create an ordering
OrderingOrdered ord; ord += X(2),L(1),X(1);
Ordering ord; ord += X(2),L(1),X(1);
GaussianFactorGraphOrdered A = createGaussianFactorGraph(ord);
VectorValuesOrdered x = createCorrectDelta(ord);
GaussianFactorGraph A = createGaussianFactorGraph(ord);
VectorValues x = createCorrectDelta(ord);
Errors actual = A * x;
Errors expected;
expected += Vector_(2,-1.0,-1.0);
@ -382,12 +382,12 @@ TEST( GaussianFactorGraphOrdered, multiplication )
/* ************************************************************************* */
// Extra test on elimination prompted by Michael's email to Frank 1/4/2010
TEST( GaussianFactorGraphOrdered, elimination )
TEST( GaussianFactorGraph, elimination )
{
OrderingOrdered ord;
Ordering ord;
ord += X(1), X(2);
// Create Gaussian Factor Graph
GaussianFactorGraphOrdered fg;
GaussianFactorGraph fg;
Matrix Ap = eye(1), An = eye(1) * -1;
Vector b = Vector_(1, 0.0);
SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,2.0);
@ -396,7 +396,7 @@ TEST( GaussianFactorGraphOrdered, elimination )
fg.add(ord[X(2)], Ap, b, sigma);
// Eliminate
GaussianBayesNetOrdered bayesNet = *GaussianSequentialSolver(fg).eliminate();
GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate();
// Check sigma
EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord[X(2)]]->get_sigmas()(0),1e-5);
@ -416,48 +416,48 @@ TEST( GaussianFactorGraphOrdered, elimination )
/* ************************************************************************* */
// Tests ported from ConstrainedGaussianFactorGraph
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, constrained_simple )
TEST( GaussianFactorGraph, constrained_simple )
{
// get a graph with a constraint in it
GaussianFactorGraphOrdered fg = createSimpleConstraintGraph();
GaussianFactorGraph fg = createSimpleConstraintGraph();
EXPECT(hasConstraints(fg));
// eliminate and solve
VectorValuesOrdered actual = *GaussianSequentialSolver(fg).optimize();
VectorValues actual = *GaussianSequentialSolver(fg).optimize();
// verify
VectorValuesOrdered expected = createSimpleConstraintValues();
VectorValues expected = createSimpleConstraintValues();
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, constrained_single )
TEST( GaussianFactorGraph, constrained_single )
{
// get a graph with a constraint in it
GaussianFactorGraphOrdered fg = createSingleConstraintGraph();
GaussianFactorGraph fg = createSingleConstraintGraph();
EXPECT(hasConstraints(fg));
// eliminate and solve
VectorValuesOrdered actual = *GaussianSequentialSolver(fg).optimize();
VectorValues actual = *GaussianSequentialSolver(fg).optimize();
// verify
VectorValuesOrdered expected = createSingleConstraintValues();
VectorValues expected = createSingleConstraintValues();
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, constrained_multi1 )
TEST( GaussianFactorGraph, constrained_multi1 )
{
// get a graph with a constraint in it
GaussianFactorGraphOrdered fg = createMultiConstraintGraph();
GaussianFactorGraph fg = createMultiConstraintGraph();
EXPECT(hasConstraints(fg));
// eliminate and solve
VectorValuesOrdered actual = *GaussianSequentialSolver(fg).optimize();
VectorValues actual = *GaussianSequentialSolver(fg).optimize();
// verify
VectorValuesOrdered expected = createMultiConstraintValues();
VectorValues expected = createMultiConstraintValues();
EXPECT(assert_equal(expected, actual));
}
@ -466,27 +466,27 @@ TEST( GaussianFactorGraphOrdered, constrained_multi1 )
static SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1);
/* ************************************************************************* */
TEST(GaussianFactorGraphOrdered, replace)
TEST(GaussianFactorGraph, replace)
{
OrderingOrdered ord; ord += X(1),X(2),X(3),X(4),X(5),X(6);
Ordering ord; ord += X(1),X(2),X(3),X(4),X(5),X(6);
SharedDiagonal noise(noiseModel::Isotropic::Sigma(3, 1.0));
GaussianFactorGraphOrdered::sharedFactor f1(new JacobianFactorOrdered(
GaussianFactorGraph::sharedFactor f1(new JacobianFactor(
ord[X(1)], eye(3,3), ord[X(2)], eye(3,3), zero(3), noise));
GaussianFactorGraphOrdered::sharedFactor f2(new JacobianFactorOrdered(
GaussianFactorGraph::sharedFactor f2(new JacobianFactor(
ord[X(2)], eye(3,3), ord[X(3)], eye(3,3), zero(3), noise));
GaussianFactorGraphOrdered::sharedFactor f3(new JacobianFactorOrdered(
GaussianFactorGraph::sharedFactor f3(new JacobianFactor(
ord[X(3)], eye(3,3), ord[X(4)], eye(3,3), zero(3), noise));
GaussianFactorGraphOrdered::sharedFactor f4(new JacobianFactorOrdered(
GaussianFactorGraph::sharedFactor f4(new JacobianFactor(
ord[X(5)], eye(3,3), ord[X(6)], eye(3,3), zero(3), noise));
GaussianFactorGraphOrdered actual;
GaussianFactorGraph actual;
actual.push_back(f1);
actual.push_back(f2);
actual.push_back(f3);
actual.replace(0, f4);
GaussianFactorGraphOrdered expected;
GaussianFactorGraph expected;
expected.push_back(f4);
expected.push_back(f2);
expected.push_back(f3);
@ -495,35 +495,35 @@ TEST(GaussianFactorGraphOrdered, replace)
}
/* ************************************************************************* */
TEST(GaussianFactorGraphOrdered, createSmoother2)
TEST(GaussianFactorGraph, createSmoother2)
{
using namespace example;
GaussianFactorGraphOrdered fg2;
OrderingOrdered ordering;
GaussianFactorGraph fg2;
Ordering ordering;
boost::tie(fg2,ordering) = createSmoother(3);
LONGS_EQUAL(5,fg2.size());
// eliminate
vector<Index> x3var; x3var.push_back(ordering[X(3)]);
vector<Index> x1var; x1var.push_back(ordering[X(1)]);
GaussianBayesNetOrdered p_x3 = *GaussianSequentialSolver(
GaussianBayesNet p_x3 = *GaussianSequentialSolver(
*GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate();
GaussianBayesNetOrdered p_x1 = *GaussianSequentialSolver(
GaussianBayesNet p_x1 = *GaussianSequentialSolver(
*GaussianSequentialSolver(fg2).jointFactorGraph(x1var)).eliminate();
CHECK(assert_equal(*p_x1.back(),*p_x3.front())); // should be the same because of symmetry
}
/* ************************************************************************* */
TEST(GaussianFactorGraphOrdered, hasConstraints)
TEST(GaussianFactorGraph, hasConstraints)
{
FactorGraphOrdered<GaussianFactorOrdered> fgc1 = createMultiConstraintGraph();
FactorGraph<GaussianFactor> fgc1 = createMultiConstraintGraph();
EXPECT(hasConstraints(fgc1));
FactorGraphOrdered<GaussianFactorOrdered> fgc2 = createSimpleConstraintGraph() ;
FactorGraph<GaussianFactor> fgc2 = createSimpleConstraintGraph() ;
EXPECT(hasConstraints(fgc2));
OrderingOrdered ordering; ordering += X(1), X(2), L(1);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering);
Ordering ordering; ordering += X(1), X(2), L(1);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
EXPECT(!hasConstraints(fg));
}
@ -534,7 +534,7 @@ TEST(GaussianFactorGraphOrdered, hasConstraints)
#include <gtsam/linear/GaussianMultifrontalSolver.h>
/* ************************************************************************* */
TEST( GaussianFactorGraphOrdered, conditional_sigma_failure) {
TEST( GaussianFactorGraph, conditional_sigma_failure) {
// This system derives from a failure case in DDF in which a Bayes Tree
// has non-unit sigmas for conditionals in the Bayes Tree, which
// should never happen by construction
@ -578,17 +578,17 @@ TEST( GaussianFactorGraphOrdered, conditional_sigma_failure) {
factors.add(RangeFactor<Pose3,Point3>(xC1, l32, relElevation, elevationModel));
factors.add(RangeFactor<Pose3,Point3>(xC1, l41, relElevation, elevationModel));
OrderingOrdered orderingC; orderingC += xC1, l32, l41;
Ordering orderingC; orderingC += xC1, l32, l41;
// Check that sigmas are correct (i.e., unit)
GaussianFactorGraphOrdered lfg = *factors.linearize(initValues, orderingC);
GaussianFactorGraph lfg = *factors.linearize(initValues, orderingC);
GaussianMultifrontalSolver solver(lfg, false);
GaussianBayesTreeOrdered actBT = *solver.eliminate();
GaussianBayesTree actBT = *solver.eliminate();
// Check that all sigmas in an unconstrained bayes tree are set to one
BOOST_FOREACH(const GaussianBayesTreeOrdered::sharedClique& clique, actBT.nodes()) {
GaussianConditionalOrdered::shared_ptr conditional = clique->conditional();
BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, actBT.nodes()) {
GaussianConditional::shared_ptr conditional = clique->conditional();
size_t dim = conditional->dim();
EXPECT(assert_equal(gtsam::ones(dim), conditional->get_sigmas(), tol));
}

158
tests/testGaussianISAM2.cpp
View File

@ -152,21 +152,21 @@ TEST_UNSAFE(ISAM2, ImplAddVariables) {
Values newTheta;
newTheta.insert(1, Pose2(.6, .7, .8));
VectorValuesOrdered delta;
VectorValues delta;
delta.insert(0, Vector_(3, .1, .2, .3));
delta.insert(1, Vector_(2, .4, .5));
VectorValuesOrdered deltaNewton;
VectorValues deltaNewton;
deltaNewton.insert(0, Vector_(3, .1, .2, .3));
deltaNewton.insert(1, Vector_(2, .4, .5));
VectorValuesOrdered deltaRg;
VectorValues deltaRg;
deltaRg.insert(0, Vector_(3, .1, .2, .3));
deltaRg.insert(1, Vector_(2, .4, .5));
vector<bool> replacedKeys(2, false);
OrderingOrdered ordering; ordering += 100, 0;
Ordering ordering; ordering += 100, 0;
// Verify initial state
LONGS_EQUAL(0, ordering[100]);
@ -180,24 +180,24 @@ TEST_UNSAFE(ISAM2, ImplAddVariables) {
thetaExpected.insert(100, Point2(.4, .5));
thetaExpected.insert(1, Pose2(.6, .7, .8));
VectorValuesOrdered deltaExpected;
VectorValues deltaExpected;
deltaExpected.insert(0, Vector_(3, .1, .2, .3));
deltaExpected.insert(1, Vector_(2, .4, .5));
deltaExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
VectorValuesOrdered deltaNewtonExpected;
VectorValues deltaNewtonExpected;
deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3));
deltaNewtonExpected.insert(1, Vector_(2, .4, .5));
deltaNewtonExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
VectorValuesOrdered deltaRgExpected;
VectorValues deltaRgExpected;
deltaRgExpected.insert(0, Vector_(3, .1, .2, .3));
deltaRgExpected.insert(1, Vector_(2, .4, .5));
deltaRgExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0));
vector<bool> replacedKeysExpected(3, false);
OrderingOrdered orderingExpected; orderingExpected += 100, 0, 1;
Ordering orderingExpected; orderingExpected += 100, 0, 1;
// Expand initial state
ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering);
@ -219,22 +219,22 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) {
theta.insert(1, Pose2(.6, .7, .8));
theta.insert(100, Point2(.4, .5));
SymbolicFactorGraphOrdered sfg;
sfg.push_back(boost::make_shared<IndexFactorOrdered>(Index(0), Index(2)));
sfg.push_back(boost::make_shared<IndexFactorOrdered>(Index(0), Index(1)));
VariableIndexOrdered variableIndex(sfg);
SymbolicFactorGraph sfg;
sfg.push_back(boost::make_shared<IndexFactor>(Index(0), Index(2)));
sfg.push_back(boost::make_shared<IndexFactor>(Index(0), Index(1)));
VariableIndex variableIndex(sfg);
VectorValuesOrdered delta;
VectorValues delta;
delta.insert(0, Vector_(3, .1, .2, .3));
delta.insert(1, Vector_(3, .4, .5, .6));
delta.insert(2, Vector_(2, .7, .8));
VectorValuesOrdered deltaNewton;
VectorValues deltaNewton;
deltaNewton.insert(0, Vector_(3, .1, .2, .3));
deltaNewton.insert(1, Vector_(3, .4, .5, .6));
deltaNewton.insert(2, Vector_(2, .7, .8));
VectorValuesOrdered deltaRg;
VectorValues deltaRg;
deltaRg.insert(0, Vector_(3, .1, .2, .3));
deltaRg.insert(1, Vector_(3, .4, .5, .6));
deltaRg.insert(2, Vector_(2, .7, .8));
@ -244,7 +244,7 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) {
replacedKeys[1] = false;
replacedKeys[2] = true;
OrderingOrdered ordering; ordering += 100, 1, 0;
Ordering ordering; ordering += 100, 1, 0;
ISAM2::Nodes nodes(3);
@ -258,26 +258,26 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) {
thetaExpected.insert(0, Pose2(.1, .2, .3));
thetaExpected.insert(100, Point2(.4, .5));
SymbolicFactorGraphOrdered sfgRemoved;
sfgRemoved.push_back(boost::make_shared<IndexFactorOrdered>(Index(0), Index(1)));
sfgRemoved.push_back(SymbolicFactorGraphOrdered::sharedFactor()); // Add empty factor to keep factor indices consistent
VariableIndexOrdered variableIndexExpected(sfgRemoved);
SymbolicFactorGraph sfgRemoved;
sfgRemoved.push_back(boost::make_shared<IndexFactor>(Index(0), Index(1)));
sfgRemoved.push_back(SymbolicFactorGraph::sharedFactor()); // Add empty factor to keep factor indices consistent
VariableIndex variableIndexExpected(sfgRemoved);
VectorValuesOrdered deltaExpected;
VectorValues deltaExpected;
deltaExpected.insert(0, Vector_(3, .1, .2, .3));
deltaExpected.insert(1, Vector_(2, .7, .8));
VectorValuesOrdered deltaNewtonExpected;
VectorValues deltaNewtonExpected;
deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3));
deltaNewtonExpected.insert(1, Vector_(2, .7, .8));
VectorValuesOrdered deltaRgExpected;
VectorValues deltaRgExpected;
deltaRgExpected.insert(0, Vector_(3, .1, .2, .3));
deltaRgExpected.insert(1, Vector_(2, .7, .8));
vector<bool> replacedKeysExpected(2, true);
OrderingOrdered orderingExpected; orderingExpected += 100, 0;
Ordering orderingExpected; orderingExpected += 100, 0;
ISAM2::Nodes nodesExpected(2);
@ -285,9 +285,9 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) {
FastSet<Key> unusedKeys;
unusedKeys.insert(1);
vector<size_t> removedFactorsI; removedFactorsI.push_back(1);
SymbolicFactorGraphOrdered removedFactors; removedFactors.push_back(sfg[1]);
SymbolicFactorGraph removedFactors; removedFactors.push_back(sfg[1]);
variableIndex.remove(removedFactorsI, removedFactors);
GaussianFactorGraphOrdered linearFactors;
GaussianFactorGraph linearFactors;
FastSet<Key> fixedVariables;
ISAM2::Impl::RemoveVariables(unusedKeys, ISAM2::sharedClique(), theta, variableIndex, delta, deltaNewton, deltaRg,
replacedKeys, ordering, nodes, linearFactors, fixedVariables);
@ -307,7 +307,7 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) {
// using namespace gtsam::planarSLAM;
// typedef GaussianISAM2<Values>::Impl Impl;
//
// OrderingOrdered ordering; ordering += (0), (0), (1);
// Ordering ordering; ordering += (0), (0), (1);
// NonlinearFactorGraph graph;
// graph.add(PriorFactor<Pose2>((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension));
// graph.addRange((0), (0), 1.0, noiseModel::Unit::Create(1));
@ -327,7 +327,7 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) {
// typedef GaussianISAM2<Values>::Impl Impl;
//
// // Create values where indices 1 and 3 are above the threshold of 0.1
// VectorValuesOrdered values;
// VectorValues values;
// values.reserve(4, 10);
// values.push_back_preallocated(Vector_(2, 0.09, 0.09));
// values.push_back_preallocated(Vector_(3, 0.11, 0.11, 0.09));
@ -341,7 +341,7 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) {
// permutation[2] = 1;
// permutation[3] = 3;
//
// Permuted<VectorValuesOrdered> permuted(permutation, values);
// Permuted<VectorValues> permuted(permutation, values);
//
// // After permutation, the indices above the threshold are 2 and 2
// FastSet<Index> expected;
@ -367,19 +367,19 @@ TEST(ISAM2, optimize2) {
10, 0.0, 0.0,
0.0, 10, 0.0,
0.0, 0.0, 31.8309886;
GaussianConditionalOrdered::shared_ptr conditional(new GaussianConditionalOrdered(0, d, R, Vector::Ones(3)));
GaussianConditional::shared_ptr conditional(new GaussianConditional(0, d, R, Vector::Ones(3)));
// Create ordering
OrderingOrdered ordering; ordering += (0);
Ordering ordering; ordering += (0);
// Expected vector
VectorValuesOrdered expected(1, 3);
VectorValues expected(1, 3);
conditional->solveInPlace(expected);
// Clique
ISAM2::sharedClique clique(
ISAM2::Clique::Create(make_pair(conditional,GaussianFactorOrdered::shared_ptr())));
VectorValuesOrdered actual(theta.dims(ordering));
ISAM2::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr())));
VectorValues actual(theta.dims(ordering));
internal::optimizeInPlace<ISAM2::Base>(clique, actual);
// expected.print("expected: ");
@ -394,12 +394,12 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
const std::string name_ = test.getName();
Values actual = isam.calculateEstimate();
OrderingOrdered ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first;
GaussianFactorGraphOrdered linearized = *fullgraph.linearize(fullinit, ordering);
Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first;
GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering);
// linearized.print("Expected linearized: ");
GaussianBayesNetOrdered gbn = *GaussianSequentialSolver(linearized).eliminate();
GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();
// gbn.print("Expected bayesnet: ");
VectorValuesOrdered delta = optimize(gbn);
VectorValues delta = optimize(gbn);
Values expected = fullinit.retract(delta, ordering);
bool isamEqual = assert_equal(expected, actual);
@ -411,13 +411,13 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraphOrdered<JacobianFactorOrdered> jfg;
jfg.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(*clique->conditional())));
VectorValuesOrdered expectedGradient(*allocateVectorValues(isam));
FactorGraph<JacobianFactor> jfg;
jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(jfg, expectedGradient);
// Compare with actual gradients
int variablePosition = 0;
for(GaussianConditionalOrdered::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
const int dim = clique->conditional()->dim(jit);
Vector actual = clique->gradientContribution().segment(variablePosition, dim);
bool gradOk = assert_equal(expectedGradient[*jit], actual);
@ -431,10 +431,10 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
}
// Check gradient
VectorValuesOrdered expectedGradient(*allocateVectorValues(isam));
gradientAtZero(FactorGraphOrdered<JacobianFactorOrdered>(isam), expectedGradient);
VectorValuesOrdered expectedGradient2(gradient(FactorGraphOrdered<JacobianFactorOrdered>(isam), VectorValuesOrdered::Zero(expectedGradient)));
VectorValuesOrdered actualGradient(*allocateVectorValues(isam));
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
VectorValues actualGradient(*allocateVectorValues(isam));
gradientAtZero(isam, actualGradient);
bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient);
EXPECT(expectedGradOk);
@ -531,49 +531,49 @@ TEST(ISAM2, permute_cached) {
typedef boost::shared_ptr<ISAM2Clique> sharedISAM2Clique;
// Construct expected permuted BayesTree (variable 2 has been changed to 1)
BayesTreeOrdered<GaussianConditionalOrdered, ISAM2Clique> expected;
BayesTree<GaussianConditional, ISAM2Clique> expected;
expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
boost::make_shared<GaussianConditionalOrdered>(pair_list_of
boost::make_shared<GaussianConditional>(pair_list_of
(3, Matrix_(1,1,1.0))
(4, Matrix_(1,1,2.0)),
2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4)
HessianFactorOrdered::shared_ptr())))); // Cached: empty
HessianFactor::shared_ptr())))); // Cached: empty
expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
boost::make_shared<GaussianConditionalOrdered>(pair_list_of
boost::make_shared<GaussianConditional>(pair_list_of
(2, Matrix_(1,1,1.0))
(3, Matrix_(1,1,2.0)),
1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3)
boost::make_shared<HessianFactorOrdered>(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3)
boost::make_shared<HessianFactor>(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3)
expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
boost::make_shared<GaussianConditionalOrdered>(pair_list_of
boost::make_shared<GaussianConditional>(pair_list_of
(0, Matrix_(1,1,1.0))
(2, Matrix_(1,1,2.0)),
1, Vector_(1,1.0), Vector_(1,1.0)), // p(0|2)
boost::make_shared<HessianFactorOrdered>(1, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(1)
boost::make_shared<HessianFactor>(1, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(1)
// Change variable 2 to 1
expected.root()->children().front()->conditional()->keys()[0] = 1;
expected.root()->children().front()->children().front()->conditional()->keys()[1] = 1;
// Construct unpermuted BayesTree
BayesTreeOrdered<GaussianConditionalOrdered, ISAM2Clique> actual;
BayesTree<GaussianConditional, ISAM2Clique> actual;
actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
boost::make_shared<GaussianConditionalOrdered>(pair_list_of
boost::make_shared<GaussianConditional>(pair_list_of
(3, Matrix_(1,1,1.0))
(4, Matrix_(1,1,2.0)),
2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4)
HessianFactorOrdered::shared_ptr())))); // Cached: empty
HessianFactor::shared_ptr())))); // Cached: empty
actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
boost::make_shared<GaussianConditionalOrdered>(pair_list_of
boost::make_shared<GaussianConditional>(pair_list_of
(2, Matrix_(1,1,1.0))
(3, Matrix_(1,1,2.0)),
1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3)
boost::make_shared<HessianFactorOrdered>(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3)
boost::make_shared<HessianFactor>(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3)
actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair(
boost::make_shared<GaussianConditionalOrdered>(pair_list_of
boost::make_shared<GaussianConditional>(pair_list_of
(0, Matrix_(1,1,1.0))
(2, Matrix_(1,1,2.0)),
1, Vector_(1,1.0), Vector_(1,1.0)), // p(0|2)
boost::make_shared<HessianFactorOrdered>(2, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(2)
boost::make_shared<HessianFactor>(2, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(2)
// Create permutation that changes variable 2 -> 0
Permutation permutation = Permutation::Identity(5);
@ -664,13 +664,13 @@ TEST_UNSAFE(ISAM2, swapFactors)
typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraphOrdered<JacobianFactorOrdered> jfg;
jfg.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(*clique->conditional())));
VectorValuesOrdered expectedGradient(*allocateVectorValues(isam));
FactorGraph<JacobianFactor> jfg;
jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(jfg, expectedGradient);
// Compare with actual gradients
int variablePosition = 0;
for(GaussianConditionalOrdered::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
const int dim = clique->conditional()->dim(jit);
Vector actual = clique->gradientContribution().segment(variablePosition, dim);
EXPECT(assert_equal(expectedGradient[*jit], actual));
@ -680,10 +680,10 @@ TEST_UNSAFE(ISAM2, swapFactors)
}
// Check gradient
VectorValuesOrdered expectedGradient(*allocateVectorValues(isam));
gradientAtZero(FactorGraphOrdered<JacobianFactorOrdered>(isam), expectedGradient);
VectorValuesOrdered expectedGradient2(gradient(FactorGraphOrdered<JacobianFactorOrdered>(isam), VectorValuesOrdered::Zero(expectedGradient)));
VectorValuesOrdered actualGradient(*allocateVectorValues(isam));
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
VectorValues actualGradient(*allocateVectorValues(isam));
gradientAtZero(isam, actualGradient);
EXPECT(assert_equal(expectedGradient2, expectedGradient));
EXPECT(assert_equal(expectedGradient, actualGradient));
@ -795,13 +795,13 @@ TEST(ISAM2, constrained_ordering)
typedef ISAM2::sharedClique sharedClique;
BOOST_FOREACH(const sharedClique& clique, isam.nodes()) {
// Compute expected gradient
FactorGraphOrdered<JacobianFactorOrdered> jfg;
jfg.push_back(JacobianFactorOrdered::shared_ptr(new JacobianFactorOrdered(*clique->conditional())));
VectorValuesOrdered expectedGradient(*allocateVectorValues(isam));
FactorGraph<JacobianFactor> jfg;
jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional())));
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(jfg, expectedGradient);
// Compare with actual gradients
int variablePosition = 0;
for(GaussianConditionalOrdered::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) {
const int dim = clique->conditional()->dim(jit);
Vector actual = clique->gradientContribution().segment(variablePosition, dim);
EXPECT(assert_equal(expectedGradient[*jit], actual));
@ -811,10 +811,10 @@ TEST(ISAM2, constrained_ordering)
}
// Check gradient
VectorValuesOrdered expectedGradient(*allocateVectorValues(isam));
gradientAtZero(FactorGraphOrdered<JacobianFactorOrdered>(isam), expectedGradient);
VectorValuesOrdered expectedGradient2(gradient(FactorGraphOrdered<JacobianFactorOrdered>(isam), VectorValuesOrdered::Zero(expectedGradient)));
VectorValuesOrdered actualGradient(*allocateVectorValues(isam));
VectorValues expectedGradient(*allocateVectorValues(isam));
gradientAtZero(FactorGraph<JacobianFactor>(isam), expectedGradient);
VectorValues expectedGradient2(gradient(FactorGraph<JacobianFactor>(isam), VectorValues::Zero(expectedGradient)));
VectorValues actualGradient(*allocateVectorValues(isam));
gradientAtZero(isam, actualGradient);
EXPECT(assert_equal(expectedGradient2, expectedGradient));
EXPECT(assert_equal(expectedGradient, actualGradient));
@ -844,9 +844,9 @@ namespace {
toKeep.push_back(i);
// Calculate expected marginal from iSAM2 tree
GaussianFactorGraphOrdered isamAsGraph(isam);
GaussianFactorGraph isamAsGraph(isam);
GaussianSequentialSolver solver(isamAsGraph);
GaussianFactorGraphOrdered marginalgfg = *solver.jointFactorGraph(toKeep);
GaussianFactorGraph marginalgfg = *solver.jointFactorGraph(toKeep);
expectedAugmentedHessian = marginalgfg.augmentedHessian();
//// Calculate expected marginal from cached linear factors
@ -864,7 +864,7 @@ namespace {
isam.marginalizeLeaves(leafKeys);
// Check
GaussianFactorGraphOrdered actualMarginalGraph(isam);
GaussianFactorGraph actualMarginalGraph(isam);
Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian();
//Matrix actual2AugmentedHessian = linearFactors_.augmentedHessian();
Matrix actual3AugmentedHessian = isam.getFactorsUnsafe().linearize(

62
tests/testGaussianJunctionTreeB.cpp
View File

@ -59,11 +59,11 @@ using symbol_shorthand::L;
TEST( GaussianJunctionTreeB, constructor2 )
{
// create a graph
OrderingOrdered ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraphOrdered fg = createSmoother(7, ordering).first;
Ordering ordering; ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraph fg = createSmoother(7, ordering).first;
// create an ordering
GaussianJunctionTreeOrdered actual(fg);
GaussianJunctionTree actual(fg);
vector<Index> frontal1; frontal1 += ordering[X(5)], ordering[X(6)], ordering[X(4)];
vector<Index> frontal2; frontal2 += ordering[X(3)], ordering[X(2)];
@ -76,10 +76,10 @@ TEST( GaussianJunctionTreeB, constructor2 )
EXPECT(assert_equal(frontal1, actual.root()->frontal));
EXPECT(assert_equal(sep1, actual.root()->separator));
LONGS_EQUAL(5, actual.root()->size());
list<GaussianJunctionTreeOrdered::sharedClique>::const_iterator child0it = actual.root()->children().begin();
list<GaussianJunctionTreeOrdered::sharedClique>::const_iterator child1it = child0it; ++child1it;
GaussianJunctionTreeOrdered::sharedClique child0 = *child0it;
GaussianJunctionTreeOrdered::sharedClique child1 = *child1it;
list<GaussianJunctionTree::sharedClique>::const_iterator child0it = actual.root()->children().begin();
list<GaussianJunctionTree::sharedClique>::const_iterator child1it = child0it; ++child1it;
GaussianJunctionTree::sharedClique child0 = *child0it;
GaussianJunctionTree::sharedClique child1 = *child1it;
EXPECT(assert_equal(frontal2, child0->frontal));
EXPECT(assert_equal(sep2, child0->separator));
LONGS_EQUAL(4, child0->size());
@ -95,16 +95,16 @@ TEST( GaussianJunctionTreeB, constructor2 )
TEST( GaussianJunctionTreeB, optimizeMultiFrontal )
{
// create a graph
GaussianFactorGraphOrdered fg;
OrderingOrdered ordering;
GaussianFactorGraph fg;
Ordering ordering;
boost::tie(fg,ordering) = createSmoother(7);
// optimize the graph
GaussianJunctionTreeOrdered tree(fg);
VectorValuesOrdered actual = tree.optimize(&EliminateQROrdered);
GaussianJunctionTree tree(fg);
VectorValues actual = tree.optimize(&EliminateQR);
// verify
VectorValuesOrdered expected(vector<size_t>(7,2)); // expected solution
VectorValues expected(vector<size_t>(7,2)); // expected solution
Vector v = Vector_(2, 0., 0.);
for (int i=1; i<=7; i++)
expected[ordering[X(i)]] = v;
@ -117,15 +117,15 @@ TEST( GaussianJunctionTreeB, optimizeMultiFrontal2)
// create a graph
example::Graph nlfg = createNonlinearFactorGraph();
Values noisy = createNoisyValues();
OrderingOrdered ordering; ordering += X(1),X(2),L(1);
GaussianFactorGraphOrdered fg = *nlfg.linearize(noisy, ordering);
Ordering ordering; ordering += X(1),X(2),L(1);
GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering);
// optimize the graph
GaussianJunctionTreeOrdered tree(fg);
VectorValuesOrdered actual = tree.optimize(&EliminateQROrdered);
GaussianJunctionTree tree(fg);
VectorValues actual = tree.optimize(&EliminateQR);
// verify
VectorValuesOrdered expected = createCorrectDelta(ordering); // expected solution
VectorValues expected = createCorrectDelta(ordering); // expected solution
EXPECT(assert_equal(expected,actual));
}
@ -177,15 +177,15 @@ TEST(GaussianJunctionTreeB, slamlike) {
++ i;
// Compare solutions
OrderingOrdered ordering = *fullgraph.orderingCOLAMD(init);
GaussianFactorGraphOrdered linearized = *fullgraph.linearize(init, ordering);
Ordering ordering = *fullgraph.orderingCOLAMD(init);
GaussianFactorGraph linearized = *fullgraph.linearize(init, ordering);
GaussianJunctionTreeOrdered gjt(linearized);
VectorValuesOrdered deltaactual = gjt.optimize(&EliminateQROrdered);
GaussianJunctionTree gjt(linearized);
VectorValues deltaactual = gjt.optimize(&EliminateQR);
Values actual = init.retract(deltaactual, ordering);
GaussianBayesNetOrdered gbn = *GaussianSequentialSolver(linearized).eliminate();
VectorValuesOrdered delta = optimize(gbn);
GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate();
VectorValues delta = optimize(gbn);
Values expected = init.retract(delta, ordering);
EXPECT(assert_equal(expected, actual));
@ -194,7 +194,7 @@ TEST(GaussianJunctionTreeB, slamlike) {
/* ************************************************************************* */
TEST(GaussianJunctionTreeB, simpleMarginal) {
typedef BayesTreeOrdered<GaussianConditionalOrdered> GaussianBayesTree;
typedef BayesTree<GaussianConditional> GaussianBayesTree;
// Create a simple graph
NonlinearFactorGraph fg;
@ -205,10 +205,10 @@ TEST(GaussianJunctionTreeB, simpleMarginal) {
init.insert(X(0), Pose2());
init.insert(X(1), Pose2(1.0, 0.0, 0.0));
OrderingOrdered ordering;
Ordering ordering;
ordering += X(1), X(0);
GaussianFactorGraphOrdered gfg = *fg.linearize(init, ordering);
GaussianFactorGraph gfg = *fg.linearize(init, ordering);
// Compute marginals with both sequential and multifrontal
Matrix expected = GaussianSequentialSolver(gfg).marginalCovariance(1);
@ -216,15 +216,15 @@ TEST(GaussianJunctionTreeB, simpleMarginal) {
Matrix actual1 = GaussianMultifrontalSolver(gfg).marginalCovariance(1);
// Compute marginal directly from marginal factor
GaussianFactorOrdered::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1);
JacobianFactorOrdered::shared_ptr marginalJacobian = boost::dynamic_pointer_cast<JacobianFactorOrdered>(marginalFactor);
GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1);
JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast<JacobianFactor>(marginalFactor);
Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin()));
// Compute marginal directly from BayesTree
GaussianBayesTree gbt;
gbt.insert(GaussianJunctionTreeOrdered(gfg).eliminate(EliminateCholeskyOrdered));
marginalFactor = gbt.marginalFactor(1, EliminateCholeskyOrdered);
marginalJacobian = boost::dynamic_pointer_cast<JacobianFactorOrdered>(marginalFactor);
gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateCholesky));
marginalFactor = gbt.marginalFactor(1, EliminateCholesky);
marginalJacobian = boost::dynamic_pointer_cast<JacobianFactor>(marginalFactor);
Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin()));
EXPECT(assert_equal(expected, actual1));

2
tests/testGraph.cpp
View File

@ -36,7 +36,7 @@ using namespace gtsam;
// x1 -> x2
// -> x3 -> x4
// -> x5
TEST ( OrderingOrdered, predecessorMap2Keys ) {
TEST ( Ordering, predecessorMap2Keys ) {
PredecessorMap<Key> p_map;
p_map.insert(1,1);
p_map.insert(2,1);

40
tests/testIterative.cpp
View File

@ -41,16 +41,16 @@ static ConjugateGradientParameters parameters;
TEST( Iterative, steepestDescent )
{
// Create factor graph
OrderingOrdered ordering;
Ordering ordering;
ordering += L(1), X(1), X(2);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// eliminate and solve
VectorValuesOrdered expected = *GaussianSequentialSolver(fg).optimize();
VectorValues expected = *GaussianSequentialSolver(fg).optimize();
// Do gradient descent
VectorValuesOrdered zero = VectorValuesOrdered::Zero(expected); // TODO, how do we do this normally?
VectorValuesOrdered actual = steepestDescent(fg, zero, parameters);
VectorValues zero = VectorValues::Zero(expected); // TODO, how do we do this normally?
VectorValues actual = steepestDescent(fg, zero, parameters);
CHECK(assert_equal(expected,actual,1e-2));
}
@ -58,12 +58,12 @@ TEST( Iterative, steepestDescent )
TEST( Iterative, conjugateGradientDescent )
{
// Create factor graph
OrderingOrdered ordering;
Ordering ordering;
ordering += L(1), X(1), X(2);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
// eliminate and solve
VectorValuesOrdered expected = *GaussianSequentialSolver(fg).optimize();
VectorValues expected = *GaussianSequentialSolver(fg).optimize();
// get matrices
Matrix A;
@ -82,8 +82,8 @@ TEST( Iterative, conjugateGradientDescent )
CHECK(assert_equal(expectedX,actualX2,1e-9));
// Do conjugate gradient descent on factor graph
VectorValuesOrdered zero = VectorValuesOrdered::Zero(expected);
VectorValuesOrdered actual = conjugateGradientDescent(fg, zero, parameters);
VectorValues zero = VectorValues::Zero(expected);
VectorValues actual = conjugateGradientDescent(fg, zero, parameters);
CHECK(assert_equal(expected,actual,1e-2));
}
@ -99,19 +99,19 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint )
graph.add(NonlinearEquality<Pose2>(X(1), pose1));
graph.add(BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)));
OrderingOrdered ordering;
Ordering ordering;
ordering += X(1), X(2);
boost::shared_ptr<GaussianFactorGraphOrdered> fg = graph.linearize(config,ordering);
boost::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config,ordering);
VectorValuesOrdered zeros = VectorValuesOrdered::Zero(2, 3);
VectorValues zeros = VectorValues::Zero(2, 3);
ConjugateGradientParameters parameters;
parameters.setEpsilon_abs(1e-3);
parameters.setEpsilon_rel(1e-5);
parameters.setMaxIterations(100);
VectorValuesOrdered actual = conjugateGradientDescent(*fg, zeros, parameters);
VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
VectorValuesOrdered expected;
VectorValues expected;
expected.insert(0, zero(3));
expected.insert(1, Vector_(3,-0.5,0.,0.));
CHECK(assert_equal(expected, actual));
@ -128,19 +128,19 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
graph.add(PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)));
graph.add(BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)));
OrderingOrdered ordering;
Ordering ordering;
ordering += X(1), X(2);
boost::shared_ptr<GaussianFactorGraphOrdered> fg = graph.linearize(config,ordering);
boost::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config,ordering);
VectorValuesOrdered zeros = VectorValuesOrdered::Zero(2, 3);
VectorValues zeros = VectorValues::Zero(2, 3);
ConjugateGradientParameters parameters;
parameters.setEpsilon_abs(1e-3);
parameters.setEpsilon_rel(1e-5);
parameters.setMaxIterations(100);
VectorValuesOrdered actual = conjugateGradientDescent(*fg, zeros, parameters);
VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
VectorValuesOrdered expected;
VectorValues expected;
expected.insert(0, zero(3));
expected.insert(1, Vector_(3,-0.5,0.,0.));
CHECK(assert_equal(expected, actual));

40
tests/testNonlinearEquality.cpp
View File

@ -55,9 +55,9 @@ TEST ( NonlinearEquality, linearization ) {
// check linearize
SharedDiagonal constraintModel = noiseModel::Constrained::All(3);
JacobianFactorOrdered expLF(0, eye(3), zero(3), constraintModel);
GaussianFactorOrdered::shared_ptr actualLF = nle->linearize(linearize, *linearize.orderingArbitrary());
EXPECT(assert_equal(*actualLF, (const GaussianFactorOrdered&)expLF));
JacobianFactor expLF(0, eye(3), zero(3), constraintModel);
GaussianFactor::shared_ptr actualLF = nle->linearize(linearize, *linearize.orderingArbitrary());
EXPECT(assert_equal(*actualLF, (const GaussianFactor&)expLF));
}
/* ********************************************************************** */
@ -71,7 +71,7 @@ TEST ( NonlinearEquality, linearization_pose ) {
// create a nonlinear equality constraint
shared_poseNLE nle(new PoseNLE(key, value));
GaussianFactorOrdered::shared_ptr actualLF = nle->linearize(config, *config.orderingArbitrary());
GaussianFactor::shared_ptr actualLF = nle->linearize(config, *config.orderingArbitrary());
EXPECT(true);
}
@ -176,11 +176,11 @@ TEST ( NonlinearEquality, allow_error_pose ) {
DOUBLES_EQUAL(500.0, actError, 1e-9);
// check linearization
GaussianFactorOrdered::shared_ptr actLinFactor = nle.linearize(config, *config.orderingArbitrary());
GaussianFactor::shared_ptr actLinFactor = nle.linearize(config, *config.orderingArbitrary());
Matrix A1 = eye(3,3);
Vector b = expVec;
SharedDiagonal model = noiseModel::Constrained::All(3);
GaussianFactorOrdered::shared_ptr expLinFactor(new JacobianFactorOrdered(0, A1, b, model));
GaussianFactor::shared_ptr expLinFactor(new JacobianFactor(0, A1, b, model));
EXPECT(assert_equal(*expLinFactor, *actLinFactor, 1e-5));
}
@ -201,7 +201,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
init.insert(key1, initPose);
// optimize
OrderingOrdered ordering;
Ordering ordering;
ordering.push_back(key1);
Values result = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
@ -235,7 +235,7 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
graph.add(prior);
// optimize
OrderingOrdered ordering;
Ordering ordering;
ordering.push_back(key1);
Values actual = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
@ -277,21 +277,21 @@ TEST( testNonlinearEqualityConstraint, unary_linearization ) {
Point2 pt(1.0, 2.0);
Symbol key1('x',1);
double mu = 1000.0;
OrderingOrdered ordering;
Ordering ordering;
ordering += key;
eq2D::UnaryEqualityConstraint constraint(pt, key, mu);
Values config1;
config1.insert(key, pt);
GaussianFactorOrdered::shared_ptr actual1 = constraint.linearize(config1, ordering);
GaussianFactorOrdered::shared_ptr expected1(new JacobianFactorOrdered(ordering[key], eye(2,2), zero(2), hard_model));
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering);
GaussianFactor::shared_ptr expected1(new JacobianFactor(ordering[key], eye(2,2), zero(2), hard_model));
EXPECT(assert_equal(*expected1, *actual1, tol));
Values config2;
Point2 ptBad(2.0, 2.0);
config2.insert(key, ptBad);
GaussianFactorOrdered::shared_ptr actual2 = constraint.linearize(config2, ordering);
GaussianFactorOrdered::shared_ptr expected2(new JacobianFactorOrdered(ordering[key], eye(2,2), Vector_(2,-1.0,0.0), hard_model));
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering);
GaussianFactor::shared_ptr expected2(new JacobianFactor(ordering[key], eye(2,2), Vector_(2,-1.0,0.0), hard_model));
EXPECT(assert_equal(*expected2, *actual2, tol));
}
@ -359,16 +359,16 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0);
Symbol key1('x',1), key2('x',2);
double mu = 1000.0;
OrderingOrdered ordering;
Ordering ordering;
ordering += key1, key2;
eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu);
Values config1;
config1.insert(key1, x1);
config1.insert(key2, x2);
GaussianFactorOrdered::shared_ptr actual1 = constraint.linearize(config1, ordering);
GaussianFactorOrdered::shared_ptr expected1(
new JacobianFactorOrdered(ordering[key1], -eye(2,2), ordering[key2],
GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering);
GaussianFactor::shared_ptr expected1(
new JacobianFactor(ordering[key1], -eye(2,2), ordering[key2],
eye(2,2), zero(2), hard_model));
EXPECT(assert_equal(*expected1, *actual1, tol));
@ -377,9 +377,9 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) {
Point2 x2bad(2.0, 2.0);
config2.insert(key1, x1bad);
config2.insert(key2, x2bad);
GaussianFactorOrdered::shared_ptr actual2 = constraint.linearize(config2, ordering);
GaussianFactorOrdered::shared_ptr expected2(
new JacobianFactorOrdered(ordering[key1], -eye(2,2), ordering[key2],
GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering);
GaussianFactor::shared_ptr expected2(
new JacobianFactor(ordering[key1], -eye(2,2), ordering[key2],
eye(2,2), Vector_(2, 1.0, 1.0), hard_model));
EXPECT(assert_equal(*expected2, *actual2, tol));
}

52
tests/testNonlinearFactor.cpp
View File

@ -112,10 +112,10 @@ TEST( NonlinearFactor, linearize_f1 )
Graph::sharedFactor nlf = nfg[0];
// We linearize at noisy config from SmallExample
GaussianFactorOrdered::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
GaussianFactorGraphOrdered lfg = createGaussianFactorGraph(*c.orderingArbitrary());
GaussianFactorOrdered::shared_ptr expected = lfg[0];
GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary());
GaussianFactor::shared_ptr expected = lfg[0];
CHECK(assert_equal(*expected,*actual));
@ -134,10 +134,10 @@ TEST( NonlinearFactor, linearize_f2 )
Graph::sharedFactor nlf = nfg[1];
// We linearize at noisy config from SmallExample
GaussianFactorOrdered::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
GaussianFactorGraphOrdered lfg = createGaussianFactorGraph(*c.orderingArbitrary());
GaussianFactorOrdered::shared_ptr expected = lfg[1];
GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary());
GaussianFactor::shared_ptr expected = lfg[1];
CHECK(assert_equal(*expected,*actual));
}
@ -151,10 +151,10 @@ TEST( NonlinearFactor, linearize_f3 )
// We linearize at noisy config from SmallExample
Values c = createNoisyValues();
GaussianFactorOrdered::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
GaussianFactorGraphOrdered lfg = createGaussianFactorGraph(*c.orderingArbitrary());
GaussianFactorOrdered::shared_ptr expected = lfg[2];
GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary());
GaussianFactor::shared_ptr expected = lfg[2];
CHECK(assert_equal(*expected,*actual));
}
@ -168,10 +168,10 @@ TEST( NonlinearFactor, linearize_f4 )
// We linearize at noisy config from SmallExample
Values c = createNoisyValues();
GaussianFactorOrdered::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary());
GaussianFactorGraphOrdered lfg = createGaussianFactorGraph(*c.orderingArbitrary());
GaussianFactorOrdered::shared_ptr expected = lfg[3];
GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary());
GaussianFactor::shared_ptr expected = lfg[3];
CHECK(assert_equal(*expected,*actual));
}
@ -205,13 +205,13 @@ TEST( NonlinearFactor, linearize_constraint1 )
Values config;
config.insert(X(1), Point2(1.0, 2.0));
GaussianFactorOrdered::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary());
GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary());
// create expected
OrderingOrdered ord(*config.orderingArbitrary());
Ordering ord(*config.orderingArbitrary());
Vector b = Vector_(2, 0., -3.);
JacobianFactorOrdered expected(ord[X(1)], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint);
CHECK(assert_equal((const GaussianFactorOrdered&)expected, *actual));
JacobianFactor expected(ord[X(1)], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint);
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
}
/* ************************************************************************* */
@ -226,14 +226,14 @@ TEST( NonlinearFactor, linearize_constraint2 )
Values config;
config.insert(X(1), Point2(1.0, 2.0));
config.insert(L(1), Point2(5.0, 4.0));
GaussianFactorOrdered::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary());
GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary());
// create expected
OrderingOrdered ord(*config.orderingArbitrary());
Ordering ord(*config.orderingArbitrary());
Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0);
Vector b = Vector_(2, -15., -3.);
JacobianFactorOrdered expected(ord[X(1)], -1*A, ord[L(1)], A, b, constraint);
CHECK(assert_equal((const GaussianFactorOrdered&)expected, *actual));
JacobianFactor expected(ord[X(1)], -1*A, ord[L(1)], A, b, constraint);
CHECK(assert_equal((const GaussianFactor&)expected, *actual));
}
/* ************************************************************************* */
@ -272,8 +272,8 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
tv.insert(X(4), LieVector(1, 4.0));
EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
OrderingOrdered ordering; ordering += X(1), X(2), X(3), X(4);
JacobianFactorOrdered jf(*boost::dynamic_pointer_cast<JacobianFactorOrdered>(tf.linearize(tv, ordering)));
Ordering ordering; ordering += X(1), X(2), X(3), X(4);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
LONGS_EQUAL(jf.keys()[2], 2);
@ -320,8 +320,8 @@ TEST(NonlinearFactor, NoiseModelFactor5) {
tv.insert(X(5), LieVector(1, 5.0));
EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
OrderingOrdered ordering; ordering += X(1), X(2), X(3), X(4), X(5);
JacobianFactorOrdered jf(*boost::dynamic_pointer_cast<JacobianFactorOrdered>(tf.linearize(tv, ordering)));
Ordering ordering; ordering += X(1), X(2), X(3), X(4), X(5);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
LONGS_EQUAL(jf.keys()[2], 2);
@ -374,8 +374,8 @@ TEST(NonlinearFactor, NoiseModelFactor6) {
tv.insert(X(6), LieVector(1, 6.0));
EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
OrderingOrdered ordering; ordering += X(1), X(2), X(3), X(4), X(5), X(6);
JacobianFactorOrdered jf(*boost::dynamic_pointer_cast<JacobianFactorOrdered>(tf.linearize(tv, ordering)));
Ordering ordering; ordering += X(1), X(2), X(3), X(4), X(5), X(6);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv, ordering)));
LONGS_EQUAL(jf.keys()[0], 0);
LONGS_EQUAL(jf.keys()[1], 1);
LONGS_EQUAL(jf.keys()[2], 2);

18
tests/testNonlinearFactorGraph.cpp
View File

@ -76,20 +76,20 @@ TEST( Graph, keys )
/* ************************************************************************* */
TEST( Graph, GET_ORDERING)
{
// OrderingOrdered expected; expected += "x1","l1","x2"; // For starting with x1,x2,l1
OrderingOrdered expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2
// Ordering expected; expected += "x1","l1","x2"; // For starting with x1,x2,l1
Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2
Graph nlfg = createNonlinearFactorGraph();
SymbolicFactorGraphOrdered::shared_ptr symbolic;
OrderingOrdered::shared_ptr ordering;
SymbolicFactorGraph::shared_ptr symbolic;
Ordering::shared_ptr ordering;
boost::tie(symbolic, ordering) = nlfg.symbolic(createNoisyValues());
OrderingOrdered actual = *nlfg.orderingCOLAMD(createNoisyValues());
Ordering actual = *nlfg.orderingCOLAMD(createNoisyValues());
EXPECT(assert_equal(expected,actual));
// Constrained ordering - put x2 at the end
std::map<Key, int> constraints;
constraints[X(2)] = 1;
OrderingOrdered actualConstrained = *nlfg.orderingCOLAMDConstrained(createNoisyValues(), constraints);
OrderingOrdered expectedConstrained; expectedConstrained += L(1), X(1), X(2);
Ordering actualConstrained = *nlfg.orderingCOLAMDConstrained(createNoisyValues(), constraints);
Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2);
EXPECT(assert_equal(expectedConstrained, actualConstrained));
}
@ -110,8 +110,8 @@ TEST( Graph, linearize )
{
Graph fg = createNonlinearFactorGraph();
Values initial = createNoisyValues();
boost::shared_ptr<FactorGraphOrdered<GaussianFactorOrdered> > linearized = fg.linearize(initial, *initial.orderingArbitrary());
FactorGraphOrdered<GaussianFactorOrdered> expected = createGaussianFactorGraph(*initial.orderingArbitrary());
boost::shared_ptr<FactorGraph<GaussianFactor> > linearized = fg.linearize(initial, *initial.orderingArbitrary());
FactorGraph<GaussianFactor> expected = createGaussianFactorGraph(*initial.orderingArbitrary());
CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations
}

4
tests/testNonlinearISAM.cpp
View File

@ -50,7 +50,7 @@ TEST(testNonlinearISAM, markov_chain ) {
// perform a check on changing orderings
if (i == 5) {
OrderingOrdered ordering = isam.getOrdering();
Ordering ordering = isam.getOrdering();
// swap last two elements
Key last = ordering.pop_back().first;
@ -59,7 +59,7 @@ TEST(testNonlinearISAM, markov_chain ) {
ordering.push_back(secondLast);
isam.setOrdering(ordering);
OrderingOrdered expected; expected += (0), (1), (2), (4), (3);
Ordering expected; expected += (0), (1), (2), (4), (3);
EXPECT(assert_equal(expected, isam.getOrdering()));
}

6
tests/testNonlinearOptimizer.cpp
View File

@ -89,7 +89,7 @@ TEST( NonlinearOptimizer, optimize )
DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);
// optimize parameters
OrderingOrdered ord;
Ordering ord;
ord.push_back(X(1));
// Gauss-Newton
@ -182,7 +182,7 @@ TEST( NonlinearOptimizer, Factorization )
graph.add(PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)));
graph.add(BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)));
OrderingOrdered ordering;
Ordering ordering;
ordering.push_back(X(1));
ordering.push_back(X(2));
@ -216,7 +216,7 @@ TEST(NonlinearOptimizer, NullFactor) {
DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);
// optimize parameters
OrderingOrdered ord;
Ordering ord;
ord.push_back(X(1));
// Gauss-Newton

18
tests/testSerializationSLAM.cpp
View File

@ -164,8 +164,8 @@ BOOST_CLASS_EXPORT(gtsam::StereoCamera);
/* Create GUIDs for factors */
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactorOrdered, "gtsam::JacobianFactorOrdered");
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactorOrdered , "gtsam::HessianFactorOrdered");
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
BOOST_CLASS_EXPORT_GUID(PriorFactorLieVector, "gtsam::PriorFactorLieVector");
BOOST_CLASS_EXPORT_GUID(PriorFactorLieMatrix, "gtsam::PriorFactorLieMatrix");
@ -234,8 +234,8 @@ BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D");
TEST (testSerializationSLAM, smallExample_linear) {
using namespace example;
OrderingOrdered ordering; ordering += X(1),X(2),L(1);
GaussianFactorGraphOrdered fg = createGaussianFactorGraph(ordering);
Ordering ordering; ordering += X(1),X(2),L(1);
GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
EXPECT(equalsObj(ordering));
EXPECT(equalsXML(ordering));
EXPECT(equalsBinary(ordering));
@ -244,7 +244,7 @@ TEST (testSerializationSLAM, smallExample_linear) {
EXPECT(equalsXML(fg));
EXPECT(equalsBinary(fg));
GaussianBayesNetOrdered cbn = createSmallGaussianBayesNet();
GaussianBayesNet cbn = createSmallGaussianBayesNet();
EXPECT(equalsObj(cbn));
EXPECT(equalsXML(cbn));
EXPECT(equalsBinary(cbn));
@ -253,11 +253,11 @@ TEST (testSerializationSLAM, smallExample_linear) {
/* ************************************************************************* */
TEST (testSerializationSLAM, gaussianISAM) {
using namespace example;
OrderingOrdered ordering;
GaussianFactorGraphOrdered smoother;
Ordering ordering;
GaussianFactorGraph smoother;
boost::tie(smoother, ordering) = createSmoother(7);
BayesTreeOrdered<GaussianConditionalOrdered> bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
GaussianISAMOrdered isam(bayesTree);
BayesTree<GaussianConditional> bayesTree = *GaussianMultifrontalSolver(smoother).eliminate();
GaussianISAM isam(bayesTree);
EXPECT(equalsObj(isam));
EXPECT(equalsXML(isam));

2
tests/testSimulated2DOriented.cpp
View File

@ -64,7 +64,7 @@ TEST( simulated2DOriented, constructor )
simulated2DOriented::Values config;
config.insert(X(1), Pose2(1., 0., 0.2));
config.insert(X(2), Pose2(2., 0., 0.1));
boost::shared_ptr<GaussianFactorOrdered> lf = factor.linearize(config, *config.orderingArbitrary());
boost::shared_ptr<GaussianFactor> lf = factor.linearize(config, *config.orderingArbitrary());
}
/* ************************************************************************* */

84
tests/testSubgraphPreconditioner.cpp
View File

@ -44,7 +44,7 @@ Symbol key(int x, int y) {
/* ************************************************************************* */
TEST( SubgraphPreconditioner, planarOrdering ) {
// Check canonical ordering
OrderingOrdered expected, ordering = planarOrdering(3);
Ordering expected, ordering = planarOrdering(3);
expected +=
key(3, 3), key(2, 3), key(1, 3),
key(3, 2), key(2, 2), key(1, 2),
@ -54,9 +54,9 @@ TEST( SubgraphPreconditioner, planarOrdering ) {
/* ************************************************************************* */
/** unnormalized error */
static double error(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x) {
static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
double total_error = 0.;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, fg)
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg)
total_error += factor->error(x);
return total_error;
}
@ -65,16 +65,16 @@ static double error(const GaussianFactorGraphOrdered& fg, const VectorValuesOrde
TEST( SubgraphPreconditioner, planarGraph )
{
// Check planar graph construction
GaussianFactorGraphOrdered A;
VectorValuesOrdered xtrue;
GaussianFactorGraph A;
VectorValues xtrue;
boost::tie(A, xtrue) = planarGraph(3);
LONGS_EQUAL(13,A.size());
LONGS_EQUAL(9,xtrue.size());
DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue
// Check that xtrue is optimal
GaussianBayesNetOrdered::shared_ptr R1 = GaussianSequentialSolver(A).eliminate();
VectorValuesOrdered actual = optimize(*R1);
GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate();
VectorValues actual = optimize(*R1);
CHECK(assert_equal(xtrue,actual));
}
@ -82,19 +82,19 @@ TEST( SubgraphPreconditioner, planarGraph )
TEST( SubgraphPreconditioner, splitOffPlanarTree )
{
// Build a planar graph
GaussianFactorGraphOrdered A;
VectorValuesOrdered xtrue;
GaussianFactorGraph A;
VectorValues xtrue;
boost::tie(A, xtrue) = planarGraph(3);
// Get the spanning tree and constraints, and check their sizes
GaussianFactorGraphOrdered T, C;
GaussianFactorGraph T, C;
boost::tie(T, C) = splitOffPlanarTree(3, A);
LONGS_EQUAL(9,T.size());
LONGS_EQUAL(4,C.size());
// Check that the tree can be solved to give the ground xtrue
GaussianBayesNetOrdered::shared_ptr R1 = GaussianSequentialSolver(T).eliminate();
VectorValuesOrdered xbar = optimize(*R1);
GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate();
VectorValues xbar = optimize(*R1);
CHECK(assert_equal(xtrue,xbar));
}
@ -103,37 +103,37 @@ TEST( SubgraphPreconditioner, splitOffPlanarTree )
TEST( SubgraphPreconditioner, system )
{
// Build a planar graph
GaussianFactorGraphOrdered Ab;
VectorValuesOrdered xtrue;
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
GaussianFactorGraphOrdered 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 GaussianFactorGraphOrdered(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraphOrdered(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
VectorValuesOrdered xbar = optimize(*Rc1); // xbar = inv(R1)*c1
VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
// Create Subgraph-preconditioned system
VectorValuesOrdered::shared_ptr xbarShared(new VectorValuesOrdered(xbar)); // TODO: horrible
VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
// Create zero config
VectorValuesOrdered zeros = VectorValuesOrdered::Zero(xbar);
VectorValues zeros = VectorValues::Zero(xbar);
// Set up y0 as all zeros
VectorValuesOrdered y0 = zeros;
VectorValues y0 = zeros;
// y1 = perturbed y0
VectorValuesOrdered y1 = zeros;
VectorValues y1 = zeros;
y1[1] = Vector_(2, 1.0, -1.0);
// Check corresponding x values
VectorValuesOrdered expected_x1 = xtrue, x1 = system.x(y1);
VectorValues expected_x1 = xtrue, x1 = system.x(y1);
expected_x1[1] = Vector_(2, 2.01, 2.99);
expected_x1[0] = Vector_(2, 3.01, 2.99);
CHECK(assert_equal(xtrue, system.x(y0)));
@ -146,8 +146,8 @@ TEST( SubgraphPreconditioner, system )
DOUBLES_EQUAL(3,error(system,y1),1e-9);
// Test gradient in x
VectorValuesOrdered expected_gx0 = zeros;
VectorValuesOrdered expected_gx1 = zeros;
VectorValues expected_gx0 = zeros;
VectorValues expected_gx1 = zeros;
CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue)));
expected_gx1[2] = Vector_(2, -100., 100.);
expected_gx1[4] = Vector_(2, -100., 100.);
@ -157,8 +157,8 @@ TEST( SubgraphPreconditioner, system )
CHECK(assert_equal(expected_gx1,gradient(Ab,x1)));
// Test gradient in y
VectorValuesOrdered expected_gy0 = zeros;
VectorValuesOrdered expected_gy1 = zeros;
VectorValues expected_gy0 = zeros;
VectorValues expected_gy1 = zeros;
expected_gy1[2] = Vector_(2, 2., -2.);
expected_gy1[4] = Vector_(2, -2., 2.);
expected_gy1[1] = Vector_(2, 3., -3.);
@ -169,7 +169,7 @@ TEST( SubgraphPreconditioner, system )
// Check it numerically for good measure
// TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1)
// Vector numerical_g1 = numericalGradient<VectorValuesOrdered> (error, y1, 0.001);
// Vector numerical_g1 = numericalGradient<VectorValues> (error, y1, 0.001);
// Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2.,
// 3., -3., 0., 0., -1., 1., 1., -1.);
// CHECK(assert_equal(expected_g1,numerical_g1));
@ -179,41 +179,41 @@ TEST( SubgraphPreconditioner, system )
TEST( SubgraphPreconditioner, conjugateGradients )
{
// Build a planar graph
GaussianFactorGraphOrdered Ab;
VectorValuesOrdered xtrue;
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
GaussianFactorGraphOrdered 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 GaussianFactorGraphOrdered(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraphOrdered(Ab2_));
SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
// Eliminate the spanning tree to build a prior
OrderingOrdered ordering = planarOrdering(N);
Ordering ordering = planarOrdering(N);
SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
VectorValuesOrdered xbar = optimize(*Rc1); // xbar = inv(R1)*c1
VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
// Create Subgraph-preconditioned system
VectorValuesOrdered::shared_ptr xbarShared(new VectorValuesOrdered(xbar)); // TODO: horrible
VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
// Create zero config y0 and perturbed config y1
VectorValuesOrdered y0 = VectorValuesOrdered::Zero(xbar);
VectorValues y0 = VectorValues::Zero(xbar);
VectorValuesOrdered y1 = y0;
VectorValues y1 = y0;
y1[1] = Vector_(2, 1.0, -1.0);
VectorValuesOrdered x1 = system.x(y1);
VectorValues x1 = system.x(y1);
// Solve for the remaining constraints using PCG
ConjugateGradientParameters parameters;
VectorValuesOrdered actual = conjugateGradients<SubgraphPreconditioner,
VectorValuesOrdered, Errors>(system, y1, parameters);
VectorValues actual = conjugateGradients<SubgraphPreconditioner,
VectorValues, Errors>(system, y1, parameters);
CHECK(assert_equal(y0,actual));
// Compare with non preconditioned version:
VectorValuesOrdered actual2 = conjugateGradientDescent(Ab, x1, parameters);
VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters);
CHECK(assert_equal(xtrue,actual2,1e-4));
}

32
tests/testSubgraphSolver.cpp
View File

@ -22,7 +22,7 @@
#include <gtsam/linear/iterative.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/inference/EliminationTreeOrdered-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
@ -38,9 +38,9 @@ using namespace example;
/* ************************************************************************* */
/** unnormalized error */
static double error(const GaussianFactorGraphOrdered& fg, const VectorValuesOrdered& x) {
static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
double total_error = 0.;
BOOST_FOREACH(const GaussianFactorOrdered::shared_ptr& factor, fg)
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg)
total_error += factor->error(x);
return total_error;
}
@ -50,8 +50,8 @@ static double error(const GaussianFactorGraphOrdered& fg, const VectorValuesOrde
TEST( SubgraphSolver, constructor1 )
{
// Build a planar graph
GaussianFactorGraphOrdered Ab;
VectorValuesOrdered xtrue;
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
@ -59,7 +59,7 @@ TEST( SubgraphSolver, constructor1 )
// and it will split the graph into A1 and A2, where A1 is a spanning tree
SubgraphSolverParameters parameters;
SubgraphSolver solver(Ab, parameters);
VectorValuesOrdered optimized = solver.optimize(); // does PCG optimization
VectorValues optimized = solver.optimize(); // does PCG optimization
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}
@ -67,20 +67,20 @@ TEST( SubgraphSolver, constructor1 )
TEST( SubgraphSolver, constructor2 )
{
// Build a planar graph
GaussianFactorGraphOrdered Ab;
VectorValuesOrdered xtrue;
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
GaussianFactorGraphOrdered 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,
// so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2)
SubgraphSolverParameters parameters;
SubgraphSolver solver(Ab1_, Ab2_, parameters);
VectorValuesOrdered optimized = solver.optimize();
VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}
@ -88,24 +88,24 @@ TEST( SubgraphSolver, constructor2 )
TEST( SubgraphSolver, constructor3 )
{
// Build a planar graph
GaussianFactorGraphOrdered Ab;
VectorValuesOrdered xtrue;
GaussianFactorGraph Ab;
VectorValues xtrue;
size_t N = 3;
boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
// Get the spanning tree and corresponding ordering
GaussianFactorGraphOrdered 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
GaussianBayesNetOrdered::shared_ptr Rc1 = //
EliminationTreeOrdered<GaussianFactorOrdered>::Create(Ab1_)->eliminate(&EliminateQROrdered);
GaussianBayesNet::shared_ptr Rc1 = //
EliminationTree<GaussianFactor>::Create(Ab1_)->eliminate(&EliminateQR);
// The third constructor allows the caller to pass an already solved preconditioner Rc1_
// as a Bayes net, in addition to the "loop closing constraints" Ab2, as before
SubgraphSolverParameters parameters;
SubgraphSolver solver(Rc1, Ab2_, parameters);
VectorValuesOrdered optimized = solver.optimize();
VectorValues optimized = solver.optimize();
DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
}

32
tests/testSummarization.cpp
View File

@ -74,15 +74,15 @@ TEST( testSummarization, example_from_ddf1 ) {
{
// Summarize to a linear system
GaussianFactorGraphOrdered actLinGraph; OrderingOrdered actOrdering;
GaussianFactorGraph actLinGraph; Ordering actOrdering;
SummarizationMode mode = PARTIAL_QR;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
OrderingOrdered expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
EXPECT(assert_equal(expSumOrdering, actOrdering));
// Does not split out subfactors where possible
GaussianFactorGraphOrdered expLinGraph;
GaussianFactorGraph expLinGraph;
expLinGraph.add(
expSumOrdering[lA3],
Matrix_(4,2,
@ -108,16 +108,16 @@ TEST( testSummarization, example_from_ddf1 ) {
{
// Summarize to a linear system using cholesky - compare to previous version
GaussianFactorGraphOrdered actLinGraph; OrderingOrdered actOrdering;
GaussianFactorGraph actLinGraph; Ordering actOrdering;
SummarizationMode mode = PARTIAL_CHOLESKY;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
OrderingOrdered expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
EXPECT(assert_equal(expSumOrdering, actOrdering));
// Does not split out subfactors where possible
GaussianFactorGraphOrdered expLinGraph;
expLinGraph.add(HessianFactorOrdered(JacobianFactorOrdered(
GaussianFactorGraph expLinGraph;
expLinGraph.add(HessianFactor(JacobianFactor(
expSumOrdering[lA3],
Matrix_(4,2,
0.595867, 0.605092,
@ -142,15 +142,15 @@ TEST( testSummarization, example_from_ddf1 ) {
{
// Summarize to a linear system with joint factor graph version
GaussianFactorGraphOrdered actLinGraph; OrderingOrdered actOrdering;
GaussianFactorGraph actLinGraph; Ordering actOrdering;
SummarizationMode mode = SEQUENTIAL_QR;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
OrderingOrdered expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
EXPECT(assert_equal(expSumOrdering, actOrdering));
// Does not split out subfactors where possible
GaussianFactorGraphOrdered expLinGraph;
GaussianFactorGraph expLinGraph;
expLinGraph.add(
expSumOrdering[lA3],
Matrix_(2,2,
@ -180,15 +180,15 @@ TEST( testSummarization, example_from_ddf1 ) {
{
// Summarize to a linear system with joint factor graph version
GaussianFactorGraphOrdered actLinGraph; OrderingOrdered actOrdering;
GaussianFactorGraph actLinGraph; Ordering actOrdering;
SummarizationMode mode = SEQUENTIAL_CHOLESKY;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
OrderingOrdered expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
Ordering expSumOrdering; expSumOrdering += xA0, xA1, xA2, lA3, lA5;
EXPECT(assert_equal(expSumOrdering, actOrdering));
// Does not split out subfactors where possible
GaussianFactorGraphOrdered expLinGraph;
GaussianFactorGraph expLinGraph;
expLinGraph.add(
expSumOrdering[lA3],
Matrix_(2,2,
@ -229,10 +229,10 @@ TEST( testSummarization, no_summarize_case ) {
values.insert(key, Pose2(0.0, 0.0, 0.1));
SummarizationMode mode = SEQUENTIAL_CHOLESKY;
GaussianFactorGraphOrdered actLinGraph; OrderingOrdered actOrdering;
GaussianFactorGraph actLinGraph; Ordering actOrdering;
boost::tie(actLinGraph, actOrdering) = summarize(graph, values, saved_keys, mode);
OrderingOrdered expOrdering; expOrdering += key;
GaussianFactorGraphOrdered expLinGraph = *graph.linearize(values, expOrdering);
Ordering expOrdering; expOrdering += key;
GaussianFactorGraph expLinGraph = *graph.linearize(values, expOrdering);
EXPECT(assert_equal(expOrdering, actOrdering));
EXPECT(assert_equal(expLinGraph, actLinGraph));
}

30
tests/timeGaussianFactorGraph.cpp
View File

@ -30,8 +30,8 @@ using namespace boost::assign;
/* ************************************************************************* */
// Create a Kalman smoother for t=1:T and optimize
double timeKalmanSmoother(int T) {
pair<GaussianFactorGraphOrdered,OrderingOrdered> smoother_ordering = createSmoother(T);
GaussianFactorGraphOrdered& smoother(smoother_ordering.first);
pair<GaussianFactorGraph,Ordering> smoother_ordering = createSmoother(T);
GaussianFactorGraph& smoother(smoother_ordering.first);
clock_t start = clock();
GaussianSequentialSolver(smoother).optimize();
clock_t end = clock ();
@ -43,8 +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<GaussianFactorGraphOrdered, VectorValuesOrdered> pg = planarGraph(N);
GaussianFactorGraphOrdered& fg(pg.get<0>());
boost::tuple<GaussianFactorGraph, VectorValues> pg = planarGraph(N);
GaussianFactorGraph& fg(pg.get<0>());
clock_t start = clock();
GaussianSequentialSolver(fg).optimize();
clock_t end = clock ();
@ -56,8 +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<GaussianFactorGraphOrdered, VectorValuesOrdered> pg = planarGraph(N);
GaussianFactorGraphOrdered& fg(pg.get<0>());
boost::tuple<GaussianFactorGraph, VectorValues> pg = planarGraph(N);
GaussianFactorGraph& fg(pg.get<0>());
clock_t start = clock();
GaussianSequentialSolver(fg).eliminate();
clock_t end = clock ();
@ -69,23 +69,23 @@ double timePlanarSmootherEliminate(int N, bool old = true) {
//// Create a planar factor graph and join factors until matrix formation
//// This variation uses the original join factors approach
//double timePlanarSmootherJoinAug(int N, size_t reps) {
// GaussianFactorGraphOrdered fgBase;
// VectorValuesOrdered config;
// GaussianFactorGraph fgBase;
// VectorValues config;
// boost::tie(fgBase,config) = planarGraph(N);
// OrderingOrdered ordering = fgBase.getOrdering();
// Ordering ordering = fgBase.getOrdering();
// Symbol key = ordering.front();
//
// clock_t start = clock();
//
// for (size_t i = 0; i<reps; ++i) {
// // setup
// GaussianFactorGraphOrdered fg(fgBase);
// GaussianFactorGraph fg(fgBase);
//
// // combine some factors
// GaussianFactor::shared_ptr joint_factor = removeAndCombineFactors(fg,key);
//
// // create an internal ordering to render Ab
// OrderingOrdered render;
// Ordering render;
// render += key;
// BOOST_FOREACH(const Symbol& k, joint_factor->keys())
// if (k != key) render += k;
@ -102,16 +102,16 @@ double timePlanarSmootherEliminate(int N, bool old = true) {
//// Create a planar factor graph and join factors until matrix formation
//// This variation uses the single-allocate version to create the matrix
//double timePlanarSmootherCombined(int N, size_t reps) {
// GaussianFactorGraphOrdered fgBase;
// VectorValuesOrdered config;
// GaussianFactorGraph fgBase;
// VectorValues config;
// boost::tie(fgBase,config) = planarGraph(N);
// OrderingOrdered ordering = fgBase.getOrdering();
// Ordering ordering = fgBase.getOrdering();
// Symbol key = ordering.front();
//
// clock_t start = clock();
//
// for (size_t i = 0; i<reps; ++i) {
// GaussianFactorGraphOrdered fg(fgBase);
// GaussianFactorGraph fg(fgBase);
// fg.eliminateOneMatrixJoin(key);
// }
//