1/**
2 * @file DummyFactor.cpp
3 *
4 * @date Sep 10, 2012
5 * @author Alex Cunningham
6 */
7
8#include <gtsam_unstable/slam/DummyFactor.h>
9
10namespace gtsam {
11
12/* ************************************************************************* */
13DummyFactor::DummyFactor(const Key& key1, size_t dim1, const Key& key2, size_t dim2)
14: NonlinearFactor(KeyVector{key1, key2})
15{
16 dims_.push_back(x: dim1);
17 dims_.push_back(x: dim2);
18 if (dim1 > dim2)
19 rowDim_ = dim1;
20 else
21 rowDim_ = dim2;
22}
23
24/* ************************************************************************* */
25void 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/* ************************************************************************* */
32bool DummyFactor::equals(const NonlinearFactor& f, double tol) const {
33 const DummyFactor* e = dynamic_cast<const DummyFactor*>(&f);
34 return e && Base::equals(other: f, tol) && dims_ == e->dims_ && rowDim_ == e->rowDim_;
35}
36
37/* ************************************************************************* */
38std::shared_ptr<GaussianFactor>
39DummyFactor::linearize(const Values& c) const {
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(rows: rowDim_, cols: dims_[j]);
49 }
50
51 noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(dim: rowDim_);
52 return GaussianFactor::shared_ptr(
53 new JacobianFactor(terms, Vector::Zero(size: rowDim_), model));
54}
55
56/* ************************************************************************* */
57
58} // \namespace gtsam
59
60
61
62
63