PoseToPointFactor.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, 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/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
#include <ostream>
 
namespace gtsam {

template<typename POSE = Pose3, typename POINT = Point3>
class PoseToPointFactor
    : public NoiseModelFactorT<typename traits<POINT>::TangentVector, POSE,
                               POINT> {
 private:
  typedef PoseToPointFactor This;
  typedef NoiseModelFactorT<typename traits<POINT>::TangentVector, POSE, POINT>
      Base;
  using ErrorVector = typename traits<POINT>::TangentVector;
 
  POINT measured_; 
 
 public:
 
  // Provide access to the Matrix& version of evaluateError:
  using Base::evaluateError;
 
  // shorthand for a smart pointer to a factor
  typedef std::shared_ptr<PoseToPointFactor> shared_ptr;

  PoseToPointFactor() {}

  PoseToPointFactor(Key key1, Key key2, const POINT& measured,
                    const SharedNoiseModel& model)
      : Base(model, key1, key2), measured_(measured) {}
 
  virtual ~PoseToPointFactor() {}


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

  bool equals(const NonlinearFactor& expected,
              double tol = 1e-9) const override {
    const This* e = dynamic_cast<const This*>(&expected);
    return e != nullptr && Base::equals(*e, tol) &&
           traits<POINT>::Equals(this->measured_, e->measured_, tol);
  }

  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
  }


  ErrorVector evaluateError(
      const POSE& w_T_b, const POINT& w_P,
      OptionalMatrixType H1,
      OptionalMatrixType H2) const override {
    return w_T_b.transformTo(w_P, H1, H2) - measured_;
  }

  const POINT& measured() const { return measured_; }
 
 private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
    ar& boost::serialization::make_nvp(
        "NoiseModelFactor2",
        boost::serialization::base_object<Base>(*this));
    ar& BOOST_SERIALIZATION_NVP(measured_);
  }
#endif
 
};  // \class PoseToPointFactor
 
}  // namespace gtsam