triangulation.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/Point3.h"
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/SphericalCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/TriangulationFactor.h>

#include <optional>

namespace gtsam {

class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
public:
  TriangulationUnderconstrainedException() :
      std::runtime_error("Triangulation Underconstrained Exception.") {
  }
};

class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error {
public:
  TriangulationCheiralityException() :
      std::runtime_error(
          "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
  }
};

GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
    const Point2Vector& measurements, double rank_tol = 1e-9);

GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
    const std::vector<Unit3>& measurements, double rank_tol = 1e-9);

GTSAM_EXPORT Point3 triangulateDLT(
    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
    const Point2Vector& measurements,
    double rank_tol = 1e-9);

GTSAM_EXPORT Point3 triangulateDLT(
    const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
    const std::vector<Unit3>& measurements,
    double rank_tol = 1e-9);

GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
                                    const Point3Vector& calibratedMeasurements,
                                    const SharedIsotropic& measurementNoise);

template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
    const std::vector<Pose3>& poses, std::shared_ptr<CALIBRATION> sharedCal,
    const Point2Vector& measurements, Key landmarkKey,
    const Point3& initialEstimate,
    const SharedNoiseModel& model = noiseModel::Unit::Create(2)) {
  Values values;
  values.insert(landmarkKey, initialEstimate); // Initial landmark value
  NonlinearFactorGraph graph;
  for (size_t i = 0; i < measurements.size(); i++) {
    const Pose3& pose_i = poses[i];
    typedef PinholePose<CALIBRATION> Camera;
    Camera camera_i(pose_i, sharedCal);
    graph.emplace_shared<TriangulationFactor<Camera> > //
        (camera_i, measurements[i], model, landmarkKey);
  }
  return {graph, values};
}

template<class CAMERA>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
    const CameraSet<CAMERA>& cameras,
    const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
    const Point3& initialEstimate,
    const SharedNoiseModel& model = nullptr) {
  Values values;
  values.insert(landmarkKey, initialEstimate); // Initial landmark value
  NonlinearFactorGraph graph;
  static SharedNoiseModel unit(noiseModel::Unit::Create(
      traits<typename CAMERA::Measurement>::dimension));
  for (size_t i = 0; i < measurements.size(); i++) {
    const CAMERA& camera_i = cameras[i];
    graph.emplace_shared<TriangulationFactor<CAMERA> > //
        (camera_i, measurements[i], model? model : unit, landmarkKey);
  }
  return {graph, values};
}

GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
    const Values& values, Key landmarkKey);

template<class CALIBRATION>
Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
    std::shared_ptr<CALIBRATION> sharedCal,
    const Point2Vector& measurements, const Point3& initialEstimate,
    const SharedNoiseModel& model = nullptr) {

  // Create a factor graph and initial values
  const auto [graph, values] = triangulationGraph<CALIBRATION> //
      (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);

  return optimize(graph, values, Symbol('p', 0));
}

template<class CAMERA>
Point3 triangulateNonlinear(
    const CameraSet<CAMERA>& cameras,
    const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate,
    const SharedNoiseModel& model = nullptr) {

  // Create a factor graph and initial values
  const auto [graph, values] = triangulationGraph<CAMERA> //
      (cameras, measurements, Symbol('p', 0), initialEstimate, model);

  return optimize(graph, values, Symbol('p', 0));
}

template<class CAMERA>
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
projectionMatricesFromCameras(const CameraSet<CAMERA> &cameras) {
  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
  for (const CAMERA &camera: cameras) {
    projection_matrices.push_back(camera.cameraProjectionMatrix());
  }
  return projection_matrices;
}

// overload, assuming pinholePose
template<class CALIBRATION>
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses(
        const std::vector<Pose3> &poses, std::shared_ptr<CALIBRATION> sharedCal) {
  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
  for (size_t i = 0; i < poses.size(); i++) {
    PinholePose<CALIBRATION> camera(poses.at(i), sharedCal);
    projection_matrices.push_back(camera.cameraProjectionMatrix());
  }
  return projection_matrices;
}

template <class CALIBRATION>
Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
  const auto& K = cal.K();
  return Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2));
}

