11 #include <gtsam/inference/Symbol.h> 15 class GaussianBayesNet;
20 template<
size_t D,
size_t ZDim>
24 typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
32 const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks,
const Matrix& E,
const Matrix3& P,
34 const SharedDiagonal& model = SharedDiagonal()) :
39 for (
size_t k = 0; k < FBlocks.size(); ++k) {
41 gfg.
add(pointKey, E.block<ZDim, 3>(ZDim * k, 0), key, FBlocks[k],
42 b.segment < ZDim > (ZDim * k), model);
47 boost::shared_ptr<GaussianBayesNet> bn;
50 variables.push_back(pointKey);
This is the base class for all factor types.
Definition: Factor.h:54
JacobianFactor class with fixed sized blcoks.
Character and index key used to refer to variables.
Definition: Symbol.h:33
JacobianFactor()
default constructor for I/O
Definition: JacobianFactor.cpp:60
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:74
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
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
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:65
JacobianFactor for Schur complement that uses Q noise model.
Definition: JacobianFactorQR.h:21
void add(const GaussianFactor &factor)
Add a factor by value - makes a copy.
Definition: GaussianFactorGraph.h:102
friend GTSAM_EXPORT std::pair< boost::shared_ptr< GaussianConditional >, 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:721
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:113
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:56
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:118
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Linear Factor Graph where all factors are Gaussians.
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition: RegularJacobianFactor.h:32