MultiProjectionFactor.h

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

 * 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/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h>
#include "gtsam/geometry/Cal3_S2.h"

namespace gtsam {

  template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
  class MultiProjectionFactor: public NoiseModelFactor {
  protected:

    // Keep a copy of measurement and calibration for I/O
    Vector measured_;                    
    std::shared_ptr<CALIBRATION> K_;  
    std::optional<POSE> body_P_sensor_; 


    // verbosity handling for Cheirality Exceptions
    bool throwCheirality_; 
    bool verboseCheirality_; 

  public:

    typedef NoiseModelFactor Base;

    typedef MultiProjectionFactor<POSE, LANDMARK, CALIBRATION> This;

    typedef std::shared_ptr<This> shared_ptr;

    MultiProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {}

    MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
        KeySet poseKeys, Key pointKey, const std::shared_ptr<CALIBRATION>& K,
        std::optional<POSE> body_P_sensor = {}) :
          Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
          throwCheirality_(false), verboseCheirality_(false) {
      keys_.assign(poseKeys.begin(), poseKeys.end());
      keys_.push_back(pointKey);
    }

    MultiProjectionFactor(const Vector& measured, const SharedNoiseModel& model,
        KeySet poseKeys, Key pointKey, const std::shared_ptr<CALIBRATION>& K,
        bool throwCheirality, bool verboseCheirality,
        std::optional<POSE> body_P_sensor = {}) :
          Base(model), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
          throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}

    ~MultiProjectionFactor() override {}

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

    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
      std::cout << s << "MultiProjectionFactor, z = ";
      std::cout << measured_ << "measurements, z = ";
      if(this->body_P_sensor_)
        this->body_P_sensor_->print("  sensor pose in body frame: ");
      Base::print("", keyFormatter);
    }

    bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
      const This *e = dynamic_cast<const This*>(&p);
      return e
          && Base::equals(p, tol)
          //&& this->measured_.equals(e->measured_, tol)
          && this->K_->equals(*e->K_, tol)
          && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
    }

    Vector unwhitenedError(const Values& x, OptionalMatrixVecType H = nullptr) const override {

      Vector a;
      return a;

//      Point3 point = x.at<Point3>(*keys_.end());
//
//      std::vector<KeyType>::iterator vit;
//      for (vit = keys_.begin(); vit != keys_.end()-1; vit++) {
//        Key key = (*vit);
//        Pose3 pose = x.at<Pose3>(key);
//
//        if(body_P_sensor_) {
//          if(H1) {
//            Matrix H0;
//            PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
//            Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
//            *H1 = *H1 * H0;
//            return reprojectionError;
//          } else {
//            PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
//            Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
//            return reprojectionError;
//          }
//        } else {
//          PinholeCamera<CALIBRATION> camera(pose, *K_);
//          Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
//          return reprojectionError;
//        }
//      }

    }


    Vector evaluateError(const Pose3& pose, const Point3& point,
        OptionalJacobian<2, 6> H1 = {}, OptionalJacobian<2,3> H2 = {}) const {
      try {
        if(body_P_sensor_) {
          if(H1) {
            Matrix H0;
            PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
            Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
            *H1 = *H1 * H0;
            return reprojectionError;
          } else {
            PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
            Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
            return reprojectionError;
          }
        } else {
          PinholeCamera<CALIBRATION> camera(pose, *K_);
          Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
          return reprojectionError;
        }
      } catch( CheiralityException& e) {
        if (H1) *H1 = Matrix::Zero(2,6);
        if (H2) *H2 = Matrix::Zero(2,3);
        if (verboseCheirality_)
          std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->keys_.at(1)) <<
              " moved behind camera " << DefaultKeyFormatter(this->keys_.at(0)) << std::endl;
        if (throwCheirality_)
          throw e;
      }
      return Vector::Ones(2) * 2.0 * K_->fx();
    }

    const Vector& measured() const {
      return measured_;
    }

    inline const std::shared_ptr<CALIBRATION> calibration() const {
      return K_;
    }

    inline bool verboseCheirality() const { return verboseCheirality_; }

    inline bool throwCheirality() const { return throwCheirality_; }

  private:

#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION    
    friend class boost::serialization::access;
    template<class ARCHIVE>
    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
      ar & BOOST_SERIALIZATION_NVP(measured_);
      ar & BOOST_SERIALIZATION_NVP(K_);
      ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
      ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
      ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
    }
#endif
  };
} // \ namespace gtsam