// Copyright (c), 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.

#include "colmap/estimators/triangulation.h"

#include "colmap/geometry/triangulation.h"
#include "colmap/optim/combination_sampler.h"
#include "colmap/optim/loransac.h"
#include "colmap/scene/projection.h"
#include "colmap/util/eigen_alignment.h"
#include "colmap/util/logging.h"

#include <algorithm>

#include <Eigen/Geometry>

namespace colmap {

TriangulationEstimator::TriangulationEstimator(double min_tri_angle,
                                               ResidualType residual_type)
    : min_tri_angle_(min_tri_angle), residual_type_(residual_type) {
  THROW_CHECK_GE(min_tri_angle, 0);
}

void TriangulationEstimator::Estimate(const std::vector<X_t>& point_data,
                                      const std::vector<Y_t>& pose_data,
                                      std::vector<M_t>* models) const {
  THROW_CHECK_GE(point_data.size(), 2);
  THROW_CHECK_EQ(point_data.size(), pose_data.size());
  THROW_CHECK(models != nullptr);

  models->clear();

  M_t xyz;
  if (point_data.size() == 2) {
    // More efficient closed-form solution for the two-view case.
    const bool all_cams_perspective =
        std::all_of(pose_data.begin(), pose_data.end(), [](const Y_t& pose) {
          return THROW_CHECK_NOTNULL(pose.camera)->IsPerspective();
        });
    if (all_cams_perspective) {
      if (!TriangulatePoint(
              pose_data[0].cam_from_world,
              pose_data[1].cam_from_world,
              Eigen::Vector2d(point_data[0].cam_ray.hnormalized()),
              Eigen::Vector2d(point_data[1].cam_ray.hnormalized()),
              &xyz)) {
        return;
      }
    } else {
      if (!TriangulatePoint(pose_data[0].cam_from_world,
                            pose_data[1].cam_from_world,
                            point_data[0].cam_ray,
                            point_data[1].cam_ray,
                            &xyz)) {
        return;
      }
    }

  } else {
    std::vector<Eigen::Matrix3x4d> cams_from_world(point_data.size());
    std::vector<Eigen::Vector3d> cam_rays(point_data.size());
    for (size_t i = 0; i < point_data.size(); ++i) {
      cams_from_world[i] = pose_data[i].cam_from_world;
      cam_rays[i] = point_data[i].cam_ray;
    }
    if (!TriangulateMultiViewPoint(
            span<const Eigen::Matrix3x4d>(cams_from_world.data(),
                                          cams_from_world.size()),
            span<const Eigen::Vector3d>(cam_rays.data(), cam_rays.size()),
            &xyz)) {
      return;
    }
  }

  // Cheirality. Perspective cameras require positive depth (the point in front
  // of the local +Z axis). Omnidirectional cameras (e.g. EQUIRECTANGULAR) have
  // no single front, but the point must still lie in the half-space the
  // observed bearing points toward.
  for (size_t i = 0; i < pose_data.size(); ++i) {
    if (pose_data[i].camera->IsPerspective()) {
      if (!HasPointPositiveDepth(pose_data[i].cam_from_world, xyz)) {
        return;
      }
    } else if ((pose_data[i].cam_from_world * xyz.homogeneous())
                   .dot(point_data[i].cam_ray) <= 0.0) {
      return;
    }
  }

  // Require a sufficient triangulation angle for at least one pair of views.
  for (size_t i = 0; i < pose_data.size(); ++i) {
    for (size_t j = 0; j < i; ++j) {
      if (CalculateTriangulationAngle(pose_data[i].proj_center,
                                      pose_data[j].proj_center,
                                      xyz) >= min_tri_angle_) {
        models->resize(1);
        (*models)[0] = xyz;
        return;
      }
    }
  }
}

void TriangulationEstimator::Residuals(const std::vector<X_t>& point_data,
                                       const std::vector<Y_t>& pose_data,
                                       const M_t& xyz,
                                       std::vector<double>* residuals) const {
  THROW_CHECK_EQ(point_data.size(), pose_data.size());

  residuals->resize(point_data.size());

  for (size_t i = 0; i < point_data.size(); ++i) {
    if (residual_type_ == ResidualType::REPROJECTION_ERROR) {
      (*residuals)[i] =
          CalculateSquaredReprojectionError(point_data[i].img_point,
                                            xyz,
                                            pose_data[i].cam_from_world,
                                            *pose_data[i].camera);
    } else if (residual_type_ == ResidualType::ANGULAR_ERROR) {
      const double angular_error = CalculateAngularReprojectionError(
          point_data[i].cam_ray, xyz, pose_data[i].cam_from_world);
      (*residuals)[i] = angular_error * angular_error;
    }
  }
}

bool EstimateTriangulation(const EstimateTriangulationOptions& options,
                           const std::vector<Eigen::Vector2d>& points,
                           const std::vector<Rigid3d>& cams_from_world,
                           const std::vector<Camera const*>& cameras,
                           std::vector<char>* inlier_mask,
                           Eigen::Vector3d* xyz) {
  THROW_CHECK_NOTNULL(inlier_mask);
  THROW_CHECK_NOTNULL(xyz);
  THROW_CHECK_GE(points.size(), 2);
  THROW_CHECK_EQ(points.size(), cams_from_world.size());
  THROW_CHECK_EQ(points.size(), cameras.size());
  options.Check();

  std::vector<TriangulationEstimator::PointData> point_data;
  point_data.resize(points.size());
  std::vector<TriangulationEstimator::PoseData> pose_data;
  pose_data.resize(points.size());
  for (size_t i = 0; i < points.size(); ++i) {
    point_data[i].img_point = points[i];
    // Unit bearing in the camera frame. CamRayFromImg yields a valid ray for
    // any camera model, including omnidirectional (EQUIRECTANGULAR)
    // back-hemisphere observations that CamFromImg cannot represent. Fall back
    // to a defined forward bearing (+Z) if unprojection fails, so downstream
    // normalize() in the DLT never sees a zero vector (which would produce
    // NaNs).
    point_data[i].cam_ray =
        cameras[i]->CamRayFromImg(points[i]).value_or(Eigen::Vector3d::UnitZ());
    pose_data[i].cam_from_world = cams_from_world[i].ToMatrix();
    pose_data[i].proj_center = cams_from_world[i].TgtOriginInSrc();
    pose_data[i].camera = cameras[i];
  }

  // Robustly estimate track using LORANSAC.
  LORANSAC<TriangulationEstimator,
           TriangulationEstimator,
           InlierSupportMeasurer,
           CombinationSampler>
      ransac(
          options.ransac_options,
          TriangulationEstimator(options.min_tri_angle, options.residual_type),
          TriangulationEstimator(options.min_tri_angle, options.residual_type));
  auto report = ransac.Estimate(point_data, pose_data);
  if (!report.success) {
    return false;
  }

  *inlier_mask = std::move(report.inlier_mask);
  *xyz = report.model;

  return report.success;
}

}  // namespace colmap
