JacobianFactorQR.h
Go to the documentation of this file.
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 
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  KeyVector variables;
48  variables.push_back(pointKey);
49  const auto [bn, fg] = gfg.eliminatePartialSequential(variables, EliminateQR);
50  //fg->print("fg");
51 
52  JacobianFactor::operator=(JacobianFactor(*fg));
53  }
54 };
55 // end class JacobianFactorQR
56 
57 }// end namespace gtsam
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Factor
Definition: Factor.h:69
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
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())
Definition: JacobianFactorQR.h:31
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
Eigen::aligned_allocator
STL compatible allocator to use with types requiring a non standrad alignment.
Definition: Memory.h:878
FBlocks
std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > FBlocks
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:222
gtsam::JacobianFactor::EliminateQR
friend GTSAM_EXPORT std::pair< std::shared_ptr< GaussianConditional >, shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Definition: JacobianFactor.cpp:778
gtsam::EliminateableFactorGraph::eliminatePartialSequential
std::pair< std::shared_ptr< BayesNetType >, std::shared_ptr< FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex={}) const
Definition: EliminateableFactorGraph-inst.h:151
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
gtsam::SharedDiagonal
noiseModel::Diagonal::shared_ptr SharedDiagonal
Definition: NoiseModel.h:743
ZDim
static const int ZDim
Measurement dimension (Point2)
Definition: testSmartProjectionPoseFactorRollingShutter.cpp:218
Symbol.h
gtsam::JacobianFactorQR::Base
RegularJacobianFactor< D > Base
Definition: JacobianFactorQR.h:23
gtsam::JacobianFactorQR::MatrixZD
Eigen::Matrix< double, ZDim, D > MatrixZD
Definition: JacobianFactorQR.h:24
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
key
const gtsam::Symbol key('X', 0)
E
DiscreteKey E(5, 2)
gtsam::b
const G & b
Definition: Group.h:79
gtsam
traits
Definition: chartTesting.h:28
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:142
gtsam::RegularJacobianFactor
Definition: RegularJacobianFactor.h:32
P
static double P[]
Definition: ellpe.c:68
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::JacobianFactor::JacobianFactor
JacobianFactor()
Definition: JacobianFactor.cpp:47
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::JacobianFactorQR
Definition: JacobianFactorQR.h:21
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Definition: GaussianFactorGraph.h:125
gtsam::Symbol
Definition: inference/Symbol.h:37


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:02:47