Program Listing for File triangulation.h

Return to documentation for file (aslam_cv2/aslam_cv_triangulation/include/aslam/triangulation/triangulation.h)

#ifndef TRIANGULATION_H_
#define TRIANGULATION_H_
#include <vector>

#include <aslam/common/memory.h>
#include <aslam/common/pose-types.h>
#include <aslam/frames/feature-track.h>
#include <Eigen/Dense>

namespace aslam {

struct TriangulationResult {
  enum class Status {
    kSuccessful,
    kTooFewMeasurments,
    kUnobservable,
    kUninitialized
  };
  // Make the enum values accessible from the outside without the additional
  // indirection.
  static Status SUCCESSFUL;
  static Status TOO_FEW_MEASUREMENTS;
  static Status UNOBSERVABLE;
  static Status UNINITIALIZED;

  constexpr TriangulationResult() : status_(Status::kUninitialized) {};
  constexpr TriangulationResult(Status status) : status_(status) {};

  explicit operator bool() const { return wasTriangulationSuccessful(); };

  friend std::ostream& operator<< (std::ostream& out, const TriangulationResult& state)  {
    std::string enum_str;
    switch (state.status_){
      case Status::kSuccessful:         enum_str = "SUCCESSFUL"; break;
      case Status::kTooFewMeasurments:  enum_str = "TOO_FEW_MEASUREMENTS"; break;
      case Status::kUnobservable:       enum_str = "UNOBSERVABLE"; break;
      default:
        case Status::kUninitialized:    enum_str = "UNINITIALIZED"; break;
    }
    out << "ProjectionResult: " << enum_str << std::endl;
    return out;
  }

  bool wasTriangulationSuccessful() const {
    return (status_ == Status::kSuccessful); };

  Status status() const { return status_; };

 private:
  Status status_;
};

TriangulationResult linearTriangulateFromNViews(
    const Aligned<std::vector, Eigen::Vector2d>& measurements_normalized,
    const aslam::TransformationVector& T_G_B,
    const aslam::Transformation& T_B_C, Eigen::Vector3d* G_point);

TriangulationResult iterativeGaussNewtonTriangulateFromNViews(
    const Aligned<std::vector, Eigen::Vector2d>& measurements_normalized,
    const Aligned<std::vector, aslam::Transformation>& T_G_B,
    const aslam::Transformation& T_B_C, Eigen::Vector3d* G_point);

TriangulationResult linearTriangulateFromNViews(
    const Eigen::Matrix3Xd& t_G_bv, const Eigen::Matrix3Xd& p_G_C, Eigen::Vector3d* p_G_P);

TriangulationResult linearTriangulateFromNViewsMultiCam(
    const Aligned<std::vector, Eigen::Vector2d>& measurements_normalized,
    const std::vector<size_t>& measurement_camera_indices,
    const Aligned<std::vector, aslam::Transformation>& T_G_B,
    const Aligned<std::vector, aslam::Transformation>& T_B_C,
    Eigen::Vector3d* G_point);

TriangulationResult triangulateFeatureTrack(
    const aslam::FeatureTrack& track,
    const aslam::TransformationVector& T_W_Bs,
    Eigen::Vector3d* W_landmark);

TriangulationResult fastTriangulateFeatureTrack(
    const aslam::FeatureTrack& track,
    const aslam::TransformationVector& T_W_Bs,
    Eigen::Vector3d* W_landmark);
}  // namespace aslam
#endif  // TRIANGULATION_H_