157 lines
5.1 KiB
C++
157 lines
5.1 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 GaussianFactor.h
|
|
* @brief A factor with a quadratic error function - a Gaussian
|
|
* @brief GaussianFactor
|
|
* @author Richard Roberts, Christian Potthast
|
|
*/
|
|
|
|
// \callgraph
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/base/Matrix.h>
|
|
#include <gtsam/inference/IndexFactor.h>
|
|
|
|
#include <boost/lexical_cast.hpp>
|
|
|
|
#include <string>
|
|
#include <utility>
|
|
|
|
namespace gtsam {
|
|
|
|
// Forward declarations
|
|
class VectorValues;
|
|
class Permutation;
|
|
class GaussianConditional;
|
|
template<class C> class BayesNet;
|
|
|
|
/**
|
|
* An abstract virtual base class for JacobianFactor and HessianFactor.
|
|
* A GaussianFactor has a quadratic error function.
|
|
* GaussianFactor is non-mutable (all methods const!).
|
|
* The factor value is exp(-0.5*||Ax-b||^2)
|
|
*/
|
|
class GTSAM_EXPORT GaussianFactor: public IndexFactor {
|
|
|
|
protected:
|
|
|
|
/** Copy constructor */
|
|
GaussianFactor(const This& f) : IndexFactor(f) {}
|
|
|
|
/** Construct from derived type */
|
|
GaussianFactor(const GaussianConditional& c);
|
|
|
|
/** Constructor from a collection of keys */
|
|
template<class KeyIterator> GaussianFactor(KeyIterator beginKey, KeyIterator endKey) :
|
|
Base(beginKey, endKey) {}
|
|
|
|
/** Default constructor for I/O */
|
|
GaussianFactor() {}
|
|
|
|
/** Construct unary factor */
|
|
GaussianFactor(Index j) : IndexFactor(j) {}
|
|
|
|
/** Construct binary factor */
|
|
GaussianFactor(Index j1, Index j2) : IndexFactor(j1, j2) {}
|
|
|
|
/** Construct ternary factor */
|
|
GaussianFactor(Index j1, Index j2, Index j3) : IndexFactor(j1, j2, j3) {}
|
|
|
|
/** Construct 4-way factor */
|
|
GaussianFactor(Index j1, Index j2, Index j3, Index j4) : IndexFactor(j1, j2, j3, j4) {}
|
|
|
|
/** Construct n-way factor */
|
|
GaussianFactor(const std::set<Index>& js) : IndexFactor(js) {}
|
|
|
|
/** Construct n-way factor */
|
|
GaussianFactor(const std::vector<Index>& js) : IndexFactor(js) {}
|
|
|
|
public:
|
|
|
|
typedef GaussianConditional ConditionalType;
|
|
typedef boost::shared_ptr<GaussianFactor> shared_ptr;
|
|
|
|
// Implementing Testable interface
|
|
virtual void print(const std::string& s = "",
|
|
const IndexFormatter& formatter = DefaultIndexFormatter) const = 0;
|
|
|
|
virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0;
|
|
|
|
virtual double error(const VectorValues& c) const = 0; /** 0.5*(A*x-b)'*D*(A*x-b) */
|
|
|
|
/** Return the dimension of the variable pointed to by the given key iterator */
|
|
virtual size_t getDim(const_iterator variable) const = 0;
|
|
|
|
/** Return the augmented information matrix represented by this GaussianFactor.
|
|
* The augmented information matrix contains the information matrix with an
|
|
* additional column holding the information vector, and an additional row
|
|
* holding the transpose of the information vector. The lower-right entry
|
|
* contains the constant error term (when \f$ \delta x = 0 \f$). The
|
|
* augmented information matrix is described in more detail in HessianFactor,
|
|
* which in fact stores an augmented information matrix.
|
|
*/
|
|
virtual Matrix augmentedInformation() const = 0;
|
|
|
|
/** Return the non-augmented information matrix represented by this
|
|
* GaussianFactor.
|
|
*/
|
|
virtual Matrix information() const = 0;
|
|
|
|
/** Clone a factor (make a deep copy) */
|
|
virtual GaussianFactor::shared_ptr clone() const = 0;
|
|
|
|
/**
|
|
* 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.
|
|
*/
|
|
virtual void permuteWithInverse(const Permutation& inversePermutation) {
|
|
IndexFactor::permuteWithInverse(inversePermutation);
|
|
}
|
|
|
|
/**
|
|
* Apply a reduction, which is a remapping of variable indices.
|
|
*/
|
|
virtual void reduceWithInverse(const internal::Reduction& inverseReduction) {
|
|
IndexFactor::reduceWithInverse(inverseReduction);
|
|
}
|
|
|
|
/**
|
|
* Construct the corresponding anti-factor to negate information
|
|
* stored stored in this factor.
|
|
* @return a HessianFactor with negated Hessian matrices
|
|
*/
|
|
virtual GaussianFactor::shared_ptr negate() const = 0;
|
|
|
|
private:
|
|
/** Serialization function */
|
|
friend class boost::serialization::access;
|
|
template<class ARCHIVE>
|
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(IndexFactor);
|
|
}
|
|
|
|
}; // GaussianFactor
|
|
|
|
/** make keys from list, vector, or map of matrices */
|
|
template<typename ITERATOR>
|
|
static std::vector<Index> GetKeys(size_t n, ITERATOR begin, ITERATOR end) {
|
|
std::vector<Index> keys;
|
|
keys.reserve(n);
|
|
for (ITERATOR it=begin;it!=end;++it) keys.push_back(it->first);
|
|
return keys;
|
|
}
|
|
|
|
} // namespace gtsam
|