template <class CALIBRATION, class MEASUREMENT>
MEASUREMENT undistortMeasurementInternal(
    const CALIBRATION& cal, const MEASUREMENT& measurement,
    std::optional<Cal3_S2> pinholeCal = {}) {
  if (!pinholeCal) {
    pinholeCal = createPinholeCalibration(cal);
  }
  return pinholeCal->uncalibrate(cal.calibrate(measurement));
}

template <class CALIBRATION>
Point2Vector undistortMeasurements(const CALIBRATION& cal,
                                   const Point2Vector& measurements) {
  Cal3_S2 pinholeCalibration = createPinholeCalibration(cal);
  Point2Vector undistortedMeasurements;
  // Calibrate with cal and uncalibrate with pinhole version of cal so that
  // measurements are undistorted.
  std::transform(measurements.begin(), measurements.end(),
                 std::back_inserter(undistortedMeasurements),
                 [&cal, &pinholeCalibration](const Point2& measurement) {
                   return undistortMeasurementInternal<CALIBRATION>(
                       cal, measurement, pinholeCalibration);
                 });
  return undistortedMeasurements;
}

template <>
inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
                                          const Point2Vector& measurements) {
  return measurements;
}

template <class CAMERA>
typename CAMERA::MeasurementVector undistortMeasurements(
    const CameraSet<CAMERA>& cameras,
    const typename CAMERA::MeasurementVector& measurements) {
  const size_t nrMeasurements = measurements.size();
  assert(nrMeasurements == cameras.size());
  typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
  for (size_t ii = 0; ii < nrMeasurements; ++ii) {
    // Calibrate with cal and uncalibrate with pinhole version of cal so that
    // measurements are undistorted.
    undistortedMeasurements[ii] =
        undistortMeasurementInternal<typename CAMERA::CalibrationType>(
            cameras[ii].calibration(), measurements[ii]);
  }
  return undistortedMeasurements;
}

template <class CAMERA = PinholeCamera<Cal3_S2>>
inline PinholeCamera<Cal3_S2>::MeasurementVector undistortMeasurements(
    const CameraSet<PinholeCamera<Cal3_S2>>& cameras,
    const PinholeCamera<Cal3_S2>::MeasurementVector& measurements) {
  return measurements;
}

template <class CAMERA = SphericalCamera>
inline SphericalCamera::MeasurementVector undistortMeasurements(
    const CameraSet<SphericalCamera>& cameras,
    const SphericalCamera::MeasurementVector& measurements) {
  return measurements;
}

template <class CALIBRATION>
inline Point3Vector calibrateMeasurementsShared(
    const CALIBRATION& cal, const Point2Vector& measurements) {
  Point3Vector calibratedMeasurements;
  // Calibrate with cal and uncalibrate with pinhole version of cal so that
  // measurements are undistorted.
  std::transform(measurements.begin(), measurements.end(),
                 std::back_inserter(calibratedMeasurements),
                 [&cal](const Point2& measurement) {
                   Point3 p;
                   p << cal.calibrate(measurement), 1.0;
                   return p;
                 });
  return calibratedMeasurements;
}

template <class CAMERA>
inline Point3Vector calibrateMeasurements(
    const CameraSet<CAMERA>& cameras,
    const typename CAMERA::MeasurementVector& measurements) {
  const size_t nrMeasurements = measurements.size();
  assert(nrMeasurements == cameras.size());
  Point3Vector calibratedMeasurements(nrMeasurements);
  for (size_t ii = 0; ii < nrMeasurements; ++ii) {
    calibratedMeasurements[ii]
        << cameras[ii].calibration().calibrate(measurements[ii]),
        1.0;
  }
  return calibratedMeasurements;
}

template <class CAMERA = SphericalCamera>
inline Point3Vector calibrateMeasurements(
    const CameraSet<SphericalCamera>& cameras,
    const SphericalCamera::MeasurementVector& measurements) {
  Point3Vector calibratedMeasurements(measurements.size());
  for (size_t ii = 0; ii < measurements.size(); ++ii) {
    calibratedMeasurements[ii] << measurements[ii].point3();
  }
  return calibratedMeasurements;
}

