gtsam 4.1.1 gtsam
JacobianFactorQR.h
1/*
2 * @file JacobianFactorQR.h
3 * @brief Jacobianfactor that combines and eliminates points
4 * @date Oct 27, 2013
5 * @uthor Frank Dellaert
6 */
7
8#pragma once
12
13namespace gtsam {
14
15class GaussianBayesNet;
16
20template<size_t D, size_t ZDim>
22
24 typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
25
26public:
27
32 const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
33 const Vector& b, //
34 const SharedDiagonal& model = SharedDiagonal()) :
35 Base() {
36 // Create a number of Jacobian factors in a factor graph
38 Symbol pointKey('p', 0);
39 for (size_t k = 0; k < FBlocks.size(); ++k) {
40 Key key = keys[k];
41 gfg.add(pointKey, E.block<ZDim, 3>(ZDim * k, 0), key, FBlocks[k],
42 b.segment < ZDim > (ZDim * k), model);
43 }
44 //gfg.print("gfg");
45
46 // eliminate the point
47 boost::shared_ptr<GaussianBayesNet> bn;
49 KeyVector variables;
50 variables.push_back(pointKey);
51 boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
52 //fg->print("fg");
53
54 JacobianFactor::operator=(JacobianFactor(*fg));
55 }
56};
57// end class JacobianFactorQR
58
59}// end namespace gtsam
Linear Factor Graph where all factors are Gaussians.
JacobianFactor class with fixed sized blcoks.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
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:140
This is the base class for all factor types.
Definition: Factor.h:56
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:125
Character and index key used to refer to variables.
Definition: Symbol.h:35
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:69
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:75
Add a factor by value - makes a copy.
Definition: GaussianFactorGraph.h:103
JacobianFactor()
default constructor for I/O
Definition: JacobianFactor.cpp:52
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:789
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition: RegularJacobianFactor.h:32
JacobianFactor for Schur complement that uses Q noise model.
Definition: JacobianFactorQR.h:21
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