BinaryMeasurement.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

#pragma once

#include <gtsam/base/Testable.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/NoiseModel.h>

#include <iostream>
#include <vector>

namespace gtsam {

template <class T> class BinaryMeasurement : public Factor {
  // Check that T type is testable
  BOOST_CONCEPT_ASSERT((IsTestable<T>));

public:
  // shorthand for a smart pointer to a measurement
  using shared_ptr = typename boost::shared_ptr<BinaryMeasurement>;

private:
  T measured_;                  
  SharedNoiseModel noiseModel_; 

 public:
  BinaryMeasurement(Key key1, Key key2, const T &measured,
                    const SharedNoiseModel &model = nullptr)
      : Factor(std::vector<Key>({key1, key2})),
        measured_(measured),
        noiseModel_(model) {}

  virtual ~BinaryMeasurement() {}


  Key key1() const { return keys_[0]; }
  Key key2() const { return keys_[1]; }
  const T &measured() const { return measured_; }
  const SharedNoiseModel &noiseModel() const { return noiseModel_; }


  void print(const std::string &s, const KeyFormatter &keyFormatter =
                                       DefaultKeyFormatter) const override {
    std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << ","
              << keyFormatter(this->key2()) << ")\n";
    traits<T>::Print(measured_, "  measured: ");
    this->noiseModel_->print("  noise model: ");
  }

  bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const {
    const BinaryMeasurement<T> *e =
        dynamic_cast<const BinaryMeasurement<T> *>(&expected);
    return e != nullptr && Factor::equals(*e) &&
           traits<T>::Equals(this->measured_, e->measured_, tol) &&
           noiseModel_->equals(*expected.noiseModel());
  }
};
} // namespace gtsam