Loading...
Searching...
No Matches
JacobianFactorQR.h
/*
* @file JacobianFactorQR.h
* @brief Jacobianfactor that combines and eliminates points
* @date Oct 27, 2013
* @uthor Frank Dellaert
*/
#pragma once
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/RegularJacobianFactor.h>
#include <gtsam/inference/Symbol.h>
namespace gtsam {
class GaussianBayesNet;
template<size_t D, size_t ZDim>
class JacobianFactorQR: public RegularJacobianFactor<D> {
typedef RegularJacobianFactor<D> Base;
typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
public:
JacobianFactorQR(const KeyVector& keys,
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
const Vector& b, //
const SharedDiagonal& model = SharedDiagonal()) :
Base() {
// Create a number of Jacobian factors in a factor graph
GaussianFactorGraph gfg;
Symbol pointKey('p', 0);
for (size_t k = 0; k < FBlocks.size(); ++k) {
Key key = keys[k];
gfg.add(pointKey, E.block<ZDim, 3>(ZDim * k, 0), key, FBlocks[k],
b.segment < ZDim > (ZDim * k), model);
}
//gfg.print("gfg");
// eliminate the point
boost::shared_ptr<GaussianBayesNet> bn;
GaussianFactorGraph::shared_ptr fg;
KeyVector variables;
variables.push_back(pointKey);
boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
//fg->print("fg");
JacobianFactor::operator=(JacobianFactor(*fg));
}
};
// end class JacobianFactorQR
}// end namespace gtsam
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition JacobianFactor.cpp:789
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::EliminateableFactorGraph::eliminatePartialSequential
std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do sequential elimination of some variables, in ordering provided, to produce a Bayes net and a remai...
Definition EliminateableFactorGraph-inst.h:154
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
gtsam::Symbol
Character and index key used to refer to variables.
Definition Symbol.h:35
gtsam::GaussianBayesNet
GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
Definition GaussianBayesNet.h:36
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Add a factor by value - makes a copy.
Definition GaussianFactorGraph.h:127
gtsam::JacobianFactor::JacobianFactor
JacobianFactor()
default constructor for I/O
Definition JacobianFactor.cpp:54
gtsam::RegularJacobianFactor::RegularJacobianFactor
RegularJacobianFactor()
Default constructor.
Definition RegularJacobianFactor.h:44
gtsam::JacobianFactorQR::JacobianFactorQR
JacobianFactorQR(const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &FBlocks, const Matrix &E, const Matrix3 &P, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Constructor.
Definition JacobianFactorQR.h:31
Symbol.h