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

536 lines
19 KiB
C++
Raw Blame History

/* ----------------------------------------------------------------------------
* 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 GaussianFactorGraph.cpp
* @brief Linear Factor Graph where all factors are Gaussians
* @author Kai Ni
* @author Christian Potthast
* @author Richard Roberts
* @author Frank Dellaert
*/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/inference/EliminateableFactorGraph-inst.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/cholesky.h>
using namespace std;
using namespace gtsam;
namespace gtsam {
// Instantiate base classes
template class FactorGraph<GaussianFactor>;
template class EliminateableFactorGraph<GaussianFactorGraph>;
/* ************************************************************************* */
bool GaussianFactorGraph::equals(const This& fg, double tol) const
{
return Base::equals(fg, tol);
}
/* ************************************************************************* */
GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
KeySet keys;
for (const sharedFactor& factor: *this)
if (factor)
keys.insert(factor->begin(), factor->end());
return keys;
}
/* ************************************************************************* */
std::map<Key, size_t> GaussianFactorGraph::getKeyDimMap() const {
map<Key, size_t> spec;
for (const GaussianFactor::shared_ptr& gf : *this) {
for (GaussianFactor::const_iterator it = gf->begin(); it != gf->end(); it++) {
map<Key,size_t>::iterator it2 = spec.find(*it);
if ( it2 == spec.end() ) {
spec.emplace(*it, gf->getDim(it));
}
}
}
return spec;
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr GaussianFactorGraph::cloneToPtr() const {
gtsam::GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
*result = *this;
return result;
}
/* ************************************************************************* */
GaussianFactorGraph GaussianFactorGraph::clone() const {
GaussianFactorGraph result;
for (const sharedFactor& f : *this) {
if (f)
result.push_back(f->clone());
else
result.push_back(sharedFactor()); // Passes on null factors so indices remain valid
}
return result;
}
/* ************************************************************************* */
GaussianFactorGraph GaussianFactorGraph::negate() const {
GaussianFactorGraph result;
for (const sharedFactor& factor: *this) {
if (factor)
result.push_back(factor->negate());
else
result.push_back(sharedFactor()); // Passes on null factors so indices remain valid
}
return result;
}
/* ************************************************************************* */
using SparseTriplets = std::vector<std::tuple<int, int, double> >;
SparseTriplets GaussianFactorGraph::sparseJacobian(const Ordering& ordering,
size_t& nrows,
size_t& ncols) const {
gttic_(GaussianFactorGraph_sparseJacobian);
// First find dimensions of each variable
typedef std::map<Key, size_t> KeySizeMap;
KeySizeMap dims;
for (const auto& factor : *this) {
if (!static_cast<bool>(factor)) continue;
for (auto it = factor->begin(); it != factor->end(); ++it) {
dims[*it] = factor->getDim(it);
}
}
// Compute first scalar column of each variable
ncols = 0;
KeySizeMap columnIndices = dims;
for (const auto key : ordering) {
columnIndices[key] = ncols;
ncols += dims[key];
}
// Iterate over all factors, adding sparse scalar entries
SparseTriplets entries;
entries.reserve(30 * size());
nrows = 0;
for (const auto& factor : *this) {
if (!static_cast<bool>(factor)) continue;
// Convert to JacobianFactor if necessary
JacobianFactor::shared_ptr jacobianFactor(
boost::dynamic_pointer_cast<JacobianFactor>(factor));
if (!jacobianFactor) {
HessianFactor::shared_ptr hessian(
boost::dynamic_pointer_cast<HessianFactor>(factor));
if (hessian)
jacobianFactor.reset(new JacobianFactor(*hessian));
else
throw std::invalid_argument(
"GaussianFactorGraph contains a factor that is neither a "
"JacobianFactor nor a HessianFactor.");
}
// Whiten the factor and add entries for it
// iterate over all variables in the factor
const JacobianFactor whitened(jacobianFactor->whiten());
for (auto key = whitened.begin(); key < whitened.end(); ++key) {
JacobianFactor::constABlock whitenedA = whitened.getA(key);
// find first column index for this key
size_t column_start = columnIndices[*key];
for (size_t i = 0; i < (size_t)whitenedA.rows(); i++)
for (size_t j = 0; j < (size_t)whitenedA.cols(); j++) {
double s = whitenedA(i, j);
if (std::abs(s) > 1e-12)
entries.emplace_back(nrows + i, column_start + j, s);
}
}
JacobianFactor::constBVector whitenedb(whitened.getb());
for (size_t i = 0; i < (size_t)whitenedb.size(); i++) {
double s = whitenedb(i);
if (std::abs(s) > 1e-12) entries.emplace_back(nrows + i, ncols, s);
}
// Increment row index
nrows += jacobianFactor->rows();
}
ncols++; // +1 for b-column
return entries;
}
/* ************************************************************************* */
SparseTriplets GaussianFactorGraph::sparseJacobian() const {
size_t nrows, ncols;
return sparseJacobian(Ordering(this->keys()), nrows, ncols);
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::sparseJacobian_() const {
gttic_(GaussianFactorGraph_sparseJacobian_matrix);
// call sparseJacobian
auto result = sparseJacobian();
// translate to base 1 matrix
size_t nzmax = result.size();
Matrix IJS(3, nzmax);
for (size_t k = 0; k < result.size(); k++) {
const auto& entry = result[k];
IJS(0, k) = double(std::get<0>(entry) + 1);
IJS(1, k) = double(std::get<1>(entry) + 1);
IJS(2, k) = std::get<2>(entry);
}
return IJS;
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::augmentedJacobian(
const Ordering& ordering) const {
// combine all factors
JacobianFactor combined(*this, ordering);
return combined.augmentedJacobian();
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::augmentedJacobian() const {
// combine all factors
JacobianFactor combined(*this);
return combined.augmentedJacobian();
}
/* ************************************************************************* */
pair<Matrix, Vector> GaussianFactorGraph::jacobian(
const Ordering& ordering) const {
Matrix augmented = augmentedJacobian(ordering);
return make_pair(augmented.leftCols(augmented.cols() - 1),
augmented.col(augmented.cols() - 1));
}
/* ************************************************************************* */
pair<Matrix, Vector> GaussianFactorGraph::jacobian() const {
Matrix augmented = augmentedJacobian();
return make_pair(augmented.leftCols(augmented.cols() - 1),
augmented.col(augmented.cols() - 1));
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::augmentedHessian(
const Ordering& ordering) const {
// combine all factors and get upper-triangular part of Hessian
Scatter scatter(*this, ordering);
HessianFactor combined(*this, scatter);
return combined.info().selfadjointView();;
}
/* ************************************************************************* */
Matrix GaussianFactorGraph::augmentedHessian() const {
// combine all factors and get upper-triangular part of Hessian
Scatter scatter(*this);
HessianFactor combined(*this, scatter);
return combined.info().selfadjointView();;
}
/* ************************************************************************* */
pair<Matrix, Vector> GaussianFactorGraph::hessian(
const Ordering& ordering) const {
Matrix augmented = augmentedHessian(ordering);
size_t n = augmented.rows() - 1;
return make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1));
}
/* ************************************************************************* */
pair<Matrix, Vector> GaussianFactorGraph::hessian() const {
Matrix augmented = augmentedHessian();
size_t n = augmented.rows() - 1;
return make_pair(augmented.topLeftCorner(n, n), augmented.topRightCorner(n, 1));
}
/* ************************************************************************* */
VectorValues GaussianFactorGraph::hessianDiagonal() const {
VectorValues d;
for (const sharedFactor& factor : *this) {
if(factor){
factor->hessianDiagonalAdd(d);
}
}
return d;
}
/* ************************************************************************* */
map<Key,Matrix> GaussianFactorGraph::hessianBlockDiagonal() const {
map<Key,Matrix> blocks;
for (const sharedFactor& factor : *this) {
if (!factor) continue;
map<Key,Matrix> BD = factor->hessianBlockDiagonal();
map<Key,Matrix>::const_iterator it = BD.begin();
for (;it!=BD.end();++it) {
Key j = it->first; // variable key for this block
const Matrix& Bj = it->second;
if (blocks.count(j))
blocks[j] += Bj;
else
blocks.emplace(j,Bj);
}
}
return blocks;
}
/* ************************************************************************ */
VectorValues GaussianFactorGraph::optimize(const Eliminate& function) const {
gttic(GaussianFactorGraph_optimize);
return BaseEliminateable::eliminateMultifrontal(Ordering::COLAMD, function)
->optimize();
}
/* ************************************************************************* */
VectorValues GaussianFactorGraph::optimize(const Ordering& ordering, const Eliminate& function) const {
gttic(GaussianFactorGraph_optimize);
return BaseEliminateable::eliminateMultifrontal(ordering, function)->optimize();
}
/* ************************************************************************* */
// TODO(frank): can we cache memory across invocations
VectorValues GaussianFactorGraph::optimizeDensely() const {
gttic(GaussianFactorGraph_optimizeDensely);
// Combine all factors in a single HessianFactor (as done in augmentedHessian)
Scatter scatter(*this);
HessianFactor combined(*this, scatter);
// TODO(frank): cast to large dynamic matrix :-(
// NOTE(frank): info only valid (I think) in upper triangle. No problem for LLT...
Matrix augmented = combined.info().selfadjointView();
// Do Cholesky Factorization
size_t n = augmented.rows() - 1;
auto AtA = augmented.topLeftCorner(n, n);
auto eta = augmented.topRightCorner(n, 1);
Eigen::LLT<Matrix, Eigen::Upper> llt(AtA);
// Solve and convert, re-using scatter data structure
Vector solution = llt.solve(eta);
return VectorValues(solution, scatter);
}
/* ************************************************************************* */
namespace {
JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) {
JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactor>(gf);
if( !result )
// Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
result = boost::make_shared<JacobianFactor>(*gf);
return result;
}
}
/* ************************************************************************* */
VectorValues GaussianFactorGraph::gradient(const VectorValues& x0) const
{
VectorValues g = VectorValues::Zero(x0);
for (const sharedFactor& factor: *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor);
Vector e = Ai->error_vector(x0);
Ai->transposeMultiplyAdd(1.0, e, g);
}
return g;
}
/* ************************************************************************* */
VectorValues GaussianFactorGraph::gradientAtZero() const {
// Zero-out the gradient
VectorValues g;
for (const sharedFactor& factor: *this) {
if (!factor) continue;
VectorValues gi = factor->gradientAtZero();
g.addInPlace_(gi);
}
return g;
}
/* ************************************************************************* */
VectorValues GaussianFactorGraph::optimizeGradientSearch() const
{
gttic(GaussianFactorGraph_optimizeGradientSearch);
gttic(Compute_Gradient);
// Compute gradient (call gradientAtZero function, which is defined for various linear systems)
VectorValues grad = gradientAtZero();
double gradientSqNorm = grad.dot(grad);
gttoc(Compute_Gradient);
gttic(Compute_Rg);
// Compute R * g
Errors Rg = *this * grad;
gttoc(Compute_Rg);
gttic(Compute_minimizing_step_size);
// Compute minimizing step size
double step = -gradientSqNorm / dot(Rg, Rg);
gttoc(Compute_minimizing_step_size);
gttic(Compute_point);
// Compute steepest descent point
grad *= step;
gttoc(Compute_point);
return grad;
}
/* ************************************************************************* */
Errors GaussianFactorGraph::operator*(const VectorValues& x) const {
Errors e;
for (const GaussianFactor::shared_ptr& factor: *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor);
e.push_back((*Ai) * x);
}
return e;
}
/* ************************************************************************* */
void GaussianFactorGraph::multiplyHessianAdd(double alpha,
const VectorValues& x, VectorValues& y) const {
for (const GaussianFactor::shared_ptr& f: *this)
f->multiplyHessianAdd(alpha, x, y);
}
/* ************************************************************************* */
void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const {
multiplyInPlace(x, e.begin());
}
/* ************************************************************************* */
void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const {
Errors::iterator ei = e;
for (const GaussianFactor::shared_ptr& factor: *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor);
*ei = (*Ai)*x;
ei++;
}
}
/* ************************************************************************* */
bool hasConstraints(const GaussianFactorGraph& factors) {
typedef JacobianFactor J;
for (const GaussianFactor::shared_ptr& factor: factors) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) {
return true;
}
}
return false;
}
/* ************************************************************************* */
// x += alpha*A'*e
void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
VectorValues& x) const {
// For each factor add the gradient contribution
Errors::const_iterator ei = e.begin();
for (const sharedFactor& factor: *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor);
Ai->transposeMultiplyAdd(alpha, *(ei++), x);
}
}
///* ************************************************************************* */
//void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
// Key i = 0 ;
// for (const GaussianFactor::shared_ptr& factor, fg) {
// JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor);
// r[i] = Ai->getb();
// i++;
// }
// VectorValues Ax = VectorValues::SameStructure(r);
// multiply(fg,x,Ax);
// axpy(-1.0,Ax,r);
//}
///* ************************************************************************* */
//void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
// r.setZero();
// Key i = 0;
// for (const GaussianFactor::shared_ptr& factor, fg) {
// JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor);
// Vector &y = r[i];
// for (JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
// y += Ai->getA(j) * x[*j];
// }
// ++i;
// }
//}
/* ************************************************************************* */
VectorValues GaussianFactorGraph::transposeMultiply(const Errors& e) const
{
VectorValues x;
Errors::const_iterator ei = e.begin();
for (const sharedFactor& factor: *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor);
for (JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
// Create the value as a zero vector if it does not exist.
pair<VectorValues::iterator, bool> xi = x.tryInsert(*j, Vector());
if(xi.second)
xi.first->second = Vector::Zero(Ai->getDim(j));
xi.first->second += Ai->getA(j).transpose() * *ei;
}
++ ei;
}
return x;
}
/* ************************************************************************* */
Errors GaussianFactorGraph::gaussianErrors(const VectorValues& x) const
{
Errors e;
for (const sharedFactor& factor: *this) {
JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(factor);
e.push_back(Ai->error_vector(x));
}
return e;
}
/* ************************************************************************* */
void GaussianFactorGraph::printErrors(
const VectorValues& values, const std::string& str,
const KeyFormatter& keyFormatter,
const std::function<bool(const Factor* /*factor*/,
double /*whitenedError*/, size_t /*index*/)>&
printCondition) const {
cout << str << "size: " << size() << endl << endl;
for (size_t i = 0; i < (*this).size(); i++) {
const sharedFactor& factor = (*this)[i];
const double errorValue =
(factor != nullptr ? (*this)[i]->error(values) : .0);
if (!printCondition(factor.get(), errorValue, i))
continue; // User-provided filter did not pass
stringstream ss;
ss << "Factor " << i << ": ";
if (factor == nullptr) {
cout << "nullptr"
<< "\n";
} else {
factor->print(ss.str(), keyFormatter);
cout << "error = " << errorValue << "\n";
}
cout << endl; // only one "endl" at end might be faster, \n for each
// factor
}
}
} // namespace gtsam
Reference in New Issue View Git Blame Copy Permalink