DummyFactor.cpp
Go to the documentation of this file.
1 
9 
10 namespace gtsam {
11 
12 /* ************************************************************************* */
13 DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2)
15 {
16  dims_.push_back(dim1);
17  dims_.push_back(dim2);
18  if (dim1 > dim2)
19  rowDim_ = dim1;
20  else
21  rowDim_ = dim2;
22 }
23 
24 /* ************************************************************************* */
25 void DummyFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
26  std::cout << s << " DummyFactor dim = " << rowDim_ << ", keys = { ";
27  for(Key key: this->keys()) { std::cout << keyFormatter(key) << " "; }
28  std::cout << "}" << std::endl;
29 }
30 
31 /* ************************************************************************* */
32 bool DummyFactor::equals(const NonlinearFactor& f, double tol) const {
33  const DummyFactor* e = dynamic_cast<const DummyFactor*>(&f);
34  return e && Base::equals(f, tol) && dims_ == e->dims_ && rowDim_ == e->rowDim_;
35 }
36 
37 /* ************************************************************************* */
38 std::shared_ptr<GaussianFactor>
40  // Only linearize if the factor is active
41  if (!this->active(c))
42  return std::shared_ptr<JacobianFactor>();
43 
44  // Fill in terms with zero matrices
45  std::vector<std::pair<Key, Matrix> > terms(this->size());
46  for(size_t j=0; j<this->size(); ++j) {
47  terms[j].first = keys()[j];
48  terms[j].second = Matrix::Zero(rowDim_, dims_[j]);
49  }
50 
53  new JacobianFactor(terms, Vector::Zero(rowDim_), model));
54 }
55 
56 /* ************************************************************************* */
57 
58 } // \namespace gtsam
59 
60 
61 
62 
key1
const Symbol key1('v', 1)
gtsam::DummyFactor::DummyFactor
DummyFactor()
Definition: gtsam_unstable/slam/DummyFactor.h:27
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
c
Scalar Scalar * c
Definition: benchVecAdd.cpp:17
gtsam::DummyFactor::dims_
std::vector< size_t > dims_
Definition: gtsam_unstable/slam/DummyFactor.h:21
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::DummyFactor
Definition: gtsam_unstable/slam/DummyFactor.h:17
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Definition: NonlinearFactor.h:142
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:92
gtsam::DummyFactor::rowDim_
size_t rowDim_
choose dimension for the rows
Definition: gtsam_unstable/slam/DummyFactor.h:22
DummyFactor.h
A simple factor that can be used to trick gtsam solvers into believing a graph is connected.
key2
const Symbol key2('v', 2)
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
gtsam::DummyFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: DummyFactor.cpp:32
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
key
const gtsam::Symbol key('X', 0)
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam::DummyFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: DummyFactor.cpp:25
gtsam
traits
Definition: SFMdata.h:40
gtsam::Values
Definition: Values.h:65
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:143
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:69
gtsam::noiseModel::Diagonal::shared_ptr
std::shared_ptr< Diagonal > shared_ptr
Definition: NoiseModel.h:321
gtsam::DummyFactor::linearize
std::shared_ptr< GaussianFactor > linearize(const Values &c) const override
Definition: DummyFactor.cpp:39
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Factor::size
size_t size() const
Definition: Factor.h:160
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:29