template <class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses,
                         std::shared_ptr<CALIBRATION> sharedCal,
                         const Point2Vector& measurements,
                         double rank_tol = 1e-9, bool optimize = false,
                         const SharedNoiseModel& model = nullptr,
                         const bool useLOST = false) {
  assert(poses.size() == measurements.size());
  if (poses.size() < 2) throw(TriangulationUnderconstrainedException());

  // Triangulate linearly
  Point3 point;
  if (useLOST) {
    // Reduce input noise model to an isotropic noise model using the mean of
    // the diagonal.
    const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
    SharedIsotropic measurementNoise =
        noiseModel::Isotropic::Sigma(2, measurementSigma);
    // calibrate the measurements to obtain homogenous coordinates in image
    // plane.
    auto calibratedMeasurements =
        calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);

    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
  } else {
    // construct projection matrices from poses & calibration
    auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);

    // Undistort the measurements, leaving only the pinhole elements in effect.
    auto undistortedMeasurements =
        undistortMeasurements<CALIBRATION>(*sharedCal, measurements);

    point =
        triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
  }

  // Then refine using non-linear optimization
  if (optimize)
    point = triangulateNonlinear<CALIBRATION>  //
        (poses, sharedCal, measurements, point, model);

#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
  // verify that the triangulated point lies in front of all cameras
  for (const Pose3& pose : poses) {
    const Point3& p_local = pose.transformTo(point);
    if (p_local.z() <= 0) throw(TriangulationCheiralityException());
  }
#endif

  return point;
}

template <class CAMERA>
Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
                         const typename CAMERA::MeasurementVector& measurements,
                         double rank_tol = 1e-9, bool optimize = false,
                         const SharedNoiseModel& model = nullptr,
                         const bool useLOST = false) {
  size_t m = cameras.size();
  assert(measurements.size() == m);

  if (m < 2) throw(TriangulationUnderconstrainedException());

  // Triangulate linearly
  Point3 point;
  if (useLOST) {
    // Reduce input noise model to an isotropic noise model using the mean of
    // the diagonal.
    const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
    SharedIsotropic measurementNoise =
        noiseModel::Isotropic::Sigma(2, measurementSigma);

    // construct poses from cameras.
    std::vector<Pose3> poses;
    poses.reserve(cameras.size());
    for (const auto& camera : cameras) poses.push_back(camera.pose());

    // calibrate the measurements to obtain homogenous coordinates in image
    // plane.
    auto calibratedMeasurements =
        calibrateMeasurements<CAMERA>(cameras, measurements);

    point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
  } else {
    // construct projection matrices from poses & calibration
    auto projection_matrices = projectionMatricesFromCameras(cameras);

    // Undistort the measurements, leaving only the pinhole elements in effect.
    auto undistortedMeasurements =
        undistortMeasurements<CAMERA>(cameras, measurements);

    point =
        triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
  }

  // Then refine using non-linear optimization
  if (optimize) {
    point = triangulateNonlinear<CAMERA>(cameras, measurements, point, model);
  }

#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
  // verify that the triangulated point lies in front of all cameras
  for (const CAMERA& camera : cameras) {
    const Point3& p_local = camera.pose().transformTo(point);
    if (p_local.z() <= 0) throw(TriangulationCheiralityException());
  }
#endif

  return point;
}

template <class CALIBRATION>
Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
                         const Point2Vector& measurements,
                         double rank_tol = 1e-9, bool optimize = false,
                         const SharedNoiseModel& model = nullptr,
                         const bool useLOST = false) {
  return triangulatePoint3<PinholeCamera<CALIBRATION>>  //
      (cameras, measurements, rank_tol, optimize, model, useLOST);
}

struct GTSAM_EXPORT TriangulationParameters {

  double rankTolerance; 
  bool enableEPI; 

  double landmarkDistanceThreshold; //

  double dynamicOutlierRejectionThreshold;

  SharedNoiseModel noiseModel; 

  TriangulationParameters(const double _rankTolerance = 1.0,
      const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
      double _dynamicOutlierRejectionThreshold = -1,
      const SharedNoiseModel& _noiseModel = nullptr) :
      rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
      landmarkDistanceThreshold(_landmarkDistanceThreshold), //
      dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
      noiseModel(_noiseModel){
  }

