Fixed indentation
Richard Roberts
parent
1e54c8c2c0
commit
a9370dead4
|
|
@ -47,194 +47,194 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
class GaussianFactorGraph;
|
||||
class GaussianFactorGraph;
|
||||
|
||||
/** A map from key to dimension, useful in various contexts */
|
||||
typedef std::map<Index, size_t> Dimensions;
|
||||
|
||||
/**
|
||||
* Base Class for a linear factor.
|
||||
* GaussianFactor is non-mutable (all methods const!).
|
||||
* The factor value is exp(-0.5*||Ax-b||^2)
|
||||
*/
|
||||
class GaussianFactor: public IndexFactor {
|
||||
|
||||
protected:
|
||||
|
||||
typedef boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> AbMatrix;
|
||||
typedef VerticalBlockView<AbMatrix> BlockAb;
|
||||
|
||||
public:
|
||||
typedef GaussianConditional Conditional;
|
||||
typedef boost::shared_ptr<GaussianFactor> shared_ptr;
|
||||
typedef BlockAb::Block ABlock;
|
||||
typedef BlockAb::constBlock constABlock;
|
||||
typedef BlockAb::Column BVector;
|
||||
typedef BlockAb::constColumn constBVector;
|
||||
|
||||
enum SolveMethod { SOLVE_QR, SOLVE_CHOLESKY };
|
||||
|
||||
protected:
|
||||
SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
|
||||
std::vector<size_t> firstNonzeroBlocks_;
|
||||
AbMatrix matrix_; // the full matrix correponding to the factor
|
||||
BlockAb Ab_; // the block view of the full matrix
|
||||
|
||||
public:
|
||||
|
||||
/** Copy constructor */
|
||||
GaussianFactor(const GaussianFactor& gf);
|
||||
|
||||
/** default constructor for I/O */
|
||||
GaussianFactor();
|
||||
|
||||
/** Construct Null factor */
|
||||
GaussianFactor(const Vector& b_in);
|
||||
|
||||
/** Construct unary factor */
|
||||
GaussianFactor(Index i1, const Matrix& A1,
|
||||
const Vector& b, const SharedDiagonal& model);
|
||||
|
||||
/** Construct binary factor */
|
||||
GaussianFactor(Index i1, const Matrix& A1,
|
||||
Index i2, const Matrix& A2,
|
||||
const Vector& b, const SharedDiagonal& model);
|
||||
|
||||
/** Construct ternary factor */
|
||||
GaussianFactor(Index i1, const Matrix& A1, Index i2,
|
||||
const Matrix& A2, Index i3, const Matrix& A3,
|
||||
const Vector& b, const SharedDiagonal& model);
|
||||
|
||||
/** Construct an n-ary factor */
|
||||
GaussianFactor(const std::vector<std::pair<Index, Matrix> > &terms,
|
||||
const Vector &b, const SharedDiagonal& model);
|
||||
|
||||
GaussianFactor(const std::list<std::pair<Index, Matrix> > &terms,
|
||||
const Vector &b, const SharedDiagonal& model);
|
||||
|
||||
/** Construct from Conditional Gaussian */
|
||||
GaussianFactor(const GaussianConditional& cg);
|
||||
|
||||
|
||||
// Implementing Testable interface
|
||||
void print(const std::string& s = "") const;
|
||||
bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
|
||||
|
||||
Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */
|
||||
Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */
|
||||
double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */
|
||||
|
||||
/** Check if the factor contains no information, i.e. zero rows. This does
|
||||
* not necessarily mean that the factor involves no variables (to check for
|
||||
* involving no variables use keys().empty()).
|
||||
*/
|
||||
bool empty() const { return Ab_.size1() == 0;}
|
||||
|
||||
/**
|
||||
* return the number of rows in the corresponding linear system
|
||||
*/
|
||||
size_t size1() const { return Ab_.size1(); }
|
||||
|
||||
/**
|
||||
* return the number of columns in the corresponding linear system
|
||||
*/
|
||||
size_t size2() const { return Ab_.size2(); }
|
||||
|
||||
|
||||
/** Get a view of the r.h.s. vector b */
|
||||
constBVector getb() const { return Ab_.column(size(), 0); }
|
||||
|
||||
/** Get a view of the A matrix for the variable pointed to be the given key iterator */
|
||||
constABlock getA(const_iterator variable) const { return Ab_(variable - keys_.begin()); }
|
||||
|
||||
BVector getb() { return Ab_.column(size(), 0); }
|
||||
|
||||
ABlock getA(iterator variable) { return Ab_(variable - keys_.begin()); }
|
||||
|
||||
/** Return the dimension of the variable pointed to by the given key iterator
|
||||
* todo: Remove this in favor of keeping track of dimensions with variables?
|
||||
*/
|
||||
size_t getDim(const_iterator variable) const { return Ab_(variable - keys_.begin()).size2(); }
|
||||
/** A map from key to dimension, useful in various contexts */
|
||||
typedef std::map<Index, size_t> Dimensions;
|
||||
|
||||
/**
|
||||
* Permutes the GaussianFactor, but for efficiency requires the permutation
|
||||
* to already be inverted. This acts just as a change-of-name for each
|
||||
* variable. The order of the variables within the factor is not changed.
|
||||
* Base Class for a linear factor.
|
||||
* GaussianFactor is non-mutable (all methods const!).
|
||||
* The factor value is exp(-0.5*||Ax-b||^2)
|
||||
*/
|
||||
void permuteWithInverse(const Permutation& inversePermutation);
|
||||
class GaussianFactor: public IndexFactor {
|
||||
|
||||
/**
|
||||
* Named constructor for combining a set of factors with pre-computed set of variables.
|
||||
*/
|
||||
static shared_ptr Combine(const FactorGraph<GaussianFactor>& factors, const VariableSlots& variableSlots);
|
||||
protected:
|
||||
|
||||
protected:
|
||||
typedef boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> AbMatrix;
|
||||
typedef VerticalBlockView<AbMatrix> BlockAb;
|
||||
|
||||
/** Internal debug check to make sure variables are sorted */
|
||||
void assertInvariants() const;
|
||||
public:
|
||||
typedef GaussianConditional Conditional;
|
||||
typedef boost::shared_ptr<GaussianFactor> shared_ptr;
|
||||
typedef BlockAb::Block ABlock;
|
||||
typedef BlockAb::constBlock constABlock;
|
||||
typedef BlockAb::Column BVector;
|
||||
typedef BlockAb::constColumn constBVector;
|
||||
|
||||
public:
|
||||
enum SolveMethod { SOLVE_QR, SOLVE_CHOLESKY };
|
||||
|
||||
/** get a copy of sigmas */
|
||||
const Vector& get_sigmas() const { return model_->sigmas(); }
|
||||
protected:
|
||||
SharedDiagonal model_; // Gaussian noise model with diagonal covariance matrix
|
||||
std::vector<size_t> firstNonzeroBlocks_;
|
||||
AbMatrix matrix_; // the full matrix correponding to the factor
|
||||
BlockAb Ab_; // the block view of the full matrix
|
||||
|
||||
/** get a copy of model */
|
||||
const SharedDiagonal& get_model() const { return model_; }
|
||||
public:
|
||||
|
||||
/** get the indices list */
|
||||
const std::vector<size_t>& get_firstNonzeroBlocks() const { return firstNonzeroBlocks_; }
|
||||
/** Copy constructor */
|
||||
GaussianFactor(const GaussianFactor& gf);
|
||||
|
||||
/** whether the noise model of this factor is constrained (i.e. contains any sigmas of 0.0) */
|
||||
bool isConstrained() const {return model_->isConstrained();}
|
||||
/** default constructor for I/O */
|
||||
GaussianFactor();
|
||||
|
||||
/**
|
||||
* return the number of rows from the b vector
|
||||
* @return a integer with the number of rows from the b vector
|
||||
*/
|
||||
size_t numberOfRows() const { return Ab_.size1(); }
|
||||
/** Construct Null factor */
|
||||
GaussianFactor(const Vector& b_in);
|
||||
|
||||
/** Return A*x */
|
||||
Vector operator*(const VectorValues& x) const;
|
||||
/** Construct unary factor */
|
||||
GaussianFactor(Index i1, const Matrix& A1,
|
||||
const Vector& b, const SharedDiagonal& model);
|
||||
|
||||
/** Construct binary factor */
|
||||
GaussianFactor(Index i1, const Matrix& A1,
|
||||
Index i2, const Matrix& A2,
|
||||
const Vector& b, const SharedDiagonal& model);
|
||||
|
||||
/** Construct ternary factor */
|
||||
GaussianFactor(Index i1, const Matrix& A1, Index i2,
|
||||
const Matrix& A2, Index i3, const Matrix& A3,
|
||||
const Vector& b, const SharedDiagonal& model);
|
||||
|
||||
/** Construct an n-ary factor */
|
||||
GaussianFactor(const std::vector<std::pair<Index, Matrix> > &terms,
|
||||
const Vector &b, const SharedDiagonal& model);
|
||||
|
||||
GaussianFactor(const std::list<std::pair<Index, Matrix> > &terms,
|
||||
const Vector &b, const SharedDiagonal& model);
|
||||
|
||||
/** Construct from Conditional Gaussian */
|
||||
GaussianFactor(const GaussianConditional& cg);
|
||||
|
||||
|
||||
/** x += A'*e */
|
||||
void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const;
|
||||
// Implementing Testable interface
|
||||
void print(const std::string& s = "") const;
|
||||
bool equals(const GaussianFactor& lf, double tol = 1e-9) const;
|
||||
|
||||
/**
|
||||
* Return (dense) matrix associated with factor
|
||||
* @param ordering of variables needed for matrix column order
|
||||
* @param set weight to true to bake in the weights
|
||||
*/
|
||||
std::pair<Matrix, Vector> matrix(bool weight = true) const;
|
||||
Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */
|
||||
Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */
|
||||
double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */
|
||||
|
||||
/**
|
||||
* Return (dense) matrix associated with factor
|
||||
* The returned system is an augmented matrix: [A b]
|
||||
* @param ordering of variables needed for matrix column order
|
||||
* @param set weight to use whitening to bake in weights
|
||||
*/
|
||||
Matrix matrix_augmented(bool weight = true) const;
|
||||
/** Check if the factor contains no information, i.e. zero rows. This does
|
||||
* not necessarily mean that the factor involves no variables (to check for
|
||||
* involving no variables use keys().empty()).
|
||||
*/
|
||||
bool empty() const { return Ab_.size1() == 0;}
|
||||
|
||||
/**
|
||||
* Return vectors i, j, and s to generate an m-by-n sparse matrix
|
||||
* such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
|
||||
* As above, the standard deviations are baked into A and b
|
||||
* @param first column index for each variable
|
||||
*/
|
||||
boost::tuple<std::list<int>, std::list<int>, std::list<double> >
|
||||
sparse(const Dimensions& columnIndices) const;
|
||||
/**
|
||||
* return the number of rows in the corresponding linear system
|
||||
*/
|
||||
size_t size1() const { return Ab_.size1(); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
// MUTABLE functions. FD:on the path to being eradicated
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* return the number of columns in the corresponding linear system
|
||||
*/
|
||||
size_t size2() const { return Ab_.size2(); }
|
||||
|
||||
GaussianConditional::shared_ptr eliminateFirst(SolveMethod solveMethod = SOLVE_QR);
|
||||
|
||||
GaussianBayesNet::shared_ptr eliminate(size_t nrFrontals = 1, SolveMethod solveMethod = SOLVE_QR);
|
||||
/** Get a view of the r.h.s. vector b */
|
||||
constBVector getb() const { return Ab_.column(size(), 0); }
|
||||
|
||||
void set_firstNonzeroBlocks(size_t row, size_t varpos) { firstNonzeroBlocks_[row] = varpos; }
|
||||
/** Get a view of the A matrix for the variable pointed to be the given key iterator */
|
||||
constABlock getA(const_iterator variable) const { return Ab_(variable - keys_.begin()); }
|
||||
|
||||
}; // GaussianFactor
|
||||
BVector getb() { return Ab_.column(size(), 0); }
|
||||
|
||||
ABlock getA(iterator variable) { return Ab_(variable - keys_.begin()); }
|
||||
|
||||
/** Return the dimension of the variable pointed to by the given key iterator
|
||||
* todo: Remove this in favor of keeping track of dimensions with variables?
|
||||
*/
|
||||
size_t getDim(const_iterator variable) const { return Ab_(variable - keys_.begin()).size2(); }
|
||||
|
||||
/**
|
||||
* Permutes the GaussianFactor, but for efficiency requires the permutation
|
||||
* to already be inverted. This acts just as a change-of-name for each
|
||||
* variable. The order of the variables within the factor is not changed.
|
||||
*/
|
||||
void permuteWithInverse(const Permutation& inversePermutation);
|
||||
|
||||
/**
|
||||
* Named constructor for combining a set of factors with pre-computed set of variables.
|
||||
*/
|
||||
static shared_ptr Combine(const FactorGraph<GaussianFactor>& factors, const VariableSlots& variableSlots);
|
||||
|
||||
protected:
|
||||
|
||||
/** Internal debug check to make sure variables are sorted */
|
||||
void assertInvariants() const;
|
||||
|
||||
public:
|
||||
|
||||
/** get a copy of sigmas */
|
||||
const Vector& get_sigmas() const { return model_->sigmas(); }
|
||||
|
||||
/** get a copy of model */
|
||||
const SharedDiagonal& get_model() const { return model_; }
|
||||
|
||||
/** get the indices list */
|
||||
const std::vector<size_t>& get_firstNonzeroBlocks() const { return firstNonzeroBlocks_; }
|
||||
|
||||
/** whether the noise model of this factor is constrained (i.e. contains any sigmas of 0.0) */
|
||||
bool isConstrained() const {return model_->isConstrained();}
|
||||
|
||||
/**
|
||||
* return the number of rows from the b vector
|
||||
* @return a integer with the number of rows from the b vector
|
||||
*/
|
||||
size_t numberOfRows() const { return Ab_.size1(); }
|
||||
|
||||
/** Return A*x */
|
||||
Vector operator*(const VectorValues& x) const;
|
||||
|
||||
|
||||
/** x += A'*e */
|
||||
void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const;
|
||||
|
||||
/**
|
||||
* Return (dense) matrix associated with factor
|
||||
* @param ordering of variables needed for matrix column order
|
||||
* @param set weight to true to bake in the weights
|
||||
*/
|
||||
std::pair<Matrix, Vector> matrix(bool weight = true) const;
|
||||
|
||||
/**
|
||||
* Return (dense) matrix associated with factor
|
||||
* The returned system is an augmented matrix: [A b]
|
||||
* @param ordering of variables needed for matrix column order
|
||||
* @param set weight to use whitening to bake in weights
|
||||
*/
|
||||
Matrix matrix_augmented(bool weight = true) const;
|
||||
|
||||
/**
|
||||
* Return vectors i, j, and s to generate an m-by-n sparse matrix
|
||||
* such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
|
||||
* As above, the standard deviations are baked into A and b
|
||||
* @param first column index for each variable
|
||||
*/
|
||||
boost::tuple<std::list<int>, std::list<int>, std::list<double> >
|
||||
sparse(const Dimensions& columnIndices) const;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// MUTABLE functions. FD:on the path to being eradicated
|
||||
/* ************************************************************************* */
|
||||
|
||||
GaussianConditional::shared_ptr eliminateFirst(SolveMethod solveMethod = SOLVE_QR);
|
||||
|
||||
GaussianBayesNet::shared_ptr eliminate(size_t nrFrontals = 1, SolveMethod solveMethod = SOLVE_QR);
|
||||
|
||||
void set_firstNonzeroBlocks(size_t row, size_t varpos) { firstNonzeroBlocks_[row] = varpos; }
|
||||
|
||||
}; // GaussianFactor
|
||||
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -38,168 +38,168 @@ using namespace gtsam;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
// Explicitly instantiate so we don't have to include everywhere
|
||||
INSTANTIATE_FACTOR_GRAPH(GaussianFactor);
|
||||
// Explicitly instantiate so we don't have to include everywhere
|
||||
INSTANTIATE_FACTOR_GRAPH(GaussianFactor);
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) :
|
||||
FactorGraph<GaussianFactor> (CBN) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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()); }
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianFactorGraph::permuteWithInverse(const Permutation& inversePermutation) {
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_) {
|
||||
factor->permuteWithInverse(inversePermutation);
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) :
|
||||
FactorGraph<GaussianFactor> (CBN) {
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double GaussianFactorGraph::error(const VectorValues& x) const {
|
||||
double total_error = 0.;
|
||||
BOOST_FOREACH(sharedFactor factor,factors_)
|
||||
total_error += factor->error(x);
|
||||
return total_error;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
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()); }
|
||||
return keys;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Errors GaussianFactorGraph::errors(const VectorValues& x) const {
|
||||
return *errors_(x);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void GaussianFactorGraph::permuteWithInverse(const Permutation& inversePermutation) {
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_) {
|
||||
factor->permuteWithInverse(inversePermutation);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<Errors> GaussianFactorGraph::errors_(const VectorValues& x) const {
|
||||
boost::shared_ptr<Errors> e(new Errors);
|
||||
BOOST_FOREACH(const sharedFactor& factor,factors_)
|
||||
e->push_back(factor->error_vector(x));
|
||||
return e;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
double GaussianFactorGraph::error(const VectorValues& x) const {
|
||||
double total_error = 0.;
|
||||
BOOST_FOREACH(sharedFactor factor,factors_)
|
||||
total_error += factor->error(x);
|
||||
return total_error;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Errors GaussianFactorGraph::operator*(const VectorValues& x) const {
|
||||
Errors e;
|
||||
BOOST_FOREACH(const sharedFactor& Ai,factors_)
|
||||
e.push_back((*Ai)*x);
|
||||
return e;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Errors GaussianFactorGraph::errors(const VectorValues& x) const {
|
||||
return *errors_(x);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const {
|
||||
multiplyInPlace(x,e.begin());
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
boost::shared_ptr<Errors> GaussianFactorGraph::errors_(const VectorValues& x) const {
|
||||
boost::shared_ptr<Errors> e(new Errors);
|
||||
BOOST_FOREACH(const sharedFactor& factor,factors_)
|
||||
e->push_back(factor->error_vector(x));
|
||||
return e;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianFactorGraph::multiplyInPlace(const VectorValues& x,
|
||||
const Errors::iterator& e) const {
|
||||
Errors::iterator ei = e;
|
||||
BOOST_FOREACH(const sharedFactor& Ai,factors_) {
|
||||
*ei = (*Ai)*x;
|
||||
ei++;
|
||||
}
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Errors GaussianFactorGraph::operator*(const VectorValues& x) const {
|
||||
Errors e;
|
||||
BOOST_FOREACH(const sharedFactor& Ai,factors_)
|
||||
e.push_back((*Ai)*x);
|
||||
return e;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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;
|
||||
BOOST_FOREACH(const sharedFactor& Ai,factors_) {
|
||||
*ei = (*Ai)*x;
|
||||
ei++;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
// 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();
|
||||
BOOST_FOREACH(const sharedFactor& Ai,factors_)
|
||||
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
// 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();
|
||||
BOOST_FOREACH(const sharedFactor& Ai,factors_)
|
||||
Ai->transposeMultiplyAdd(alpha,*(ei++),x);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
VectorValues GaussianFactorGraph::gradient(const VectorValues& x) const {
|
||||
// It is crucial for performance to make a zero-valued clone of x
|
||||
VectorValues g = VectorValues::zero(x);
|
||||
transposeMultiplyAdd(1.0, errors(x), g);
|
||||
return g;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
VectorValues GaussianFactorGraph::gradient(const VectorValues& x) const {
|
||||
// It is crucial for performance to make a zero-valued clone of x
|
||||
VectorValues g = VectorValues::zero(x);
|
||||
transposeMultiplyAdd(1.0, errors(x), g);
|
||||
return g;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg){
|
||||
for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){
|
||||
push_back(*factor);
|
||||
}
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg){
|
||||
for(const_iterator factor=lfg.factors_.begin(); factor!=lfg.factors_.end(); factor++){
|
||||
push_back(*factor);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg1,
|
||||
const GaussianFactorGraph& lfg2) {
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph GaussianFactorGraph::combine2(const GaussianFactorGraph& lfg1,
|
||||
const GaussianFactorGraph& lfg2) {
|
||||
|
||||
// create new linear factor graph equal to the first one
|
||||
GaussianFactorGraph fg = lfg1;
|
||||
// create new linear factor graph equal to the first one
|
||||
GaussianFactorGraph fg = lfg1;
|
||||
|
||||
// add the second factors_ in the graph
|
||||
for (const_iterator factor = lfg2.factors_.begin(); factor
|
||||
!= lfg2.factors_.end(); factor++) {
|
||||
fg.push_back(*factor);
|
||||
}
|
||||
return fg;
|
||||
}
|
||||
// add the second factors_ in the graph
|
||||
for (const_iterator factor = lfg2.factors_.begin(); factor
|
||||
!= lfg2.factors_.end(); factor++) {
|
||||
fg.push_back(*factor);
|
||||
}
|
||||
return fg;
|
||||
}
|
||||
|
||||
void GaussianFactorGraph::residual(const VectorValues &x, VectorValues &r) const {
|
||||
void GaussianFactorGraph::residual(const VectorValues &x, VectorValues &r) const {
|
||||
|
||||
getb(r) ;
|
||||
VectorValues Ax = VectorValues::SameStructure(r) ;
|
||||
multiply(x,Ax) ;
|
||||
axpy(-1.0,Ax,r) ;
|
||||
}
|
||||
getb(r) ;
|
||||
VectorValues Ax = VectorValues::SameStructure(r) ;
|
||||
multiply(x,Ax) ;
|
||||
axpy(-1.0,Ax,r) ;
|
||||
}
|
||||
|
||||
void GaussianFactorGraph::multiply(const VectorValues &x, VectorValues &r) const {
|
||||
void GaussianFactorGraph::multiply(const VectorValues &x, VectorValues &r) const {
|
||||
|
||||
r.makeZero() ;
|
||||
Index i = 0 ;
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_) {
|
||||
for(GaussianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
|
||||
r[i] += prod(factor->getA(j), x[*j]);
|
||||
}
|
||||
++i ;
|
||||
}
|
||||
}
|
||||
r.makeZero() ;
|
||||
Index i = 0 ;
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_) {
|
||||
for(GaussianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
|
||||
r[i] += prod(factor->getA(j), x[*j]);
|
||||
}
|
||||
++i ;
|
||||
}
|
||||
}
|
||||
|
||||
void GaussianFactorGraph::transposeMultiply(const VectorValues &r, VectorValues &x) const {
|
||||
x.makeZero() ;
|
||||
Index i = 0 ;
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_) {
|
||||
for(GaussianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
|
||||
x[*j] += prod(trans(factor->getA(j)), r[i]) ;
|
||||
}
|
||||
++i ;
|
||||
}
|
||||
}
|
||||
void GaussianFactorGraph::transposeMultiply(const VectorValues &r, VectorValues &x) const {
|
||||
x.makeZero() ;
|
||||
Index i = 0 ;
|
||||
BOOST_FOREACH(const sharedFactor& factor, factors_) {
|
||||
for(GaussianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
|
||||
x[*j] += prod(trans(factor->getA(j)), r[i]) ;
|
||||
}
|
||||
++i ;
|
||||
}
|
||||
}
|
||||
|
||||
VectorValues GaussianFactorGraph::allocateVectorValuesb() const {
|
||||
VectorValues GaussianFactorGraph::allocateVectorValuesb() const {
|
||||
std::vector<size_t> dimensions(size()) ;
|
||||
Index i = 0 ;
|
||||
BOOST_FOREACH( const sharedFactor& factor, factors_) {
|
||||
dimensions[i] = factor->numberOfRows() ;
|
||||
i++;
|
||||
dimensions[i] = factor->numberOfRows() ;
|
||||
i++;
|
||||
}
|
||||
|
||||
return VectorValues(dimensions) ;
|
||||
}
|
||||
}
|
||||
|
||||
void GaussianFactorGraph::getb(VectorValues &b) const {
|
||||
Index i = 0 ;
|
||||
BOOST_FOREACH( const sharedFactor& factor, factors_) {
|
||||
b[i] = factor->getb() ;
|
||||
i++;
|
||||
}
|
||||
}
|
||||
void GaussianFactorGraph::getb(VectorValues &b) const {
|
||||
Index i = 0 ;
|
||||
BOOST_FOREACH( const sharedFactor& factor, factors_) {
|
||||
b[i] = factor->getb() ;
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
VectorValues GaussianFactorGraph::getb() const {
|
||||
VectorValues b = allocateVectorValuesb() ;
|
||||
getb(b) ;
|
||||
return b ;
|
||||
}
|
||||
VectorValues GaussianFactorGraph::getb() const {
|
||||
VectorValues b = allocateVectorValuesb() ;
|
||||
getb(b) ;
|
||||
return b ;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@
|
|||
* @author Christian Potthast
|
||||
* @author Alireza Fathi
|
||||
*/
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
|
@ -55,43 +55,43 @@ namespace gtsam {
|
|||
push_back(fg);
|
||||
}
|
||||
|
||||
/* dummy constructor, to be compatible with conjugate gradient solver */
|
||||
/* dummy constructor, to be compatible with conjugate gradient solver */
|
||||
template<class DERIVEDFACTOR>
|
||||
GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& fg, const VectorValues &x0) {
|
||||
push_back(fg);
|
||||
}
|
||||
|
||||
/** Add a null factor */
|
||||
/** Add a null factor */
|
||||
void add(const Vector& b) {
|
||||
push_back(sharedFactor(new GaussianFactor(b)));
|
||||
}
|
||||
push_back(sharedFactor(new GaussianFactor(b)));
|
||||
}
|
||||
|
||||
/** Add a unary factor */
|
||||
/** Add a unary factor */
|
||||
void add(Index key1, const Matrix& A1,
|
||||
const Vector& b, const SharedDiagonal& model) {
|
||||
push_back(sharedFactor(new GaussianFactor(key1,A1,b,model)));
|
||||
}
|
||||
const Vector& b, const SharedDiagonal& model) {
|
||||
push_back(sharedFactor(new GaussianFactor(key1,A1,b,model)));
|
||||
}
|
||||
|
||||
/** Add a binary factor */
|
||||
/** Add a binary factor */
|
||||
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)));
|
||||
}
|
||||
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 */
|
||||
/** Add a ternary factor */
|
||||
void add(Index key1, const Matrix& A1,
|
||||
Index key2, const Matrix& A2,
|
||||
Index key3, const Matrix& A3,
|
||||
const Vector& b, const SharedDiagonal& model) {
|
||||
push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,key3,A3,b,model)));
|
||||
}
|
||||
Index key2, const Matrix& A2,
|
||||
Index key3, const Matrix& A3,
|
||||
const Vector& b, const SharedDiagonal& model) {
|
||||
push_back(sharedFactor(new GaussianFactor(key1,A1,key2,A2,key3,A3,b,model)));
|
||||
}
|
||||
|
||||
/** Add an n-ary factor */
|
||||
/** Add an n-ary factor */
|
||||
void add(const std::vector<std::pair<Index, Matrix> > &terms,
|
||||
const Vector &b, const SharedDiagonal& model) {
|
||||
push_back(sharedFactor(new GaussianFactor(terms,b,model)));
|
||||
}
|
||||
const Vector &b, const SharedDiagonal& model) {
|
||||
push_back(sharedFactor(new GaussianFactor(terms,b,model)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the set of variables involved in the factors (computes a set
|
||||
|
|
@ -103,38 +103,38 @@ namespace gtsam {
|
|||
/** Permute the variables in the factors */
|
||||
void permuteWithInverse(const Permutation& inversePermutation);
|
||||
|
||||
/** return A*x-b */
|
||||
Errors errors(const VectorValues& x) const;
|
||||
/** return A*x-b */
|
||||
Errors errors(const VectorValues& x) const;
|
||||
|
||||
/** shared pointer version */
|
||||
boost::shared_ptr<Errors> errors_(const VectorValues& x) const;
|
||||
/** shared pointer version */
|
||||
boost::shared_ptr<Errors> errors_(const VectorValues& x) const;
|
||||
|
||||
/** unnormalized error */
|
||||
double error(const VectorValues& x) const;
|
||||
/** unnormalized error */
|
||||
double error(const VectorValues& x) const;
|
||||
|
||||
/** return A*x */
|
||||
Errors operator*(const VectorValues& x) const;
|
||||
/** return A*x */
|
||||
Errors operator*(const VectorValues& x) const;
|
||||
|
||||
/* In-place version e <- A*x that overwrites e. */
|
||||
void multiplyInPlace(const VectorValues& x, Errors& e) const;
|
||||
/* In-place version e <- A*x that overwrites e. */
|
||||
void multiplyInPlace(const VectorValues& x, Errors& e) const;
|
||||
|
||||
/* In-place version e <- A*x that takes an iterator. */
|
||||
void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
|
||||
/* In-place version e <- A*x that takes an iterator. */
|
||||
void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
|
||||
|
||||
/** x += alpha*A'*e */
|
||||
void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const;
|
||||
/** x += alpha*A'*e */
|
||||
void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const;
|
||||
|
||||
/**
|
||||
* Calculate Gradient of A^(A*x-b) for a given config
|
||||
* @param x: VectorValues specifying where to calculate gradient
|
||||
* @return gradient, as a VectorValues as well
|
||||
*/
|
||||
VectorValues gradient(const VectorValues& x) const;
|
||||
/**
|
||||
* Calculate Gradient of A^(A*x-b) for a given config
|
||||
* @param x: VectorValues specifying where to calculate gradient
|
||||
* @return gradient, as a VectorValues as well
|
||||
*/
|
||||
VectorValues gradient(const VectorValues& x) const;
|
||||
|
||||
/** Unnormalized probability. O(n) */
|
||||
double probPrime(const VectorValues& c) const {
|
||||
return exp(-0.5 * error(c));
|
||||
}
|
||||
/** Unnormalized probability. O(n) */
|
||||
double probPrime(const VectorValues& c) const {
|
||||
return exp(-0.5 * error(c));
|
||||
}
|
||||
|
||||
/**
|
||||
* static function that combines two factor graphs
|
||||
|
|
@ -143,8 +143,8 @@ namespace gtsam {
|
|||
* @return a new combined factor graph
|
||||
*/
|
||||
static GaussianFactorGraph combine2(const GaussianFactorGraph& lfg1,
|
||||
const GaussianFactorGraph& lfg2);
|
||||
|
||||
const GaussianFactorGraph& lfg2);
|
||||
|
||||
/**
|
||||
* combine two factor graphs
|
||||
* @param *lfg Linear factor graph
|
||||
|
|
@ -152,13 +152,13 @@ namespace gtsam {
|
|||
void combine(const GaussianFactorGraph &lfg);
|
||||
|
||||
// matrix-vector operations
|
||||
void residual(const VectorValues &x, VectorValues &r) const ;
|
||||
void multiply(const VectorValues &x, VectorValues &r) const ;
|
||||
void transposeMultiply(const VectorValues &r, VectorValues &x) const ;
|
||||
void residual(const VectorValues &x, VectorValues &r) const ;
|
||||
void multiply(const VectorValues &x, VectorValues &r) const ;
|
||||
void transposeMultiply(const VectorValues &r, VectorValues &x) const ;
|
||||
|
||||
// get b
|
||||
void getb(VectorValues &b) const ;
|
||||
VectorValues getb() const ;
|
||||
// get b
|
||||
void getb(VectorValues &b) const ;
|
||||
VectorValues getb() const ;
|
||||
|
||||
// allocate a vectorvalues of b's structure
|
||||
VectorValues allocateVectorValuesb() const ;
|
||||
|
|
|
|||
Loading…
Reference in New Issue