Removed commented code in GaussianFactorGraph
Richard Roberts
parent
d94fa41f8a
commit
b37346dffb
|
|
@ -40,7 +40,7 @@
|
|||
|
||||
#define INSTANTIATE_FACTOR_GRAPH(F) \
|
||||
template class FactorGraph<F>; \
|
||||
template FactorGraph<F> combine(const FactorGraph<F>&, const FactorGraph<F>&);
|
||||
template FactorGraph<F> combine(const FactorGraph<F>&, const FactorGraph<F>&)
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ using namespace boost::assign;
|
|||
namespace gtsam {
|
||||
|
||||
// Explicitly instantiate so we don't have to include everywhere
|
||||
template class FactorGraph<GaussianFactor>;
|
||||
INSTANTIATE_FACTOR_GRAPH(GaussianFactor);
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) :
|
||||
|
|
@ -46,8 +46,7 @@ GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) :
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::set<Index, std::less<Index>, boost::fast_pool_allocator<Index> >
|
||||
GaussianFactorGraph::keys() const {
|
||||
GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
|
||||
std::set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > keys;
|
||||
BOOST_FOREACH(const sharedFactor& factor, *this) {
|
||||
if(factor) keys.insert(factor->begin(), factor->end()); }
|
||||
|
|
@ -135,199 +134,6 @@ VectorValues GaussianFactorGraph::gradient(const VectorValues& x) const {
|
|||
return g;
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//set<Index> GaussianFactorGraph::find_separator(Index key) const
|
||||
//{
|
||||
// set<Index> separator;
|
||||
// BOOST_FOREACH(const sharedFactor& factor,factors_)
|
||||
// factor->tally_separator(key,separator);
|
||||
//
|
||||
// return separator;
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//template <class Factors>
|
||||
//std::pair<Matrix, SharedDiagonal> combineFactorsAndCreateMatrix(
|
||||
// const Factors& factors,
|
||||
// const Ordering& order, const Dimensions& dimensions) {
|
||||
// // find the size of Ab
|
||||
// size_t m = 0, n = 1;
|
||||
//
|
||||
// // number of rows
|
||||
// BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) {
|
||||
// m += factor->numberOfRows();
|
||||
// }
|
||||
//
|
||||
// // find the number of columns
|
||||
// BOOST_FOREACH(Index key, order) {
|
||||
// n += dimensions.at(key);
|
||||
// }
|
||||
//
|
||||
// // Allocate the new matrix
|
||||
// Matrix Ab = zeros(m,n);
|
||||
//
|
||||
// // Allocate a sigmas vector to make into a full noisemodel
|
||||
// Vector sigmas = ones(m);
|
||||
//
|
||||
// // copy data over
|
||||
// size_t cur_m = 0;
|
||||
// bool constrained = false;
|
||||
// bool unit = true;
|
||||
// BOOST_FOREACH(GaussianFactor::shared_ptr factor, factors) {
|
||||
// // loop through ordering
|
||||
// size_t cur_n = 0;
|
||||
// BOOST_FOREACH(Index key, order) {
|
||||
// // copy over matrix if it exists
|
||||
// if (factor->involves(key)) {
|
||||
// insertSub(Ab, factor->get_A(key), cur_m, cur_n);
|
||||
// }
|
||||
// // move onto next element
|
||||
// cur_n += dimensions.at(key);
|
||||
// }
|
||||
// // copy over the RHS
|
||||
// insertColumn(Ab, factor->get_b(), cur_m, n-1);
|
||||
//
|
||||
// // check if the model is unit already
|
||||
// if (!boost::shared_dynamic_cast<noiseModel::Unit>(factor->get_model())) {
|
||||
// unit = false;
|
||||
// const Vector& subsigmas = factor->get_model()->sigmas();
|
||||
// subInsert(sigmas, subsigmas, cur_m);
|
||||
//
|
||||
// // check for constraint
|
||||
// if (boost::shared_dynamic_cast<noiseModel::Constrained>(factor->get_model()))
|
||||
// constrained = true;
|
||||
// }
|
||||
//
|
||||
// // move to next row
|
||||
// cur_m += factor->numberOfRows();
|
||||
// }
|
||||
//
|
||||
// // combine the noisemodels
|
||||
// SharedDiagonal model;
|
||||
// if (unit) {
|
||||
// model = noiseModel::Unit::Create(m);
|
||||
// } else if (constrained) {
|
||||
// model = noiseModel::Constrained::MixedSigmas(sigmas);
|
||||
// } else {
|
||||
// model = noiseModel::Diagonal::Sigmas(sigmas);
|
||||
// }
|
||||
// return make_pair(Ab, model);
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//GaussianConditional::shared_ptr
|
||||
//GaussianFactorGraph::eliminateOneMatrixJoin(Index key) {
|
||||
// // find and remove all factors connected to key
|
||||
// vector<GaussianFactor::shared_ptr> factors = findAndRemoveFactors(key);
|
||||
//
|
||||
// // Collect all dimensions as well as the set of separator keys
|
||||
// set<Index> separator;
|
||||
// Dimensions dimensions;
|
||||
// BOOST_FOREACH(const sharedFactor& factor, factors) {
|
||||
// Dimensions factor_dim = factor->dimensions();
|
||||
// dimensions.insert(factor_dim.begin(), factor_dim.end());
|
||||
// BOOST_FOREACH(Index k, factor->keys())
|
||||
// if (!k == key)
|
||||
// separator.insert(k);
|
||||
// }
|
||||
//
|
||||
// // add the keys to the rendering
|
||||
// Ordering render; render += key;
|
||||
// BOOST_FOREACH(Index k, separator)
|
||||
// if (k != key) render += k;
|
||||
//
|
||||
// // combine the factors to get a noisemodel and a combined matrix
|
||||
// Matrix Ab; SharedDiagonal model;
|
||||
//
|
||||
// boost::tie(Ab, model) = combineFactorsAndCreateMatrix(factors,render,dimensions);
|
||||
//
|
||||
// // eliminate that joint factor
|
||||
// GaussianFactor::shared_ptr factor;
|
||||
// GaussianConditional::shared_ptr conditional;
|
||||
// render.pop_front();
|
||||
// boost::tie(conditional, factor) =
|
||||
// GaussianFactor::eliminateMatrix(Ab, model, key, render, dimensions);
|
||||
//
|
||||
// // add new factor on separator back into the graph
|
||||
// if (!factor->empty()) push_back(factor);
|
||||
//
|
||||
// // return the conditional Gaussian
|
||||
// return conditional;
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//GaussianBayesNet
|
||||
//GaussianFactorGraph::eliminateFrontals(const Ordering& frontals)
|
||||
//{
|
||||
// // find the factors that contain at least one of the frontal variables
|
||||
// Dimensions dimensions = this->dimensions();
|
||||
//
|
||||
// // collect separator
|
||||
// Ordering separator;
|
||||
// set<Index> frontal_set(frontals.begin(), frontals.end());
|
||||
// BOOST_FOREACH(Index key, this->keys()) {
|
||||
// if (frontal_set.find(key) == frontal_set.end())
|
||||
// separator.push_back(key);
|
||||
// }
|
||||
//
|
||||
// Matrix Ab; SharedDiagonal model;
|
||||
// Ordering ord = frontals;
|
||||
// ord.insert(ord.end(), separator.begin(), separator.end());
|
||||
// boost::tie(Ab, model) = combineFactorsAndCreateMatrix(*this, ord, dimensions);
|
||||
//
|
||||
// // eliminate that joint factor
|
||||
// GaussianFactor::shared_ptr factor;
|
||||
// GaussianBayesNet bn;
|
||||
// boost::tie(bn, factor) =
|
||||
// GaussianFactor::eliminateMatrix(Ab, model, frontals, separator, dimensions);
|
||||
//
|
||||
// // add new factor on separator back into the graph
|
||||
// *this = GaussianFactorGraph();
|
||||
// if (!factor->empty()) push_back(factor);
|
||||
//
|
||||
// return bn;
|
||||
//}
|
||||
|
||||
|
||||
///* ************************************************************************* */
|
||||
//VectorValues GaussianFactorGraph::optimize(const Ordering& ordering, bool old)
|
||||
//{
|
||||
// // eliminate all nodes in the given ordering -> chordal Bayes net
|
||||
// GaussianBayesNet chordalBayesNet = eliminate(ordering, old);
|
||||
//
|
||||
// // calculate new values structure (using backsubstitution)
|
||||
// VectorValues delta = ::optimize(chordalBayesNet);
|
||||
// return delta;
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//VectorValues GaussianFactorGraph::optimizeMultiFrontals(const Ordering& ordering)
|
||||
//{
|
||||
// GaussianJunctionTree junctionTree(*this, ordering);
|
||||
//
|
||||
// // calculate new values structure (using backsubstitution)
|
||||
// VectorValues delta = junctionTree.optimize();
|
||||
// return delta;
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//boost::shared_ptr<GaussianBayesNet>
|
||||
//GaussianFactorGraph::eliminate_(const Ordering& ordering)
|
||||
//{
|
||||
// boost::shared_ptr<GaussianBayesNet> chordalBayesNet(new GaussianBayesNet); // empty
|
||||
// BOOST_FOREACH(Index key, ordering) {
|
||||
// GaussianConditional::shared_ptr cg = eliminateOne(key);
|
||||
// chordalBayesNet->push_back(cg);
|
||||
// }
|
||||
// return chordalBayesNet;
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//boost::shared_ptr<VectorValues>
|
||||
//GaussianFactorGraph::optimize_(const Ordering& ordering) {
|
||||
// return boost::shared_ptr<VectorValues>(new VectorValues(optimize(ordering)));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg){
|
||||
for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){
|
||||
|
|
@ -350,18 +156,6 @@ GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg
|
|||
return fg;
|
||||
}
|
||||
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//Dimensions GaussianFactorGraph::dimensions() const {
|
||||
// Dimensions result;
|
||||
// BOOST_FOREACH(const sharedFactor& factor,factors_) {
|
||||
// Dimensions vs = factor->dimensions();
|
||||
// Index key; size_t dim;
|
||||
// FOREACH_PAIR(key,dim,vs) result.insert(make_pair(key,dim));
|
||||
// }
|
||||
// return result;
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const GaussianVariableIndex<>& variableIndex) const {
|
||||
|
||||
|
|
@ -380,178 +174,4 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const Gaussian
|
|||
return result;
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//Errors GaussianFactorGraph::rhs() const {
|
||||
// Errors e;
|
||||
// BOOST_FOREACH(const sharedFactor& factor,factors_)
|
||||
// e.push_back(ediv(factor->get_b(),factor->get_sigmas()));
|
||||
// return e;
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//Vector GaussianFactorGraph::rhsVector() const {
|
||||
// Errors e = rhs();
|
||||
// return concatVectors(e);
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//pair<Matrix,Vector> GaussianFactorGraph::matrix(const Ordering& ordering) const {
|
||||
//
|
||||
// // get all factors
|
||||
// GaussianFactorSet found;
|
||||
// BOOST_FOREACH(const sharedFactor& factor,factors_)
|
||||
// found.push_back(factor);
|
||||
//
|
||||
// // combine them
|
||||
// GaussianFactor lf(found);
|
||||
//
|
||||
// // Return Matrix and Vector
|
||||
// return lf.matrix(ordering);
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//VectorValues GaussianFactorGraph::assembleValues(const Vector& vs, const Ordering& ordering) const {
|
||||
// Dimensions dims = dimensions();
|
||||
// VectorValues config;
|
||||
// Vector::const_iterator itSrc = vs.begin();
|
||||
// Vector::iterator itDst;
|
||||
// BOOST_FOREACH(Index key, ordering){
|
||||
// int dim = dims.find(key)->second;
|
||||
// Vector v(dim);
|
||||
// for (itDst=v.begin(); itDst!=v.end(); itDst++, itSrc++)
|
||||
// *itDst = *itSrc;
|
||||
// config.insert(key, v);
|
||||
// }
|
||||
// if (itSrc != vs.end())
|
||||
// throw runtime_error("assembleValues: input vector and ordering are not compatible with the graph");
|
||||
// return config;
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//pair<Dimensions, size_t> GaussianFactorGraph::columnIndices_(const Ordering& ordering) const {
|
||||
//
|
||||
// // get the dimensions for all variables
|
||||
// Dimensions variableSet = dimensions();
|
||||
//
|
||||
// // Find the starting index and dimensions for all variables given the order
|
||||
// size_t j = 1;
|
||||
// Dimensions result;
|
||||
// BOOST_FOREACH(Index key, ordering) {
|
||||
// // associate key with first column index
|
||||
// result.insert(make_pair(key,j));
|
||||
// // find dimension for this key
|
||||
// Dimensions::const_iterator it = variableSet.find(key);
|
||||
// if (it==variableSet.end()) // key not found, now what ?
|
||||
// throw invalid_argument("ColumnIndices: this ordering contains keys not in the graph");
|
||||
// // advance column index to next block by adding dim(key)
|
||||
// j += it->second;
|
||||
// }
|
||||
//
|
||||
// return make_pair(result, j-1);
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//Dimensions GaussianFactorGraph::columnIndices(const Ordering& ordering) const {
|
||||
// return columnIndices_(ordering).first;
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//pair<size_t, size_t> GaussianFactorGraph::sizeOfA() const {
|
||||
// size_t m = 0, n = 0;
|
||||
// Dimensions variableSet = dimensions();
|
||||
// BOOST_FOREACH(const Dimensions::value_type value, variableSet)
|
||||
// n += value.second;
|
||||
// BOOST_FOREACH(const sharedFactor& factor,factors_)
|
||||
// m += factor->numberOfRows();
|
||||
// return make_pair(m, n);
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//Matrix GaussianFactorGraph::sparse(const Ordering& ordering) const {
|
||||
//
|
||||
// // get the starting column indices for all variables
|
||||
// Dimensions indices = columnIndices(ordering);
|
||||
//
|
||||
// return sparse(indices);
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//Matrix GaussianFactorGraph::sparse(const Dimensions& indices) const {
|
||||
//
|
||||
// // return values
|
||||
// list<int> I,J;
|
||||
// list<double> S;
|
||||
//
|
||||
// // Collect the I,J,S lists for all factors
|
||||
// int row_index = 0;
|
||||
// BOOST_FOREACH(const sharedFactor& factor,factors_) {
|
||||
//
|
||||
// // get sparse lists for the factor
|
||||
// list<int> i1,j1;
|
||||
// list<double> s1;
|
||||
// boost::tie(i1,j1,s1) = factor->sparse(indices);
|
||||
//
|
||||
// // add row_start to every row index
|
||||
// transform(i1.begin(), i1.end(), i1.begin(), bind2nd(plus<int>(), row_index));
|
||||
//
|
||||
// // splice lists from factor to the end of the global lists
|
||||
// I.splice(I.end(), i1);
|
||||
// J.splice(J.end(), j1);
|
||||
// S.splice(S.end(), s1);
|
||||
//
|
||||
// // advance row start
|
||||
// row_index += factor->numberOfRows();
|
||||
// }
|
||||
//
|
||||
// // Convert them to vectors for MATLAB
|
||||
// // TODO: just create a sparse matrix class already
|
||||
// size_t nzmax = S.size();
|
||||
// Matrix ijs(3,nzmax);
|
||||
// copy(I.begin(),I.end(),ijs.begin2());
|
||||
// copy(J.begin(),J.end(),ijs.begin2()+nzmax);
|
||||
// copy(S.begin(),S.end(),ijs.begin2()+2*nzmax);
|
||||
//
|
||||
// // return the result
|
||||
// return ijs;
|
||||
//}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//VectorValues GaussianFactorGraph::steepestDescent(const VectorValues& x0,
|
||||
// bool verbose, double epsilon, size_t maxIterations) const {
|
||||
// return gtsam::steepestDescent(*this, x0, verbose, epsilon, maxIterations);
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//boost::shared_ptr<VectorValues> GaussianFactorGraph::steepestDescent_(
|
||||
// const VectorValues& x0, bool verbose, double epsilon, size_t maxIterations) const {
|
||||
// return boost::shared_ptr<VectorValues>(new VectorValues(
|
||||
// gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
|
||||
// maxIterations)));
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//VectorValues GaussianFactorGraph::conjugateGradientDescent(
|
||||
// const VectorValues& x0, bool verbose, double epsilon, size_t maxIterations) const {
|
||||
// return gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
|
||||
// maxIterations);
|
||||
//}
|
||||
//
|
||||
///* ************************************************************************* */
|
||||
//boost::shared_ptr<VectorValues> GaussianFactorGraph::conjugateGradientDescent_(
|
||||
// const VectorValues& x0, bool verbose, double epsilon, size_t maxIterations) const {
|
||||
// return boost::shared_ptr<VectorValues>(new VectorValues(
|
||||
// gtsam::conjugateGradientDescent(*this, x0, verbose, epsilon,
|
||||
// maxIterations)));
|
||||
//}
|
||||
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
//template std::pair<Matrix, SharedDiagonal> combineFactorsAndCreateMatrix<vector<GaussianFactor::shared_ptr> >(
|
||||
// const vector<GaussianFactor::shared_ptr>& factors, const Ordering& order, const Dimensions& dimensions);
|
||||
//
|
||||
//template std::pair<Matrix, SharedDiagonal> combineFactorsAndCreateMatrix<GaussianFactorGraph>(
|
||||
// const GaussianFactorGraph& factors, const Ordering& order, const Dimensions& dimensions);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -52,25 +52,25 @@ namespace gtsam {
|
|||
GaussianFactorGraph(const GaussianBayesNet& CBN);
|
||||
|
||||
/** Add a null factor */
|
||||
inline void add(const Vector& b) {
|
||||
void add(const Vector& b) {
|
||||
push_back(sharedFactor(new GaussianFactor(b)));
|
||||
}
|
||||
|
||||
/** Add a unary factor */
|
||||
inline void add(Index key1, const Matrix& A1,
|
||||
void add(Index key1, const Matrix& A1,
|
||||
const Vector& b, const SharedDiagonal& model) {
|
||||
push_back(sharedFactor(new GaussianFactor(key1,A1,b,model)));
|
||||
}
|
||||
|
||||
/** Add a binary factor */
|
||||
inline void add(Index key1, const Matrix& A1,
|
||||
void add(Index key1, const Matrix& A1,
|
||||
Index key2, const Matrix& A2,
|
||||
const Vector& b, const SharedDiagonal& model) {
|
||||
push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,b,model)));
|
||||
}
|
||||
|
||||
/** Add a ternary factor */
|
||||
inline void add(Index key1, const Matrix& A1,
|
||||
void add(Index key1, const Matrix& A1,
|
||||
Index key2, const Matrix& A2,
|
||||
Index key3, const Matrix& A3,
|
||||
const Vector& b, const SharedDiagonal& model) {
|
||||
|
|
@ -78,7 +78,7 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** Add an n-ary factor */
|
||||
inline void add(const std::vector<std::pair<Index, Matrix> > &terms,
|
||||
void add(const std::vector<std::pair<Index, Matrix> > &terms,
|
||||
const Vector &b, const SharedDiagonal& model) {
|
||||
push_back(sharedFactor(new GaussianFactor(terms,b,model)));
|
||||
}
|
||||
|
|
@ -87,7 +87,8 @@ namespace gtsam {
|
|||
* Return the set of variables involved in the factors (computes a set
|
||||
* union).
|
||||
*/
|
||||
std::set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > keys() const;
|
||||
typedef std::set<Index, std::less<Index>, boost::fast_pool_allocator<Index> > Keys;
|
||||
Keys keys() const;
|
||||
|
||||
/** Permute the variables in the factors */
|
||||
void permuteWithInverse(const Permutation& inversePermutation);
|
||||
|
|
@ -128,44 +129,6 @@ namespace gtsam {
|
|||
return exp(-0.5 * error(c));
|
||||
}
|
||||
|
||||
// /**
|
||||
// * find the separator, i.e. all the nodes that have at least one
|
||||
// * common factor with the given node. FD: not used AFAIK.
|
||||
// */
|
||||
// std::set<Index> find_separator(Index key) const;
|
||||
|
||||
// /**
|
||||
// * Peforms a supposedly-faster (fewer matrix copy) version of elimination
|
||||
// * CURRENTLY IN TESTING
|
||||
// */
|
||||
// GaussianConditional::shared_ptr eliminateOneMatrixJoin(Index key);
|
||||
//
|
||||
//
|
||||
// /**
|
||||
// * Eliminate multiple variables at once, mostly used to eliminate frontal variables
|
||||
// */
|
||||
// GaussianBayesNet eliminateFrontals(const Ordering& frontals);
|
||||
|
||||
// /**
|
||||
// * optimize a linear factor graph
|
||||
// * @param ordering fg in order
|
||||
// * @param enableJoinFactor uses the older joint factor combine process when true,
|
||||
// * and when false uses the newer single matrix combine
|
||||
// */
|
||||
// VectorValues optimize(const Ordering& ordering, bool enableJoinFactor = true);
|
||||
|
||||
// /**
|
||||
// * optimize a linear factor graph using a multi-frontal solver
|
||||
// * @param ordering fg in order
|
||||
// */
|
||||
// VectorValues optimizeMultiFrontals(const Ordering& ordering);
|
||||
|
||||
// /**
|
||||
// * shared pointer versions for MATLAB
|
||||
// */
|
||||
// boost::shared_ptr<GaussianBayesNet> eliminate_(const Ordering&);
|
||||
// boost::shared_ptr<VectorValues> optimize_(const Ordering&);
|
||||
|
||||
/**
|
||||
* static function that combines two factor graphs
|
||||
* @param const &lfg1 Linear factor graph
|
||||
|
|
@ -181,11 +144,6 @@ namespace gtsam {
|
|||
*/
|
||||
void combine(const GaussianFactorGraph &lfg);
|
||||
|
||||
// /**
|
||||
// * Find all variables and their dimensions
|
||||
// * @return The set of all variable/dimension pairs
|
||||
// */
|
||||
// Dimensions dimensions() const;
|
||||
|
||||
/**
|
||||
* Add zero-mean i.i.d. Gaussian prior terms to each variable
|
||||
|
|
@ -193,84 +151,6 @@ namespace gtsam {
|
|||
*/
|
||||
GaussianFactorGraph add_priors(double sigma, const GaussianVariableIndex<>& variableIndex) const;
|
||||
|
||||
// /**
|
||||
// * Return RHS (b./sigmas) as Errors class
|
||||
// */
|
||||
// Errors rhs() const;
|
||||
//
|
||||
// /**
|
||||
// * Return RHS (b./sigmas) as Vector
|
||||
// */
|
||||
// Vector rhsVector() const;
|
||||
//
|
||||
// /**
|
||||
// * Return (dense) matrix associated with factor graph
|
||||
// * @param ordering of variables needed for matrix column order
|
||||
// */
|
||||
// std::pair<Matrix,Vector> matrix (const Ordering& ordering) const;
|
||||
|
||||
// /**
|
||||
// * split the source vector w.r.t. the given ordering and assemble a vector config
|
||||
// * @param v: the source vector
|
||||
// * @param ordeirng: the ordering corresponding to the vector
|
||||
// */
|
||||
// VectorValues assembleValues(const Vector& v, const Ordering& ordering) const;
|
||||
//
|
||||
// /**
|
||||
// * get the 1-based starting column indices for all variables
|
||||
// * @param ordering of variables needed for matrix column order
|
||||
// * @return The set of all variable/index pairs
|
||||
// */
|
||||
// std::pair<Dimensions, size_t> columnIndices_(const Ordering& ordering) const;
|
||||
// Dimensions columnIndices(const Ordering& ordering) const;
|
||||
//
|
||||
// /**
|
||||
// * return the size of corresponding A matrix
|
||||
// */
|
||||
// std::pair<std::size_t, std::size_t> sizeOfA() const;
|
||||
|
||||
// /**
|
||||
// * Return 3*nzmax matrix where the rows correspond to the vectors i, j, and s
|
||||
// * to generate an m-by-n sparse matrix, which can be given to MATLAB's sparse function.
|
||||
// * The standard deviations are baked into A and b
|
||||
// * @param ordering of variables needed for matrix column order
|
||||
// */
|
||||
// Matrix sparse(const Ordering& ordering) const;
|
||||
//
|
||||
// /**
|
||||
// * Version that takes column indices rather than ordering
|
||||
// */
|
||||
// Matrix sparse(const Dimensions& indices) const;
|
||||
|
||||
// /**
|
||||
// * Find solution using gradient descent
|
||||
// * @param x0: VectorValues specifying initial estimate
|
||||
// * @return solution
|
||||
// */
|
||||
// VectorValues steepestDescent(const VectorValues& x0, bool verbose = false,
|
||||
// double epsilon = 1e-3, size_t maxIterations = 0) const;
|
||||
//
|
||||
// /**
|
||||
// * shared pointer versions for MATLAB
|
||||
// */
|
||||
// boost::shared_ptr<VectorValues>
|
||||
// steepestDescent_(const VectorValues& x0, bool verbose = false,
|
||||
// double epsilon = 1e-3, size_t maxIterations = 0) const;
|
||||
//
|
||||
// /**
|
||||
// * Find solution using conjugate gradient descent
|
||||
// * @param x0: VectorValues specifying initial estimate
|
||||
// * @return solution
|
||||
// */
|
||||
// VectorValues conjugateGradientDescent(const VectorValues& x0, bool verbose =
|
||||
// false, double epsilon = 1e-3, size_t maxIterations = 0) const;
|
||||
//
|
||||
// /**
|
||||
// * shared pointer versions for MATLAB
|
||||
// */
|
||||
// boost::shared_ptr<VectorValues> conjugateGradientDescent_(
|
||||
// const VectorValues& x0, bool verbose = false, double epsilon = 1e-3,
|
||||
// size_t maxIterations = 0) const;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue