Cleanup in linear solving:
- No longer need to fill solution vector with rhs - Removed rhs functions - Combined/removed redundant optimize functions for GaussianConditional, GaussianBayesNet, and GaussianBayesTree - Renamed some to optimizeInPlace and optimizeWildfire - Moved BayesTree optimize functions from GaussianISAM(2) to GaussianBayesTree.release/4.3a0
Richard Roberts
parent
0531983c74
commit
1c63d72785
|
|
@ -83,33 +83,14 @@ boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn)
|
|||
|
||||
/* ************************************************************************* */
|
||||
VectorValues optimize(const GaussianBayesNet& bn) {
|
||||
return *optimize_(bn);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<VectorValues> optimize_(const GaussianBayesNet& bn)
|
||||
{
|
||||
// get the RHS as a VectorValues to initialize system
|
||||
boost::shared_ptr<VectorValues> result(new VectorValues(rhs(bn)));
|
||||
|
||||
/** solve each node in turn in topological sort order (parents first)*/
|
||||
BOOST_REVERSE_FOREACH(GaussianConditional::shared_ptr cg, bn)
|
||||
cg->solveInPlace(*result); // solve and store solution in same step
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& y) {
|
||||
VectorValues x(y);
|
||||
backSubstituteInPlace(bn,x);
|
||||
VectorValues x = *allocateVectorValues(bn);
|
||||
optimizeInPlace(bn, x);
|
||||
return x;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
|
||||
void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y) {
|
||||
VectorValues& x = y;
|
||||
void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x) {
|
||||
/** solve each node in turn in topological sort order (parents first)*/
|
||||
BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, bn) {
|
||||
// i^th part of R*x=y, x=inv(R)*y
|
||||
|
|
@ -194,15 +175,6 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
|
|||
return make_pair(R,d);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues rhs(const GaussianBayesNet& bn) {
|
||||
boost::shared_ptr<VectorValues> result(allocateVectorValues(bn));
|
||||
BOOST_FOREACH(boost::shared_ptr<const GaussianConditional> cg,bn)
|
||||
cg->rhs(*result);
|
||||
|
||||
return *result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double determinant(const GaussianBayesNet& bayesNet) {
|
||||
double logDet = 0.0;
|
||||
|
|
|
|||
|
|
@ -55,26 +55,19 @@ namespace gtsam {
|
|||
boost::shared_ptr<VectorValues> allocateVectorValues(const GaussianBayesNet& bn);
|
||||
|
||||
/**
|
||||
* optimize, i.e. return x = inv(R)*d
|
||||
* Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by
|
||||
* back-substitution.
|
||||
*/
|
||||
VectorValues optimize(const GaussianBayesNet&);
|
||||
VectorValues optimize(const GaussianBayesNet& bn);
|
||||
|
||||
/**
|
||||
* shared pointer version
|
||||
*/
|
||||
boost::shared_ptr<VectorValues> optimize_(const GaussianBayesNet& bn);
|
||||
|
||||
/**
|
||||
* Backsubstitute
|
||||
* (R*x)./sigmas = y by solving x=inv(R)*(y.*sigmas)
|
||||
* @param y is the RHS of the system
|
||||
*/
|
||||
VectorValues backSubstitute(const GaussianBayesNet& bn, const VectorValues& y);
|
||||
|
||||
/**
|
||||
* Backsubstitute in place, y starts as RHS and is replaced with solution
|
||||
*/
|
||||
void backSubstituteInPlace(const GaussianBayesNet& bn, VectorValues& y);
|
||||
/**
|
||||
* Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, computed by
|
||||
* back-substitution, writes the solution \f$ x \f$ into a pre-allocated
|
||||
* VectorValues. You can use allocateVectorValues(const GaussianBayesNet&)
|
||||
* allocate it. See also optimize(const GaussianBayesNet&), which does not
|
||||
* require pre-allocation.
|
||||
*/
|
||||
void optimizeInPlace(const GaussianBayesNet& bn, VectorValues& x);
|
||||
|
||||
/**
|
||||
* Transpose Backsubstitute
|
||||
|
|
@ -91,12 +84,6 @@ namespace gtsam {
|
|||
*/
|
||||
std::pair<Matrix, Vector> matrix(const GaussianBayesNet&);
|
||||
|
||||
/**
|
||||
* Return RHS d as a VectorValues
|
||||
* Such that backSubstitute(bn,d) = optimize(bn)
|
||||
*/
|
||||
VectorValues rhs(const GaussianBayesNet&);
|
||||
|
||||
/**
|
||||
* Computes the determinant of a GassianBayesNet
|
||||
* A GaussianBayesNet is an upper triangular matrix and for an upper triangular matrix
|
||||
|
|
|
|||
|
|
@ -0,0 +1,55 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file GaussianBayesTree.cpp
|
||||
* @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree
|
||||
* @brief GaussianBayesTree
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
|
||||
#include <gtsam/linear/GaussianBayesTree.h> // Only to help Eclipse
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace internal {
|
||||
template<class CLIQUE>
|
||||
inline static void optimizeInPlace(const boost::shared_ptr<CLIQUE>& clique, VectorValues& result) {
|
||||
// parents are assumed to already be solved and available in result
|
||||
clique->conditional()->solveInPlace(result);
|
||||
|
||||
// starting from the root, call optimize on each conditional
|
||||
BOOST_FOREACH(const boost::shared_ptr<CLIQUE>& child, clique->children_)
|
||||
optimizeInPlace(child, result);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
VectorValues optimize(const BayesTree<GaussianConditional, CLIQUE>& bayesTree) {
|
||||
VectorValues result = *allocateVectorValues(bayesTree);
|
||||
internal::optimizeInPlace(bayesTree.root(), result);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
void optimizeInPlace(const BayesTree<GaussianConditional, CLIQUE>& bayesTree, VectorValues& result) {
|
||||
internal::optimizeInPlace(bayesTree.root(), result);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file GaussianBayesTree.h
|
||||
* @brief Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree
|
||||
* @brief GaussianBayesTree
|
||||
* @author Frank Dellaert
|
||||
* @author Richard Roberts
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/inference/BayesTree.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef BayesTree<GaussianConditional> GaussianBayesTree;
|
||||
|
||||
/// optimize the BayesTree, starting from the root
|
||||
template<class CLIQUE>
|
||||
VectorValues optimize(const BayesTree<GaussianConditional, CLIQUE>& bayesTree);
|
||||
|
||||
/// recursively optimize this conditional and all subtrees
|
||||
template<class CLIQUE>
|
||||
void optimizeInPlace(const BayesTree<GaussianConditional, CLIQUE>& clique, VectorValues& result);
|
||||
|
||||
}
|
||||
|
||||
#include <gtsam/linear/GaussianBayesTree-inl.h>
|
||||
|
||||
|
|
@ -31,19 +31,19 @@ namespace gtsam {
|
|||
// Helper function used only in this file - extracts vectors with variable indices
|
||||
// in the first and last iterators, and concatenates them in that order into the
|
||||
// output.
|
||||
template<typename ITERATOR>
|
||||
static Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR first, ITERATOR last) {
|
||||
template<class VALUES, typename ITERATOR>
|
||||
static Vector extractVectorValuesSlices(const VALUES& values, ITERATOR first, ITERATOR last) {
|
||||
// Find total dimensionality
|
||||
int dim = 0;
|
||||
for(ITERATOR j = first; j != last; ++j)
|
||||
dim += values.dim(*j);
|
||||
dim += values[*j].rows();
|
||||
|
||||
// Copy vectors
|
||||
Vector ret(dim);
|
||||
int varStart = 0;
|
||||
for(ITERATOR j = first; j != last; ++j) {
|
||||
ret.segment(varStart, values.dim(*j)) = values[*j];
|
||||
varStart += values.dim(*j);
|
||||
ret.segment(varStart, values[*j].rows()) = values[*j];
|
||||
varStart += values[*j].rows();
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -52,13 +52,13 @@ static Vector extractVectorValuesSlices(const VectorValues& values, ITERATOR fir
|
|||
// Helper function used only in this file - writes to the variables in values
|
||||
// with indices iterated over by first and last, interpreting vector as the
|
||||
// concatenated vectors to write.
|
||||
template<class VECTOR, typename ITERATOR>
|
||||
static void writeVectorValuesSlices(const VECTOR& vector, VectorValues& values, ITERATOR first, ITERATOR last) {
|
||||
template<class VECTOR, class VALUES, typename ITERATOR>
|
||||
static void writeVectorValuesSlices(const VECTOR& vector, VALUES& values, ITERATOR first, ITERATOR last) {
|
||||
// Copy vectors
|
||||
int varStart = 0;
|
||||
for(ITERATOR j = first; j != last; ++j) {
|
||||
values[*j] = vector.segment(varStart, values.dim(*j));
|
||||
varStart += values.dim(*j);
|
||||
values[*j] = vector.segment(varStart, values[*j].rows());
|
||||
varStart += values[*j].rows();
|
||||
}
|
||||
assert(varStart == vector.rows());
|
||||
}
|
||||
|
|
@ -221,78 +221,34 @@ JacobianFactor::shared_ptr GaussianConditional::toFactor() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianConditional::rhs(VectorValues& x) const {
|
||||
writeVectorValuesSlices(get_d(), x, beginFrontals(), endFrontals());
|
||||
}
|
||||
template<class VALUES>
|
||||
inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES& x) {
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianConditional::rhs(Permuted<VectorValues>& x) const {
|
||||
// Copy the rhs into x, accounting for the permutation
|
||||
Vector d = get_d();
|
||||
size_t rhsPosition = 0; // We walk through the rhs by variable
|
||||
for(const_iterator j = beginFrontals(); j != endFrontals(); ++j) {
|
||||
// Get the segment of the rhs for this variable
|
||||
x[*j] = d.segment(rhsPosition, this->dim(j));
|
||||
// Increment the position
|
||||
rhsPosition += this->dim(j);
|
||||
// Helper function to solve-in-place on a VectorValues or Permuted<VectorValues>,
|
||||
// called by GaussianConditional::solveInPlace(VectorValues&) and by
|
||||
// GaussianConditional::solveInPlace(Permuted<VectorValues>&).
|
||||
|
||||
static const bool debug = false;
|
||||
if(debug) conditional.print("Solving conditional in place");
|
||||
Vector xS = extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents());
|
||||
xS = conditional.get_d() - conditional.get_S() * xS;
|
||||
Vector soln = conditional.permutation().transpose() *
|
||||
conditional.get_R().triangularView<Eigen::Upper>().solve(xS);
|
||||
if(debug) {
|
||||
gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on ");
|
||||
gtsam::print(soln, "full back-substitution solution: ");
|
||||
}
|
||||
writeVectorValuesSlices(soln, x, conditional.beginFrontals(), conditional.endFrontals());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianConditional::solveInPlace(VectorValues& x) const {
|
||||
static const bool debug = false;
|
||||
if(debug) print("Solving conditional in place");
|
||||
Vector rhs = extractVectorValuesSlices(x, beginFrontals(), endFrontals());
|
||||
for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
|
||||
rhs += -get_S(parent) * x[*parent];
|
||||
}
|
||||
Vector soln = permutation_.transpose() * get_R().triangularView<Eigen::Upper>().solve(rhs);
|
||||
if(debug) {
|
||||
gtsam::print(Matrix(get_R()), "Calling backSubstituteUpper on ");
|
||||
gtsam::print(rhs, "rhs: ");
|
||||
gtsam::print(soln, "full back-substitution solution: ");
|
||||
}
|
||||
writeVectorValuesSlices(soln, x, beginFrontals(), endFrontals());
|
||||
doSolveInPlace(*this, x); // Call helper version above
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianConditional::solveInPlace(Permuted<VectorValues>& x) const {
|
||||
static const bool debug = false;
|
||||
if(debug) print("Solving conditional in place (permuted)");
|
||||
// Extract RHS from values - inlined from VectorValues
|
||||
size_t s = 0;
|
||||
for (const_iterator it=beginFrontals(); it!=endFrontals(); ++it)
|
||||
s += x[*it].size();
|
||||
Vector rhs(s); size_t start = 0;
|
||||
for (const_iterator it=beginFrontals(); it!=endFrontals(); ++it) {
|
||||
SubVector v = x[*it];
|
||||
const size_t d = v.size();
|
||||
rhs.segment(start, d) = v;
|
||||
start += d;
|
||||
}
|
||||
|
||||
// apply parents to rhs
|
||||
for (const_iterator parent = beginParents(); parent != endParents(); ++parent) {
|
||||
rhs += -get_S(parent) * x[*parent];
|
||||
}
|
||||
|
||||
// solve system - backsubstitution
|
||||
Vector soln = permutation_.transpose() * get_R().triangularView<Eigen::Upper>().solve(rhs);
|
||||
|
||||
// apply solution: inlined manually due to permutation
|
||||
size_t solnStart = 0;
|
||||
for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
|
||||
const size_t d = this->dim(frontal);
|
||||
x[*frontal] = soln.segment(solnStart, d);
|
||||
solnStart += d;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues GaussianConditional::solve(const VectorValues& x) const {
|
||||
VectorValues result = x;
|
||||
solveInPlace(result);
|
||||
return result;
|
||||
doSolveInPlace(*this, x); // Call helper version above
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -197,46 +197,39 @@ public:
|
|||
*/
|
||||
boost::shared_ptr<JacobianFactor> toFactor() const;
|
||||
|
||||
/**
|
||||
* Adds the RHS to a given VectorValues for use in solve() functions.
|
||||
* @param x is the values to be updated, assumed allocated
|
||||
*/
|
||||
void rhs(VectorValues& x) const;
|
||||
|
||||
/**
|
||||
* Adds the RHS to a given VectorValues for use in solve() functions.
|
||||
* @param x is the values to be updated, assumed allocated
|
||||
*/
|
||||
void rhs(Permuted<VectorValues>& x) const;
|
||||
|
||||
/**
|
||||
* solves a conditional Gaussian and stores the result in x
|
||||
* Solves a conditional Gaussian and writes the solution into the entries of
|
||||
* \c x for each frontal variable of the conditional. The parents are
|
||||
* assumed to have already been solved in and their values are read from \c x.
|
||||
* This function works for multiple frontal variables.
|
||||
* NOTE: assumes that the RHS for the frontals is stored in x, and
|
||||
* then replaces the RHS with the partial result for this conditional,
|
||||
* assuming that parents have been solved already.
|
||||
*
|
||||
* @param x values structure with solved parents, and the RHS for this conditional
|
||||
* @return solution \f$ x = R \ (d - Sy - Tz - ...) \f$ for each frontal variable
|
||||
* Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2,
|
||||
* where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator
|
||||
* variables of this conditional, this solve function computes
|
||||
* \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.
|
||||
*
|
||||
* @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the
|
||||
* solution \f$ x_f \f$ will be written.
|
||||
*/
|
||||
void solveInPlace(VectorValues& x) const;
|
||||
|
||||
/**
|
||||
* solves a conditional Gaussian and stores the result in x
|
||||
* Identical to solveInPlace() above, with a permuted x
|
||||
* Solves a conditional Gaussian and writes the solution into the entries of
|
||||
* \c x for each frontal variable of the conditional (version for permuted
|
||||
* VectorValues). The parents are assumed to have already been solved in
|
||||
* and their values are read from \c x. This function works for multiple
|
||||
* frontal variables.
|
||||
*
|
||||
* Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2,
|
||||
* where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator
|
||||
* variables of this conditional, this solve function computes
|
||||
* \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.
|
||||
*
|
||||
* @param x VectorValues structure with solved parents \f$ x_s \f$, and into which the
|
||||
* solution \f$ x_f \f$ will be written.
|
||||
*/
|
||||
void solveInPlace(Permuted<VectorValues>& x) const;
|
||||
|
||||
/**
|
||||
* Solves a conditional Gaussian and returns a new VectorValues
|
||||
* This function works for multiple frontal variables, but should
|
||||
* only be used for testing as it copies the input vector values
|
||||
*
|
||||
* Assumes, as in solveInPlace, that the RHS has been stored in x
|
||||
* for all frontal variables
|
||||
*/
|
||||
VectorValues solve(const VectorValues& x) const;
|
||||
|
||||
// functions for transpose backsubstitution
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -42,7 +42,6 @@ namespace gtsam {
|
|||
Index k = firstFrontalKey();
|
||||
// a VectorValues that only has a value for k: cannot be printed if k<>0
|
||||
x.insert(k, Vector(sigmas_.size()));
|
||||
rhs(x);
|
||||
solveInPlace(x);
|
||||
return x[k];
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@
|
|||
|
||||
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
|
||||
#include <gtsam/linear/GaussianISAM.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
|
||||
#include <gtsam/inference/ISAM-inl.h>
|
||||
|
||||
|
|
@ -50,50 +51,14 @@ Matrix GaussianISAM::marginalCovariance(Index j) const {
|
|||
return Super::jointBayesNet(key1, key2, &EliminateQR);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void optimize(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& result) {
|
||||
// parents are assumed to already be solved and available in result
|
||||
// RHS for current conditional should already be in place in result
|
||||
clique->conditional()->solveInPlace(result);
|
||||
|
||||
BOOST_FOREACH(const BayesTree<GaussianConditional>::sharedClique& child, clique->children_)
|
||||
optimize(child, result);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void treeRHS(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& result) {
|
||||
clique->conditional()->rhs(result);
|
||||
BOOST_FOREACH(const BayesTree<GaussianConditional>::sharedClique& child, clique->children_)
|
||||
treeRHS(child, result);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues rhs(const BayesTree<GaussianConditional>& bayesTree, boost::optional<const GaussianISAM::Dims&> dims) {
|
||||
VectorValues result;
|
||||
if(dims)
|
||||
result = VectorValues(*dims);
|
||||
else
|
||||
result = *allocateVectorValues(bayesTree); // allocate
|
||||
treeRHS(bayesTree.root(), result); // recursively fill
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues optimize(const GaussianISAM& isam) {
|
||||
VectorValues result = rhs(isam, isam.dims_);
|
||||
// starting from the root, call optimize on each conditional
|
||||
optimize(isam.root(), result);
|
||||
VectorValues result(isam.dims_);
|
||||
// Call optimize for BayesTree
|
||||
optimizeInPlace((const BayesTree<GaussianConditional>&)isam, result);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues optimize(const BayesTree<GaussianConditional>& bayesTree) {
|
||||
VectorValues result = rhs(bayesTree);
|
||||
// starting from the root, call optimize on each conditional
|
||||
optimize(bayesTree.root(), result);
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
BayesNet<GaussianConditional> GaussianISAM::shortcut(sharedClique clique, sharedClique root) {
|
||||
return clique->shortcut(root,&EliminateQR);
|
||||
|
|
|
|||
|
|
@ -90,19 +90,7 @@ public:
|
|||
|
||||
}; // \class GaussianISAM
|
||||
|
||||
/** load a VectorValues with the RHS of the system for backsubstitution */
|
||||
VectorValues rhs(const BayesTree<GaussianConditional>& bayesTree, boost::optional<const GaussianISAM::Dims&> dims = boost::none);
|
||||
|
||||
/** recursively load RHS for system */
|
||||
void treeRHS(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& result);
|
||||
|
||||
// recursively optimize this conditional and all subtrees
|
||||
void optimize(const BayesTree<GaussianConditional>::sharedClique& clique, VectorValues& result);
|
||||
|
||||
// optimize the BayesTree, starting from the root
|
||||
VectorValues optimize(const GaussianISAM& isam);
|
||||
|
||||
// optimize the BayesTree, starting from the root
|
||||
VectorValues optimize(const BayesTree<GaussianConditional>& bayesTree);
|
||||
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@
|
|||
#include <gtsam/inference/ClusterTree.h>
|
||||
#include <gtsam/inference/JunctionTree.h>
|
||||
#include <gtsam/linear/GaussianJunctionTree.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
|
||||
#include <vector>
|
||||
|
||||
|
|
@ -33,32 +34,6 @@ namespace gtsam {
|
|||
|
||||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianJunctionTree::btreeBackSubstitute(const BTClique::shared_ptr& current, VectorValues& config) const {
|
||||
// solve the bayes net in the current node
|
||||
current->conditional()->solveInPlace(config);
|
||||
|
||||
// GaussianBayesNet::const_reverse_iterator it = current->rbegin();
|
||||
// for (; it!=current->rend(); ++it) {
|
||||
// (*it)->solveInPlace(config); // solve and store result
|
||||
//
|
||||
//// Vector x = (*it)->solve(config); // Solve for that variable
|
||||
//// config[(*it)->key()] = x; // store result in partial solution
|
||||
// }
|
||||
|
||||
// solve the bayes nets in the child nodes
|
||||
BOOST_FOREACH(const BTClique::shared_ptr& child, current->children()) {
|
||||
btreeBackSubstitute(child, config);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianJunctionTree::btreeRHS(const BTClique::shared_ptr& current, VectorValues& config) const {
|
||||
current->conditional()->rhs(config);
|
||||
BOOST_FOREACH(const BTClique::shared_ptr& child, current->children())
|
||||
btreeRHS(child, config);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues GaussianJunctionTree::optimize(Eliminate function) const {
|
||||
tic(1, "GJT eliminate");
|
||||
|
|
@ -71,12 +46,11 @@ namespace gtsam {
|
|||
vector<size_t> dims(rootClique->conditional()->back()+1, 0);
|
||||
countDims(rootClique, dims);
|
||||
VectorValues result(dims);
|
||||
btreeRHS(rootClique, result);
|
||||
toc(2, "allocate VectorValues");
|
||||
|
||||
// back-substitution
|
||||
tic(3, "back-substitute");
|
||||
btreeBackSubstitute(rootClique, result);
|
||||
internal::optimizeInPlace(rootClique, result);
|
||||
toc(3, "back-substitute");
|
||||
return result;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -45,13 +45,6 @@ namespace gtsam {
|
|||
typedef Base::sharedClique sharedClique;
|
||||
typedef GaussianFactorGraph::Eliminate Eliminate;
|
||||
|
||||
protected:
|
||||
// back-substitute in topological sort order (parents first)
|
||||
void btreeBackSubstitute(const BTClique::shared_ptr& current, VectorValues& config) const;
|
||||
|
||||
// find the RHS for the system in order to perform backsubstitution
|
||||
void btreeRHS(const BTClique::shared_ptr& current, VectorValues& config) const;
|
||||
|
||||
public :
|
||||
|
||||
/** Default constructor */
|
||||
|
|
|
|||
|
|
@ -78,7 +78,8 @@ VectorValues::shared_ptr GaussianSequentialSolver::optimize() const {
|
|||
|
||||
tic(2,"optimize");
|
||||
// Back-substitute
|
||||
VectorValues::shared_ptr solution(gtsam::optimize_(*bayesNet));
|
||||
VectorValues::shared_ptr solution(
|
||||
new VectorValues(gtsam::optimize(*bayesNet)));
|
||||
toc(2,"optimize");
|
||||
|
||||
if(debug) solution->print("GaussianSequentialSolver, solution ");
|
||||
|
|
|
|||
|
|
@ -144,48 +144,6 @@ TEST( GaussianConditional, equals )
|
|||
EXPECT( expected.equals(actual) );
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianConditional, rhs_permuted )
|
||||
{
|
||||
// Test filling the rhs when the VectorValues is permuted
|
||||
|
||||
// Create a VectorValues
|
||||
VectorValues unpermuted(5, 2);
|
||||
unpermuted[0] << 1, 2;
|
||||
unpermuted[1] << 3, 4;
|
||||
unpermuted[2] << 5, 6;
|
||||
unpermuted[3] << 7, 8;
|
||||
unpermuted[4] << 9, 10;
|
||||
|
||||
// Create a permutation
|
||||
Permutation permutation(5);
|
||||
permutation[0] = 4;
|
||||
permutation[1] = 3;
|
||||
permutation[2] = 2;
|
||||
permutation[3] = 1;
|
||||
permutation[4] = 0;
|
||||
|
||||
// Permuted VectorValues
|
||||
Permuted<VectorValues> permuted(permutation, unpermuted);
|
||||
|
||||
// Expected VectorValues
|
||||
VectorValues expected(5, 2);
|
||||
expected[0] << 1, 2;
|
||||
expected[1] << 3, 4;
|
||||
expected[2] << 5, 6;
|
||||
expected[3] << 7, 8;
|
||||
expected[4] << 11, 12;
|
||||
|
||||
// GaussianConditional
|
||||
Vector d(2); d << 11, 12;
|
||||
GaussianConditional conditional(0, d, Matrix::Identity(2,2), Vector::Ones(2));
|
||||
|
||||
// Fill rhs, conditional is on index 0, which should fill slot 4 of the values
|
||||
conditional.rhs(permuted);
|
||||
|
||||
EXPECT(assert_equal(expected, unpermuted));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianConditional, solve )
|
||||
{
|
||||
|
|
@ -208,8 +166,7 @@ TEST( GaussianConditional, solve )
|
|||
|
||||
Vector tau = ones(2);
|
||||
|
||||
// RHS is different than the one in the solution vector
|
||||
GaussianConditional cg(_x_,ones(2), R, _x1_, A1, _l1_, A2, tau);
|
||||
GaussianConditional cg(_x_, d, R, _x1_, A1, _l1_, A2, tau);
|
||||
|
||||
Vector sx1(2);
|
||||
sx1(0) = 1.0; sx1(1) = 1.0;
|
||||
|
|
@ -218,21 +175,16 @@ TEST( GaussianConditional, solve )
|
|||
sl1(0) = 1.0; sl1(1) = 1.0;
|
||||
|
||||
VectorValues solution(vector<size_t>(3, 2));
|
||||
solution[_x_] = d; // RHS
|
||||
solution[_x_] = d;
|
||||
solution[_x1_] = sx1; // parents
|
||||
solution[_l1_] = sl1;
|
||||
|
||||
// NOTE: the solve functions assume the RHS is passed as the initialization of
|
||||
// the solution.
|
||||
VectorValues expected(vector<size_t>(3, 2));
|
||||
expected[_x_] = expectedX;
|
||||
expected[_x1_] = sx1;
|
||||
expected[_l1_] = sl1;
|
||||
|
||||
VectorValues copy_result = cg.solve(solution);
|
||||
cg.solveInPlace(solution);
|
||||
|
||||
EXPECT(assert_equal(expected, copy_result, 0.0001));
|
||||
EXPECT(assert_equal(expected, solution, 0.0001));
|
||||
}
|
||||
|
||||
|
|
@ -240,12 +192,11 @@ TEST( GaussianConditional, solve )
|
|||
TEST( GaussianConditional, solve_simple )
|
||||
{
|
||||
// no pivoting from LDL, so R matrix is not permuted
|
||||
// RHS is deliberately not the same as below
|
||||
Matrix full_matrix = Matrix_(4, 7,
|
||||
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0,
|
||||
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.0);
|
||||
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
|
||||
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
|
||||
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
|
||||
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4);
|
||||
|
||||
// solve system as a non-multifrontal version first
|
||||
// 2 variables, frontal has dim=4
|
||||
|
|
@ -261,7 +212,6 @@ TEST( GaussianConditional, solve_simple )
|
|||
// elimination order; _x_, _x1_
|
||||
vector<size_t> vdim; vdim += 4, 2;
|
||||
VectorValues actual(vdim);
|
||||
actual[_x_] = Vector_(4, 0.1, 0.2, 0.3, 0.4); // d
|
||||
actual[_x1_] = sx1; // parent
|
||||
|
||||
VectorValues expected(vdim);
|
||||
|
|
@ -283,10 +233,10 @@ TEST( GaussianConditional, solve_multifrontal )
|
|||
// create full system, 3 variables, 2 frontals, all 2 dim
|
||||
// no pivoting from LDL, so R matrix is not permuted
|
||||
Matrix full_matrix = Matrix_(4, 7,
|
||||
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.5,
|
||||
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.6,
|
||||
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.7,
|
||||
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.8);
|
||||
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
|
||||
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
|
||||
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
|
||||
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4);
|
||||
|
||||
// 3 variables, all dim=2
|
||||
vector<size_t> dims; dims += 2, 2, 2, 1;
|
||||
|
|
@ -295,15 +245,13 @@ TEST( GaussianConditional, solve_multifrontal )
|
|||
vector<size_t> cgdims; cgdims += _x_, _x1_, _l1_;
|
||||
GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas);
|
||||
|
||||
EXPECT(assert_equal(Vector_(4, 0.5, 0.6, 0.7, 0.8), cg.get_d()));
|
||||
EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d()));
|
||||
|
||||
// partial solution
|
||||
Vector sl1 = Vector_(2, 9.0, 10.0);
|
||||
|
||||
// elimination order; _x_, _x1_, _l1_
|
||||
VectorValues actual(vector<size_t>(3, 2));
|
||||
actual[_x_] = Vector_(2, 0.1, 0.2); // rhs
|
||||
actual[_x1_] = Vector_(2, 0.3, 0.4); // rhs
|
||||
actual[_l1_] = sl1; // parent
|
||||
|
||||
VectorValues expected(vector<size_t>(3, 2));
|
||||
|
|
@ -327,10 +275,10 @@ TEST( GaussianConditional, solve_multifrontal_permuted )
|
|||
// create full system, 3 variables, 2 frontals, all 2 dim
|
||||
// no pivoting from LDL, so R matrix is not permuted
|
||||
Matrix full_matrix = Matrix_(4, 7,
|
||||
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.5,
|
||||
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.6,
|
||||
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.7,
|
||||
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.8);
|
||||
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
|
||||
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
|
||||
0.0, 0.0, 3.0, 0.0, 4.0, 0.0, 0.3,
|
||||
0.0, 0.0, 0.0, 3.0, 0.0, 4.0, 0.4);
|
||||
|
||||
// 3 variables, all dim=2
|
||||
vector<size_t> dims; dims += 2, 2, 2, 1;
|
||||
|
|
@ -339,7 +287,7 @@ TEST( GaussianConditional, solve_multifrontal_permuted )
|
|||
vector<size_t> cgdims; cgdims += _x_, _x1_, _l1_;
|
||||
GaussianConditional cg(cgdims.begin(), cgdims.end(), 2, matrices, sigmas);
|
||||
|
||||
EXPECT(assert_equal(Vector_(4, 0.5, 0.6, 0.7, 0.8), cg.get_d()));
|
||||
EXPECT(assert_equal(Vector_(4, 0.1, 0.2, 0.3, 0.4), cg.get_d()));
|
||||
|
||||
// partial solution
|
||||
Vector sl1 = Vector_(2, 9.0, 10.0);
|
||||
|
|
|
|||
|
|
@ -25,8 +25,9 @@ namespace gtsam {
|
|||
using namespace std;
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace internal {
|
||||
template<class CLIQUE>
|
||||
void optimize2(const boost::shared_ptr<CLIQUE>& clique, double threshold,
|
||||
void optimizeWildfire(const boost::shared_ptr<CLIQUE>& clique, double threshold,
|
||||
vector<bool>& changed, const vector<bool>& replaced, Permuted<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
|
||||
|
|
@ -63,7 +64,6 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
// Back-substitute
|
||||
(*clique)->rhs(delta);
|
||||
(*clique)->solveInPlace(delta);
|
||||
count += (*clique)->nrFrontals();
|
||||
|
||||
|
|
@ -97,42 +97,19 @@ namespace gtsam {
|
|||
|
||||
// Recurse to children
|
||||
BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
|
||||
optimize2(child, threshold, changed, replaced, delta, count);
|
||||
optimizeWildfire(child, threshold, changed, replaced, delta, count);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// fast full version without threshold
|
||||
template<class CLIQUE>
|
||||
void optimize2(const boost::shared_ptr<CLIQUE>& clique, VectorValues& delta) {
|
||||
|
||||
// parents are assumed to already be solved and available in result
|
||||
(*clique)->rhs(delta);
|
||||
(*clique)->solveInPlace(delta);
|
||||
|
||||
// Solve chilren recursively
|
||||
BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) {
|
||||
optimize2(child, delta);
|
||||
}
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//boost::shared_ptr<VectorValues> optimize2(const GaussianISAM2::sharedClique& root) {
|
||||
// boost::shared_ptr<VectorValues> delta(new VectorValues());
|
||||
// set<Key> changed;
|
||||
// // starting from the root, call optimize on each conditional
|
||||
// optimize2(root, delta);
|
||||
// return delta;
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class CLIQUE>
|
||||
int optimize2(const boost::shared_ptr<CLIQUE>& root, double threshold, const vector<bool>& keys, Permuted<VectorValues>& delta) {
|
||||
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root, double threshold, const vector<bool>& keys, Permuted<VectorValues>& delta) {
|
||||
vector<bool> changed(keys.size(), false);
|
||||
int count = 0;
|
||||
// starting from the root, call optimize on each conditional
|
||||
optimize2(root, threshold, changed, keys, delta, count);
|
||||
internal::optimizeWildfire(root, threshold, changed, keys, delta, count);
|
||||
return count;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -63,10 +63,6 @@ public:
|
|||
|
||||
};
|
||||
|
||||
/** optimize the BayesTree, starting from the root */
|
||||
template<class CLIQUE>
|
||||
void optimize2(const boost::shared_ptr<CLIQUE>& root, VectorValues& delta);
|
||||
|
||||
/// optimize the BayesTree, starting from the root; "replaced" needs to contain
|
||||
/// all variables that are contained in the top of the Bayes tree that has been
|
||||
/// redone; "delta" is the current solution, an offset from the linearization
|
||||
|
|
@ -76,7 +72,7 @@ void optimize2(const boost::shared_ptr<CLIQUE>& root, VectorValues& delta);
|
|||
/// variables are contained in the subtree.
|
||||
/// returns the number of variables that were solved for
|
||||
template<class CLIQUE>
|
||||
int optimize2(const boost::shared_ptr<CLIQUE>& root,
|
||||
int optimizeWildfire(const boost::shared_ptr<CLIQUE>& root,
|
||||
double threshold, const std::vector<bool>& replaced, Permuted<VectorValues>& delta);
|
||||
|
||||
/// calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
|
||||
|
|
|
|||
|
|
@ -15,6 +15,8 @@
|
|||
* @author Michael Kaess, Richard Roberts
|
||||
*/
|
||||
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -374,7 +376,7 @@ size_t ISAM2<CONDITIONAL,GRAPH>::Impl::UpdateDelta(const boost::shared_ptr<ISAM2
|
|||
VectorValues newDelta(dims);
|
||||
|
||||
// Optimize full solution delta
|
||||
optimize2(root, newDelta);
|
||||
internal::optimizeInPlace(root, newDelta);
|
||||
|
||||
// Copy solution into delta
|
||||
delta.permutation() = Permutation::Identity(delta.size());
|
||||
|
|
@ -384,7 +386,7 @@ size_t ISAM2<CONDITIONAL,GRAPH>::Impl::UpdateDelta(const boost::shared_ptr<ISAM2
|
|||
|
||||
} else {
|
||||
// Optimize with wildfire
|
||||
lastBacksubVariableCount = optimize2(root, wildfireThreshold, replacedKeys, delta); // modifies delta_
|
||||
lastBacksubVariableCount = optimizeWildfire(root, wildfireThreshold, replacedKeys, delta); // modifies delta_
|
||||
|
||||
#ifndef NDEBUG
|
||||
for(size_t j=0; j<delta.container().size(); ++j)
|
||||
|
|
|
|||
|
|
@ -634,7 +634,7 @@ const Permuted<VectorValues>& ISAM2<CONDITIONAL, GRAPH>::getDelta() const {
|
|||
template<class CONDITIONAL, class GRAPH>
|
||||
VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam) {
|
||||
VectorValues delta = *allocateVectorValues(isam);
|
||||
optimize2(isam.root(), delta);
|
||||
internal::optimizeInPlace(isam.root(), delta);
|
||||
return delta;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -119,64 +119,25 @@ TEST( GaussianBayesNet, optimize2 )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianBayesNet, backSubstitute )
|
||||
TEST( GaussianBayesNet, optimize3 )
|
||||
{
|
||||
// y=R*x, x=inv(R)*y
|
||||
// 2 = 1 1 -1
|
||||
// 3 1 3
|
||||
// 9 = 1 1 4
|
||||
// 5 1 5
|
||||
// NOTE: we are supplying a new RHS here
|
||||
GaussianBayesNet cbn = createSmallGaussianBayesNet();
|
||||
|
||||
VectorValues y(vector<size_t>(2,1)), x(vector<size_t>(2,1));
|
||||
y[_x_] = Vector_(1,2.);
|
||||
y[_y_] = Vector_(1,3.);
|
||||
x[_x_] = Vector_(1,-1.);
|
||||
x[_y_] = Vector_(1, 3.);
|
||||
VectorValues expected(vector<size_t>(2,1)), x(vector<size_t>(2,1));
|
||||
expected[_x_] = Vector_(1, 4.);
|
||||
expected[_y_] = Vector_(1, 5.);
|
||||
|
||||
// test functional version
|
||||
VectorValues actual = backSubstitute(cbn,y);
|
||||
EXPECT(assert_equal(x,actual));
|
||||
VectorValues actual = optimize(cbn);
|
||||
EXPECT(assert_equal(expected,actual));
|
||||
|
||||
// test imperative version
|
||||
backSubstituteInPlace(cbn,y);
|
||||
EXPECT(assert_equal(x,y));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianBayesNet, rhs )
|
||||
{
|
||||
// y=R*x, x=inv(R)*y
|
||||
// 2 = 1 1 -1
|
||||
// 3 1 3
|
||||
GaussianBayesNet cbn = createSmallGaussianBayesNet();
|
||||
VectorValues expected = gtsam::optimize(cbn);
|
||||
VectorValues d = rhs(cbn);
|
||||
VectorValues actual = backSubstitute(cbn, d);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussianBayesNet, rhs_with_sigmas )
|
||||
{
|
||||
Matrix R11 = Matrix_(1, 1, 1.0), S12 = Matrix_(1, 1, 1.0);
|
||||
Matrix R22 = Matrix_(1, 1, 1.0);
|
||||
Vector d1(1), d2(1);
|
||||
d1(0) = 9;
|
||||
d2(0) = 5;
|
||||
Vector tau(1);
|
||||
tau(0) = 0.25;
|
||||
|
||||
// define nodes and specify in reverse topological sort (i.e. parents last)
|
||||
GaussianConditional::shared_ptr Px_y(new GaussianConditional(_x_, d1, R11,
|
||||
_y_, S12, tau)), Py(new GaussianConditional(_y_, d2, R22, tau));
|
||||
GaussianBayesNet cbn;
|
||||
cbn.push_back(Px_y);
|
||||
cbn.push_back(Py);
|
||||
|
||||
VectorValues expected = gtsam::optimize(cbn);
|
||||
VectorValues d = rhs(cbn);
|
||||
VectorValues actual = backSubstitute(cbn, d);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
optimizeInPlace(cbn,x);
|
||||
EXPECT(assert_equal(expected,x));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -16,6 +16,7 @@ using namespace boost::assign;
|
|||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/GaussianSequentialSolver.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
#include <gtsam/nonlinear/GaussianISAM2.h>
|
||||
#include <gtsam/slam/smallExample.h>
|
||||
#include <gtsam/slam/planarSLAM.h>
|
||||
|
|
@ -167,15 +168,13 @@ TEST(ISAM2, optimize2) {
|
|||
|
||||
// Expected vector
|
||||
VectorValues expected(1, 3);
|
||||
conditional->rhs(expected);
|
||||
conditional->solveInPlace(expected);
|
||||
|
||||
// Clique
|
||||
GaussianISAM2<>::sharedClique clique(
|
||||
GaussianISAM2<>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr())));
|
||||
VectorValues actual(theta.dims(ordering));
|
||||
conditional->rhs(actual);
|
||||
optimize2(clique, actual);
|
||||
internal::optimizeInPlace(clique, actual);
|
||||
|
||||
// expected.print("expected: ");
|
||||
// actual.print("actual: ");
|
||||
|
|
|
|||
Loading…
Reference in New Issue