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/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>

namespace gtsam {

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

class 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>& projection_matrices,
    const std::vector<Point2>& measurements, double rank_tol = 1e-9);

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

template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
    const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
    const std::vector<Point2>& measurements, Key landmarkKey,
    const Point3& initialEstimate) {
  Values values;
  values.insert(landmarkKey, initialEstimate); // Initial landmark value
  NonlinearFactorGraph graph;
  static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
  static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
  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.push_back(TriangulationFactor<Camera> //
        (camera_i, measurements[i], unit2, landmarkKey));
  }
  return std::make_pair(graph, values);
}

template<class CAMERA>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
    const std::vector<CAMERA>& cameras,
    const std::vector<typename CAMERA::Measurement>& measurements, Key landmarkKey,
    const Point3& initialEstimate) {
  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.push_back(TriangulationFactor<CAMERA> //
        (camera_i, measurements[i], unit, landmarkKey));
  }
  return std::make_pair(graph, values);
}

template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
    const std::vector<PinholeCamera<CALIBRATION> >& cameras,
    const std::vector<Point2>& measurements, Key landmarkKey,
    const Point3& initialEstimate) {
  return triangulationGraph<PinholeCamera<CALIBRATION> > //
  (cameras, measurements, landmarkKey, initialEstimate);
}

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

template<class CALIBRATION>
Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
    boost::shared_ptr<CALIBRATION> sharedCal,
    const std::vector<Point2>& measurements, const Point3& initialEstimate) {

  // Create a factor graph and initial values
  Values values;
  NonlinearFactorGraph graph;
  boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
      (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate);

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

template<class CAMERA>
Point3 triangulateNonlinear(
    const std::vector<CAMERA>& cameras,
    const std::vector<typename CAMERA::Measurement>& measurements, const Point3& initialEstimate) {

  // Create a factor graph and initial values
  Values values;
  NonlinearFactorGraph graph;
  boost::tie(graph, values) = triangulationGraph<CAMERA> //
      (cameras, measurements, Symbol('p', 0), initialEstimate);

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

template<class CALIBRATION>
Point3 triangulateNonlinear(
    const std::vector<PinholeCamera<CALIBRATION> >& cameras,
    const std::vector<Point2>& measurements, const Point3& initialEstimate) {
  return triangulateNonlinear<PinholeCamera<CALIBRATION> > //
  (cameras, measurements, initialEstimate);
}

template<class CALIBRATION>
struct CameraProjectionMatrix {
  CameraProjectionMatrix(const CALIBRATION& calibration) :
      K_(calibration.K()) {
  }
  Matrix34 operator()(const Pose3& pose) const {
    return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0);
  }
private:
  const Matrix3 K_;
};

template<class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses,
    boost::shared_ptr<CALIBRATION> sharedCal,
    const std::vector<Point2>& measurements, double rank_tol = 1e-9,
    bool optimize = false) {

  assert(poses.size() == measurements.size());
  if (poses.size() < 2)
    throw(TriangulationUnderconstrainedException());

  // construct projection matrices from poses & calibration
  std::vector<Matrix34> projection_matrices;
  CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
  for(const Pose3& pose: poses)
    projection_matrices.push_back(createP(pose));

  // Triangulate linearly
  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);

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

#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.transform_to(point);
    if (p_local.z() <= 0)
      throw(TriangulationCheiralityException());
  }
#endif

  return point;
}

template<class CAMERA>
Point3 triangulatePoint3(
    const std::vector<CAMERA>& cameras,
    const std::vector<Point2>& measurements, double rank_tol = 1e-9,
    bool optimize = false) {

  size_t m = cameras.size();
  assert(measurements.size() == m);

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

  // construct projection matrices from poses & calibration
  std::vector<Matrix34> projection_matrices;
  for(const CAMERA& camera: cameras)
    projection_matrices.push_back(
        CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(
            camera.pose()));
  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);

  // The n refine using non-linear optimization
  if (optimize)
    point = triangulateNonlinear<CAMERA>(cameras, measurements, point);

#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().transform_to(point);
    if (p_local.z() <= 0)
      throw(TriangulationCheiralityException());
  }
#endif

  return point;
}

template<class CALIBRATION>
Point3 triangulatePoint3(
    const std::vector<PinholeCamera<CALIBRATION> >& cameras,
    const std::vector<Point2>& measurements, double rank_tol = 1e-9,
    bool optimize = false) {
  return triangulatePoint3<PinholeCamera<CALIBRATION> > //
  (cameras, measurements, rank_tol, optimize);
}

struct TriangulationParameters {

  double rankTolerance; 
  bool enableEPI; 

  double landmarkDistanceThreshold; //

  double dynamicOutlierRejectionThreshold;

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

  // 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;
    return os;
  }

private:

  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);
  }
};

class TriangulationResult: public boost::optional<Point3> {
  enum Status {
    VALID, DEGENERATE, BEHIND_CAMERA
  };
  Status status_;
  TriangulationResult(Status s) :
      status_(s) {
  }
public:

  TriangulationResult() {}

  TriangulationResult(const Point3& p) :
      status_(VALID) {
    reset(p);
  }
  static TriangulationResult Degenerate() {
    return TriangulationResult(DEGENERATE);
  }
  static TriangulationResult BehindCamera() {
    return TriangulationResult(BEHIND_CAMERA);
  }
  bool degenerate() const {
    return status_ == DEGENERATE;
  }
  bool behindCamera() const {
    return status_ == BEHIND_CAMERA;
  }
  // 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:

  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int version) {
    ar & BOOST_SERIALIZATION_NVP(status_);
  }
};

template<class CAMERA>
TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
    const std::vector<Point2>& 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);

      // Check landmark distance and re-projection errors to avoid outliers
      size_t i = 0;
      double totalReprojError = 0.0;
      for(const CAMERA& camera: cameras) {
        const Pose3& pose = camera.pose();
        if (params.landmarkDistanceThreshold > 0
            && distance3(pose.translation(), point)
                > params.landmarkDistanceThreshold)
          return TriangulationResult::Degenerate();
#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.transform_to(point);
        if (p_local.z() <= 0)
          return TriangulationResult::BehindCamera();
#endif
        // Check reprojection error
        if (params.dynamicOutlierRejectionThreshold > 0) {
          const Point2& zi = measured.at(i);
          Point2 reprojectionError(camera.project(point) - zi);
          totalReprojError += reprojectionError.norm();
        }
        i += 1;
      }
      // Flag as degenerate if average reprojection error is too large
      if (params.dynamicOutlierRejectionThreshold > 0
          && totalReprojError / m > params.dynamicOutlierRejectionThreshold)
        return TriangulationResult::Degenerate();

      // 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();
    }
}

} // \namespace gtsam