82 lines
2.1 KiB
C++
82 lines
2.1 KiB
C++
/**
|
|
* @file Factorization
|
|
* @brief Template Linear solver class that uses a Gaussian Factor Graph
|
|
* @author Kai Ni
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
// $Id: GaussianFactorGraph.h,v 1.24 2009/08/14 20:48:51 acunning Exp $
|
|
|
|
#pragma once
|
|
|
|
#include <stdexcept>
|
|
#include "GaussianFactorGraph.h"
|
|
|
|
namespace gtsam {
|
|
|
|
class Ordering;
|
|
|
|
/**
|
|
* A linear system solver using factorization
|
|
*/
|
|
template <class NonlinearGraph, class Config>
|
|
class Factorization {
|
|
private:
|
|
boost::shared_ptr<const Ordering> ordering_;
|
|
bool useOldEliminate_;
|
|
|
|
public:
|
|
Factorization(boost::shared_ptr<const Ordering> ordering, bool old=true)
|
|
: ordering_(ordering), useOldEliminate_(old) {
|
|
if (!ordering) throw std::invalid_argument("Factorization constructor: ordering = NULL");
|
|
}
|
|
|
|
/**
|
|
* solve for the optimal displacement in the tangent space, and then solve
|
|
* the resulted linear system
|
|
*/
|
|
VectorConfig optimize(GaussianFactorGraph& fg) const {
|
|
return fg.optimize(*ordering_, useOldEliminate_);
|
|
}
|
|
|
|
/**
|
|
* linearize the non-linear graph around the current config
|
|
*/
|
|
boost::shared_ptr<GaussianFactorGraph> linearize(const NonlinearGraph& g, const Config& config) const {
|
|
return g.linearize(config);
|
|
}
|
|
};
|
|
|
|
#ifdef USE_SPQR
|
|
/**
|
|
* A linear system solver using factorization
|
|
*/
|
|
template <class NonlinearGraph, class Config>
|
|
class FactorizationSPQR {
|
|
private:
|
|
boost::shared_ptr<const Ordering> ordering_;
|
|
|
|
public:
|
|
FactorizationSPQR(boost::shared_ptr<const Ordering> ordering, bool old=true)
|
|
: ordering_(ordering) {
|
|
if (!ordering) throw std::invalid_argument("FactorizationSPQR constructor: ordering = NULL");
|
|
}
|
|
|
|
/**
|
|
* solve for the optimal displacement in the tangent space, and then solve
|
|
* the resulted linear system
|
|
*/
|
|
VectorConfig optimize(GaussianFactorGraph& fg) const {
|
|
return fg.optimizeSPQR(*ordering_);
|
|
}
|
|
|
|
/**
|
|
* linearize the non-linear graph around the current config
|
|
*/
|
|
boost::shared_ptr<GaussianFactorGraph> linearize(const NonlinearGraph& g, const Config& config) const {
|
|
return g.linearize(config);
|
|
}
|
|
};
|
|
#endif
|
|
}
|