PoseToPointFactor.h

 
#pragma once
 
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <ostream>
 
namespace gtsam {
 
class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
 private:
  typedef PoseToPointFactor This;
  typedef NoiseModelFactor2<Pose3, Point3> Base;
 
  Point3 measured_; 
 public:
  // shorthand for a smart pointer to a factor
  typedef boost::shared_ptr<PoseToPointFactor> shared_ptr;
 
  PoseToPointFactor() {}
 
  PoseToPointFactor(Key key1, Key key2, const Point3& measured,
                    const SharedNoiseModel& model)
      : Base(model, key1, key2), measured_(measured) {}
 
  virtual ~PoseToPointFactor() {}
 
  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
                                               DefaultKeyFormatter) const {
    std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << ","
              << keyFormatter(this->key2()) << ")\n"
              << "  measured: " << measured_.transpose() << std::endl;
    this->noiseModel_->print("  noise model: ");
  }
 
  virtual bool equals(const NonlinearFactor& expected,
                      double tol = 1e-9) const {
    const This* e = dynamic_cast<const This*>(&expected);
    return e != nullptr && Base::equals(*e, tol) &&
           traits<Point3>::Equals(this->measured_, e->measured_, tol);
  }
 
  Vector evaluateError(const Pose3& wTwi, const Point3& wPwp,
                       boost::optional<Matrix&> H1 = boost::none,
                       boost::optional<Matrix&> H2 = boost::none) const {
    return wTwi.transformTo(wPwp, H1, H2) - measured_;
  }
 
  const Point3& measured() const { return measured_; }
 
 private:
  friend class boost::serialization::access;
  template <class ARCHIVE>
  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
    ar& boost::serialization::make_nvp(
        "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
    ar& BOOST_SERIALIZATION_NVP(measured_);
  }
 
};  // \class PoseToPointFactor
 
}  // namespace gtsam