gtsam  4.1.0
gtsam
BinaryMeasurement.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
12 #pragma once
13 
26 #include <gtsam/base/Testable.h>
27 #include <gtsam/inference/Factor.h>
28 #include <gtsam/inference/Key.h>
30 
31 #include <iostream>
32 #include <vector>
33 
34 namespace gtsam {
35 
36 template <class T> class BinaryMeasurement : public Factor {
37  // Check that T type is testable
38  BOOST_CONCEPT_ASSERT((IsTestable<T>));
39 
40 public:
41  // shorthand for a smart pointer to a measurement
42  using shared_ptr = typename boost::shared_ptr<BinaryMeasurement>;
43 
44 private:
45  T measured_;
46  SharedNoiseModel noiseModel_;
47 
48 public:
49  BinaryMeasurement(Key key1, Key key2, const T &measured,
50  const SharedNoiseModel &model = nullptr)
51  : Factor(std::vector<Key>({key1, key2})), measured_(measured),
52  noiseModel_(model) {}
53 
56 
57  Key key1() const { return keys_[0]; }
58  Key key2() const { return keys_[1]; }
59  const T &measured() const { return measured_; }
60  const SharedNoiseModel &noiseModel() const { return noiseModel_; }
61 
65 
66  void print(const std::string &s,
67  const KeyFormatter &keyFormatter = DefaultKeyFormatter) const {
68  std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << ","
69  << keyFormatter(this->key2()) << ")\n";
70  traits<T>::Print(measured_, " measured: ");
71  this->noiseModel_->print(" noise model: ");
72  }
73 
74  bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const {
75  const BinaryMeasurement<T> *e =
76  dynamic_cast<const BinaryMeasurement<T> *>(&expected);
77  return e != nullptr && Factor::equals(*e) &&
78  traits<T>::Equals(this->measured_, e->measured_, tol) &&
79  noiseModel_->equals(*expected.noiseModel());
80  }
82 };
83 } // namespace gtsam
Testable.h
Concept check for values that can be used in unit tests.
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::BinaryMeasurement
Definition: BinaryMeasurement.h:36
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::IsTestable
Definition: Testable.h:57
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition: Factor.h:72
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Factor.h
The base class for all factors.
Key.h
gtsam::KeyFormatter
boost::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::Factor
This is the base class for all factor types.
Definition: Factor.h:55
NoiseModel.h
gtsam::Factor::Factor
Factor()
Default constructor for I/O.
Definition: Factor.h:78
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition: Factor.cpp:42