gtsam  4.0.0
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
11 #include <gtsam/inference/Symbol.h>
12 
13 namespace gtsam {
14 
15 class GaussianBayesNet;
16 
20 template<size_t D, size_t ZDim>
22 
24  typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
25 
26 public:
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
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