TriangulationFactor.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/NoiseModelFactorN.h>
#include <gtsam/geometry/CalibratedCamera.h>
 
#include <type_traits>
 
namespace gtsam {

template<class CAMERA>
class TriangulationFactor: public NoiseModelFactorN<Point3> {
 
public:

  using Camera = CAMERA;
  using Measurement = typename CAMERA::Measurement;
 
protected:

  using Base = NoiseModelFactorN<Point3>;

  using This = TriangulationFactor<CAMERA>;
 
  // Keep a copy of measurement and calibration for I/O
  const CAMERA camera_; 
  const Measurement measured_; 
 
  // verbosity handling for Cheirality Exceptions
  const bool throwCheirality_; 
  const bool verboseCheirality_; 
 
public:
  using shared_ptr = std::shared_ptr<This>;
 
  // Provide access to the Matrix& version of evaluateError:
  using NoiseModelFactor1<Point3>::evaluateError;

  TriangulationFactor() :
      throwCheirality_(false), verboseCheirality_(false), measured_() {
  }

  TriangulationFactor(const CAMERA& camera, const Measurement& measured,
      const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false,
      bool verboseCheirality = false) :
      Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
          throwCheirality), verboseCheirality_(verboseCheirality) {
    if (model && !noiseModel::matchesDimension(*model, measured_))
      throw std::invalid_argument(
          "TriangulationFactor must be created with "
              + std::to_string((int) traits<Measurement>::dimension)
              + "-dimensional noise model.");
  }

  ~TriangulationFactor() override {
  }

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

  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
      DefaultKeyFormatter) const override {
    std::cout << s << "TriangulationFactor,";
    camera_.print("camera");
    traits<Measurement>::Print(measured_, "z");
    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->camera_.equals(e->camera_, tol)
        && traits<Measurement>::Equals(this->measured_, e->measured_, tol);
  }

  Vector evaluateError(const Point3& point, OptionalMatrixType H2) const override {
    try {
      const Measurement predicted = camera_.project2(point, OptionalNone, H2);
      if constexpr (internal::HasLocalJacobians<Measurement>::value &&
                    !std::is_base_of_v<
                        vector_space_tag,
                        typename traits<Measurement>::structure_category>) {
        if (H2) {
          typename traits<Measurement>::ChartJacobian::Jacobian Hlocal;
          const Vector error =
              traits<Measurement>::Local(measured_, predicted, {}, Hlocal);
          *H2 = Hlocal * *H2;
          return error;
        }
      }
      return traits<Measurement>::Local(measured_, predicted);
    } catch (CheiralityException& e) {
      if (H2)
        *H2 = Matrix::Zero(traits<Measurement>::dimension, 3);
      if (verboseCheirality_)
        std::cout << e.what() << ": Landmark "
            << DefaultKeyFormatter(this->key()) << " moved behind camera"
            << std::endl;
      if (throwCheirality_)
        throw e;
      return camera_.defaultErrorWhenTriangulatingBehindCamera();
    }
  }

  mutable VerticalBlockMatrix Ab;
  mutable Matrix A;
  mutable Vector b;

  std::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
    // Only linearize if the factor is active
    if (!this->active(x))
      return std::shared_ptr<JacobianFactor>();
 
    // Allocate memory for Jacobian factor, do only once
    if (Ab.rows() == 0) {
      std::vector<size_t> dimensions(1, 3);
      Ab = VerticalBlockMatrix(dimensions, traits<Measurement>::dimension, true);
      A.resize(traits<Measurement>::dimension,3);
      b.resize(traits<Measurement>::dimension);
    }
 
    // Would be even better if we could pass blocks to project
    const Point3& point = x.at<Point3>(key());
    b = -evaluateError(point, &A);
    if (noiseModel_)
      this->noiseModel_->WhitenSystem(A, b);
 
    Ab(0) = A;
    Ab(1) = b;
 
    return std::make_shared<JacobianFactor>(this->keys_, Ab);
  }

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

  inline bool verboseCheirality() const {
    return verboseCheirality_;
  }

  inline bool throwCheirality() const {
    return throwCheirality_;
  }
 
private:
 
#if 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(camera_);
    ar & BOOST_SERIALIZATION_NVP(measured_);
    ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
    ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
  }
#endif
};
} // \ namespace gtsam