// Copyright (c) 2022, ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)

#ifndef COLMAP_SRC_ESTIMATORS_TRIANGULATION_H_
#define COLMAP_SRC_ESTIMATORS_TRIANGULATION_H_

#include "base/camera.h"

#include <vector>

#include <Eigen/Core>

#include "optim/ransac.h"
#include "util/alignment.h"
#include "util/math.h"
#include "util/types.h"

namespace colmap {

// Triangulation estimator to estimate 3D point from multiple observations.
// The triangulation must satisfy the following constraints:
//    - Sufficient triangulation angle between observation pairs.
//    - All observations must satisfy cheirality constraint.
//
// An observation is composed of an image measurement and the corresponding
// camera pose and calibration.
class TriangulationEstimator {
 public:
  enum class ResidualType {
    ANGULAR_ERROR,
    REPROJECTION_ERROR,
  };

  struct PointData {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PointData() {}
    PointData(const Eigen::Vector2d& point_, const Eigen::Vector2d& point_N_)
        : point(point_), point_normalized(point_N_) {}
    // Image observation in pixels. Only needs to be set for REPROJECTION_ERROR.
    Eigen::Vector2d point;
    // Normalized image observation. Must always be set.
    Eigen::Vector2d point_normalized;
  };

  struct PoseData {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PoseData() : camera(nullptr) {}
    PoseData(const Eigen::Matrix3x4d& proj_matrix_,
             const Eigen::Vector3d& pose_, const Camera* camera_)
        : proj_matrix(proj_matrix_), proj_center(pose_), camera(camera_) {}
    // The projection matrix for the image of the observation.
    Eigen::Matrix3x4d proj_matrix;
    // The projection center for the image of the observation.
    Eigen::Vector3d proj_center;
    // The camera for the image of the observation.
    const Camera* camera;
  };

  typedef PointData X_t;
  typedef PoseData Y_t;
  typedef Eigen::Vector3d M_t;

  // Specify settings for triangulation estimator.
  void SetMinTriAngle(const double min_tri_angle);
  void SetResidualType(const ResidualType residual_type);

  // The minimum number of samples needed to estimate a model.
  static const int kMinNumSamples = 2;

  // Estimate a 3D point from a two-view observation.
  //
  // @param point_data        Image measurement.
  // @param point_data        Camera poses.
  //
  // @return                  Triangulated point if successful, otherwise none.
  std::vector<M_t> Estimate(const std::vector<X_t>& point_data,
                            const std::vector<Y_t>& pose_data) const;

  // Calculate residuals in terms of squared reprojection or angular error.
  //
  // @param point_data        Image measurements.
  // @param point_data        Camera poses.
  // @param xyz               3D point.
  //
  // @return                  Residual for each observation.
  void Residuals(const std::vector<X_t>& point_data,
                 const std::vector<Y_t>& pose_data, const M_t& xyz,
                 std::vector<double>* residuals) const;

 private:
  ResidualType residual_type_ = ResidualType::REPROJECTION_ERROR;
  double min_tri_angle_ = 0.0;
};

struct EstimateTriangulationOptions {
  // Minimum triangulation angle in radians.
  double min_tri_angle = 0.0;

  // The employed residual type.
  TriangulationEstimator::ResidualType residual_type =
      TriangulationEstimator::ResidualType::ANGULAR_ERROR;

  // RANSAC options for TriangulationEstimator.
  RANSACOptions ransac_options;

  void Check() const {
    CHECK_GE(min_tri_angle, 0.0);
    ransac_options.Check();
  }
};

// Robustly estimate 3D point from observations in multiple views using RANSAC
// and a subsequent non-linear refinement using all inliers. Returns true
// if the estimated number of inliers has more than two views.
bool EstimateTriangulation(
    const EstimateTriangulationOptions& options,
    const std::vector<TriangulationEstimator::PointData>& point_data,
    const std::vector<TriangulationEstimator::PoseData>& pose_data,
    std::vector<char>* inlier_mask, Eigen::Vector3d* xyz);

}  // namespace colmap

EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(
    colmap::TriangulationEstimator::PointData)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION_CUSTOM(
    colmap::TriangulationEstimator::PoseData)

#endif  // COLMAP_SRC_ESTIMATORS_TRIANGULATION_H_