  // stream to output
  friend std::ostream &operator<<(std::ostream &os,
      const TriangulationParameters& p) {
    os << "rankTolerance = " << p.rankTolerance << std::endl;
    os << "enableEPI = " << p.enableEPI << std::endl;
    os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
        << std::endl;
    os << "dynamicOutlierRejectionThreshold = "
        << p.dynamicOutlierRejectionThreshold << std::endl;
    os << "noise model" << std::endl;
    return os;
  }

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_NVP(rankTolerance);
    ar & BOOST_SERIALIZATION_NVP(enableEPI);
    ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
    ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
  }
#endif
};

class TriangulationResult : public std::optional<Point3> {
 public:
  enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
  Status status;

 private:
  TriangulationResult(Status s) : status(s) {}

 public:
  TriangulationResult() {}

  TriangulationResult(const Point3& p) : status(VALID) { emplace(p); }
  static TriangulationResult Degenerate() {
    return TriangulationResult(DEGENERATE);
  }
  static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); }
  static TriangulationResult FarPoint() {
    return TriangulationResult(FAR_POINT);
  }
  static TriangulationResult BehindCamera() {
    return TriangulationResult(BEHIND_CAMERA);
  }
  bool valid() const { return status == VALID; }
  bool degenerate() const { return status == DEGENERATE; }
  bool outlier() const { return status == OUTLIER; }
  bool farPoint() const { return status == FAR_POINT; }
  bool behindCamera() const { return status == BEHIND_CAMERA; }
  const gtsam::Point3& get() const {
    if (!has_value()) throw std::runtime_error("TriangulationResult has no value");
    return value();
  }
  // stream to output
  friend std::ostream& operator<<(std::ostream& os,
                                  const TriangulationResult& result) {
    if (result)
      os << "point = " << *result << std::endl;
    else
      os << "no point, status = " << result.status << std::endl;
    return os;
  }

 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_NVP(status);
  }
#endif
};

template<class CAMERA>
TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
    const typename CAMERA::MeasurementVector& measured,
    const TriangulationParameters& params) {

  size_t m = cameras.size();

  // if we have a single pose the corresponding factor is uninformative
  if (m < 2)
    return TriangulationResult::Degenerate();
  else
    // We triangulate the 3D position of the landmark
    try {
      Point3 point =
          triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
                                    params.enableEPI, params.noiseModel);

      // Check landmark distance and re-projection errors to avoid outliers
      size_t i = 0;
      double maxReprojError = 0.0;
      for(const CAMERA& camera: cameras) {
        const Pose3& pose = camera.pose();
        if (params.landmarkDistanceThreshold > 0
            && distance3(pose.translation(), point)
                > params.landmarkDistanceThreshold)
          return TriangulationResult::FarPoint();
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
        // verify that the triangulated point lies in front of all cameras
        // Only needed if this was not yet handled by exception
        const Point3& p_local = pose.transformTo(point);
        if (p_local.z() <= 0)
          return TriangulationResult::BehindCamera();
#endif
        // Check reprojection error
        if (params.dynamicOutlierRejectionThreshold > 0) {
          const typename CAMERA::Measurement& zi = measured.at(i);
          Point2 reprojectionError = camera.reprojectionError(point, zi);
          maxReprojError = std::max(maxReprojError, reprojectionError.norm());
        }
        i += 1;
      }
      // Flag as degenerate if average reprojection error is too large
      if (params.dynamicOutlierRejectionThreshold > 0
          && maxReprojError > params.dynamicOutlierRejectionThreshold)
        return TriangulationResult::Outlier();

      // all good!
      return TriangulationResult(point);
    } catch (TriangulationUnderconstrainedException&) {
      // This exception is thrown if
      // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
      // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
      return TriangulationResult::Degenerate();
    } catch (TriangulationCheiralityException&) {
      // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
      return TriangulationResult::BehindCamera();
    }
}

// Vector of Cameras - used by the Python/MATLAB wrapper
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
using CameraSetSpherical = CameraSet<SphericalCamera>;
} // \namespace gtsam