text: // ===============================================================================================================
  // Copyright (c) 2019, Cornell University. 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 otice, 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 Cornell University 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: Kai Zhang (kz298@cornell.edu)
  //
  // The research is based upon work supported by the Office of the Director of National Intelligence (ODNI),     
  // Intelligence Advanced Research Projects Activity (IARPA), via DOI/IBC Contract Number D17PC00287.            
  // The U.S. Government is authorized to reproduce and distribute copies of this work for Governmental purposes. 
  // ===============================================================================================================
  //
  //
  // Copyright (c) 2018, 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)
  
  #include "base/reconstruction.h"
  
  #include <fstream>
  #include <iomanip>
  
  #include "base/database_cache.h"
  #include "base/pose.h"
  #include "base/projection.h"
  #include "base/similarity_transform.h"
  #include "base/triangulation.h"
  #include "estimators/similarity_transform.h"
  #include "optim/loransac.h"
  #include "util/bitmap.h"
  #include "util/misc.h"
  #include "util/ply.h"
  
  #define PRECISION 17
  
  namespace colmap {
  
  Reconstruction::Reconstruction()
      : correspondence_graph_(nullptr), num_added_points3D_(0) {}
  
  std::unordered_set<point3D_t> Reconstruction::Point3DIds() const {
    std::unordered_set<point3D_t> point3D_ids;
    point3D_ids.reserve(points3D_.size());
  
    for (const auto& point3D : points3D_) {
      point3D_ids.insert(point3D.first);
    }
  
    return point3D_ids;
  }
  
  void Reconstruction::Load(const DatabaseCache& database_cache) {
    correspondence_graph_ = nullptr;
  
    // Add cameras.
    cameras_.reserve(database_cache.NumCameras());
    for (const auto& camera : database_cache.Cameras()) {
      if (!ExistsCamera(camera.first)) {
        AddCamera(camera.second);
      }
      // Else: camera was added before, e.g. with `ReadAllCameras`.
    }
  
    // Add images.
    images_.reserve(database_cache.NumImages());
  
    for (const auto& image : database_cache.Images()) {
      if (ExistsImage(image.second.ImageId())) {
        class Image& existing_image = Image(image.second.ImageId());
        CHECK_EQ(existing_image.Name(), image.second.Name());
        if (existing_image.NumPoints2D() == 0) {
          existing_image.SetPoints2D(image.second.Points2D());
        } else {
          CHECK_EQ(image.second.NumPoints2D(), existing_image.NumPoints2D());
        }
        existing_image.SetNumObservations(image.second.NumObservations());
        existing_image.SetNumCorrespondences(image.second.NumCorrespondences());
      } else {
        AddImage(image.second);
      }
    }
  
    // Add image pairs.
    for (const auto& image_pair :
         database_cache.CorrespondenceGraph().NumCorrespondencesBetweenImages()) {
      image_pairs_[image_pair.first] = std::make_pair(0, image_pair.second);
    }
  }
  
  void Reconstruction::SetUp(const CorrespondenceGraph* correspondence_graph) {
    CHECK_NOTNULL(correspondence_graph);
    for (auto& image : images_) {
      image.second.SetUp(Camera(image.second.CameraId()));
    }
    correspondence_graph_ = correspondence_graph;
  
    // If an existing model was loaded from disk and there were already images
    // registered previously, we need to set observations as triangulated.
    for (const auto image_id : reg_image_ids_) {
      const class Image& image = Image(image_id);
      for (point2D_t point2D_idx = 0; point2D_idx < image.NumPoints2D();
           ++point2D_idx) {
        if (image.Point2D(point2D_idx).HasPoint3D()) {
          const bool kIsContinuedPoint3D = false;
          SetObservationAsTriangulated(image_id, point2D_idx,
                                       kIsContinuedPoint3D);
        }
      }
    }
  }
  
  void Reconstruction::TearDown() {
    correspondence_graph_ = nullptr;
  
    // Remove all not yet registered images.
    std::unordered_set<camera_t> keep_camera_ids;
    for (auto it = images_.begin(); it != images_.end();) {
      if (it->second.IsRegistered()) {
        keep_camera_ids.insert(it->second.CameraId());
        it->second.TearDown();
        ++it;
      } else {
        it = images_.erase(it);
      }
    }
  
    // Remove all unused cameras.
    for (auto it = cameras_.begin(); it != cameras_.end();) {
      if (keep_camera_ids.count(it->first) == 0) {
        it = cameras_.erase(it);
      } else {
        ++it;
      }
    }
  
    // Compress tracks.
    for (auto& point3D : points3D_) {
      point3D.second.Track().Compress();
    }
  }
  // @lin 读取内方位元素矩阵
  void Reconstruction::AddCamera(const class Camera& camera) {
    CHECK(!ExistsCamera(camera.CameraId()));
    CHECK(camera.VerifyParams());
    cameras_.emplace(camera.CameraId(), camera);
  }
  // @lin 读取外方位元素
  void Reconstruction::AddImage(const class Image& image) {
    CHECK(!ExistsImage(image.ImageId()));
    images_[image.ImageId()] = image;
  }
  // @lin 添加三维点信息
  point3D_t Reconstruction::AddPoint3D(const Eigen::Vector3d& xyz,
                                       const Track& track,
                                       const Eigen::Vector3ub& color) {
    const point3D_t point3D_id = ++num_added_points3D_;
    CHECK(!ExistsPoint3D(point3D_id));
  
    class Point3D& point3D = points3D_[point3D_id];
  
    point3D.SetXYZ(xyz);      //添加xyz坐标
    point3D.SetTrack(track);  //添加到track之中
    point3D.SetColor(color);  //设置点云的颜色
  
    for (const auto& track_el : track.Elements()) {
      class Image& image = Image(track_el.image_id);
      CHECK(!image.Point2D(track_el.point2D_idx).HasPoint3D());
      image.SetPoint3DForPoint2D(track_el.point2D_idx, point3D_id);
      CHECK_LE(image.NumPoints3D(), image.NumPoints2D());
    }
  
    const bool kIsContinuedPoint3D = false;
  
    for (const auto& track_el : track.Elements()) {
      SetObservationAsTriangulated(track_el.image_id, track_el.point2D_idx,
                                   kIsContinuedPoint3D);
    }
  
    return point3D_id;
  }
  // @lin 添加观测方程 3D点->3D点对应的二维点
  void Reconstruction::AddObservation(const point3D_t point3D_id,
                                      const TrackElement& track_el) {
    class Image& image = Image(track_el.image_id);//通过TrackElement获取三维点对应的相机的内方位元素
    CHECK(!image.Point2D(track_el.point2D_idx).HasPoint3D());
  
    image.SetPoint3DForPoint2D(track_el.point2D_idx, point3D_id);//设置两者的像点和物点的对应关系
    CHECK_LE(image.NumPoints3D(), image.NumPoints2D());
  
    class Point3D& point3D = Point3D(point3D_id);
    point3D.Track().AddElement(track_el);// 设置该物方点的track
  
    const bool kIsContinuedPoint3D = true;
    SetObservationAsTriangulated(track_el.image_id, track_el.point2D_idx,
                                 kIsContinuedPoint3D);
  }
  
  point3D_t Reconstruction::MergePoints3D(const point3D_t point3D_id1,
                                          const point3D_t point3D_id2) {
    const class Point3D& point3D1 = Point3D(point3D_id1);
    const class Point3D& point3D2 = Point3D(point3D_id2);
  
    const Eigen::Vector3d merged_xyz =
        (point3D1.Track().Length() * point3D1.XYZ() +
         point3D2.Track().Length() * point3D2.XYZ()) /
        (point3D1.Track().Length() + point3D2.Track().Length());
    const Eigen::Vector3d merged_rgb =
        (point3D1.Track().Length() * point3D1.Color().cast<double>() +
         point3D2.Track().Length() * point3D2.Color().cast<double>()) /
        (point3D1.Track().Length() + point3D2.Track().Length());
  
    Track merged_track;
    merged_track.Reserve(point3D1.Track().Length() + point3D2.Track().Length());
    merged_track.AddElements(point3D1.Track().Elements());
    merged_track.AddElements(point3D2.Track().Elements());
  
    DeletePoint3D(point3D_id1);
    DeletePoint3D(point3D_id2);
  
    const point3D_t merged_point3D_id =
        AddPoint3D(merged_xyz, merged_track, merged_rgb.cast<uint8_t>());
  
    return merged_point3D_id;
  }
  
  void Reconstruction::DeletePoint3D(const point3D_t point3D_id) {
    // Note: Do not change order of these instructions, especially with respect to
    // `Reconstruction::ResetTriObservations`
  
    const class Track& track = Point3D(point3D_id).Track();
  
    const bool kIsDeletedPoint3D = true;
  
    for (const auto& track_el : track.Elements()) {
      ResetTriObservations(track_el.image_id, track_el.point2D_idx,
                           kIsDeletedPoint3D);
    }
  
    for (const auto& track_el : track.Elements()) {
      class Image& image = Image(track_el.image_id);
      image.ResetPoint3DForPoint2D(track_el.point2D_idx);
    }
  
    points3D_.erase(point3D_id);
  }
  
  void Reconstruction::DeleteObservation(const image_t image_id,
                                         const point2D_t point2D_idx) {
    // Note: Do not change order of these instructions, especially with respect to
    // `Reconstruction::ResetTriObservations`
  
    class Image& image = Image(image_id);
    const point3D_t point3D_id = image.Point2D(point2D_idx).Point3DId();
    class Point3D& point3D = Point3D(point3D_id);
  
    if (point3D.Track().Length() <= 2) {
      DeletePoint3D(point3D_id);
      return;
    }
  
    point3D.Track().DeleteElement(image_id, point2D_idx);
  
    const bool kIsDeletedPoint3D = false;
    ResetTriObservations(image_id, point2D_idx, kIsDeletedPoint3D);
  
    image.ResetPoint3DForPoint2D(point2D_idx);
  }
  
  void Reconstruction::RegisterImage(const image_t image_id) {
    class Image& image = Image(image_id);
    if (!image.IsRegistered()) {
      image.SetRegistered(true);
      reg_image_ids_.push_back(image_id);
    }
  }
  
  void Reconstruction::DeRegisterImage(const image_t image_id) {
    class Image& image = Image(image_id);
  
    for (point2D_t point2D_idx = 0; point2D_idx < image.NumPoints2D();
         ++point2D_idx) {
      if (image.Point2D(point2D_idx).HasPoint3D()) {
        DeleteObservation(image_id, point2D_idx);
      }
    }
  
    image.SetRegistered(false);
  
    reg_image_ids_.erase(
        std::remove(reg_image_ids_.begin(), reg_image_ids_.end(), image_id),
        reg_image_ids_.end());
  }
  
  // @kai
  // void Reconstruction::Normalize(const double extent, const double p0,
  //                                const double p1, const bool use_images) {
  // }
  
  void Reconstruction::Normalize(const double extent, const double p0,
                                const double p1, const bool use_images, Eigen::Vector3d *translation_applied, double *scale_applied) {
      
   std::cout << "Reconstruction Normalization is called!!!!\n\n";
   
   CHECK_GT(extent, 0);
   CHECK_GE(p0, 0);
   CHECK_LE(p0, 1);
   CHECK_GE(p1, 0);
   CHECK_LE(p1, 1);
   CHECK_LE(p0, p1);
  
   if ((use_images && reg_image_ids_.size() < 2) ||
       (!use_images && points3D_.size() < 2)) {
     return;
   }
  
   EIGEN_STL_UMAP(class Image*, Eigen::Vector3d) proj_centers;
  
   for (size_t i = 0; i < reg_image_ids_.size(); ++i) {
     class Image& image = Image(reg_image_ids_[i]);
     const Eigen::Vector3d proj_center = image.ProjectionCenter();
     proj_centers[&image] = proj_center;
   }
  
   // Coordinates of image centers or point locations.
   std::vector<float> coords_x;
   std::vector<float> coords_y;
   std::vector<float> coords_z;
   if (use_images) {
     coords_x.reserve(proj_centers.size());
     coords_y.reserve(proj_centers.size());
     coords_z.reserve(proj_centers.size());
     for (const auto& proj_center : proj_centers) {
       coords_x.push_back(static_cast<float>(proj_center.second(0)));
       coords_y.push_back(static_cast<float>(proj_center.second(1)));
       coords_z.push_back(static_cast<float>(proj_center.second(2)));
     }
   } else {
     coords_x.reserve(points3D_.size());
     coords_y.reserve(points3D_.size());
     coords_z.reserve(points3D_.size());
     for (const auto& point3D : points3D_) {
       coords_x.push_back(static_cast<float>(point3D.second.X()));
       coords_y.push_back(static_cast<float>(point3D.second.Y()));
       coords_z.push_back(static_cast<float>(point3D.second.Z()));
     }
   }
  
   // Determine robust bounding box and mean.
  
   std::sort(coords_x.begin(), coords_x.end());
   std::sort(coords_y.begin(), coords_y.end());
   std::sort(coords_z.begin(), coords_z.end());
  
   const size_t P0 = static_cast<size_t>(
       (coords_x.size() > 3) ? p0 * (coords_x.size() - 1) : 0);
   const size_t P1 = static_cast<size_t>(
       (coords_x.size() > 3) ? p1 * (coords_x.size() - 1) : coords_x.size() - 1);
  
   const Eigen::Vector3d bbox_min(coords_x[P0], coords_y[P0], coords_z[P0]);
   const Eigen::Vector3d bbox_max(coords_x[P1], coords_y[P1], coords_z[P1]);
  
   Eigen::Vector3d mean_coord(0, 0, 0);
   for (size_t i = P0; i <= P1; ++i) {
     mean_coord(0) += coords_x[i];
     mean_coord(1) += coords_y[i];
     mean_coord(2) += coords_z[i];
   }
   mean_coord /= P1 - P0 + 1;
  
   // Calculate scale and translation, such that
   // translation is applied before scaling.
   const double old_extent = (bbox_max - bbox_min).norm();
   double scale;
   if (old_extent < std::numeric_limits<double>::epsilon()) {
     scale = 1;
   } else {
     scale = extent / old_extent;
   }
  
   const Eigen::Vector3d translation = mean_coord;
  
   // Transform images.
   for (auto& image_proj_center : proj_centers) {
     image_proj_center.second -= translation;
     image_proj_center.second *= scale;
     const Eigen::Quaterniond quat(
         image_proj_center.first->Qvec(0), image_proj_center.first->Qvec(1),
         image_proj_center.first->Qvec(2), image_proj_center.first->Qvec(3));
     image_proj_center.first->SetTvec(quat * -image_proj_center.second);
   }
  
   // Transform points.
   for (auto& point3D : points3D_) {
     point3D.second.XYZ() -= translation;
     point3D.second.XYZ() *= scale;
   }
  
   // save result
   if (translation_applied && scale_applied) {
      //std::cout << "writing the result ...";
      (*translation_applied)(0) = translation(0);
      (*translation_applied)(1) = translation(1);
      (*translation_applied)(2) = translation(2);
      *scale_applied = scale;
   }
  }
  
  void Reconstruction::Transform(const SimilarityTransform3& tform) {
    for (auto& image : images_) {
      tform.TransformPose(&image.second.Qvec(), &image.second.Tvec());
    }
    for (auto& point3D : points3D_) {
      tform.TransformPoint(&point3D.second.XYZ());
    }
  }
  
  bool Reconstruction::Merge(const Reconstruction& reconstruction,
                             const double max_reproj_error) {
    const double kMinInlierObservations = 0.3;
  
    Eigen::Matrix3x4d alignment;
    if (!ComputeAlignmentBetweenReconstructions(reconstruction, *this,
                                                kMinInlierObservations,
                                                max_reproj_error, &alignment)) {
      return false;
    }
  
    const SimilarityTransform3 tform(alignment);
  
    // Find common and missing images in the two reconstructions.
  
    std::unordered_set<image_t> common_image_ids;
    common_image_ids.reserve(reconstruction.NumRegImages());
    std::unordered_set<image_t> missing_image_ids;
    missing_image_ids.reserve(reconstruction.NumRegImages());
  
    for (const auto& image_id : reconstruction.RegImageIds()) {
      if (ExistsImage(image_id)) {
        common_image_ids.insert(image_id);
      } else {
        missing_image_ids.insert(image_id);
      }
    }
  
    // Register the missing images in this reconstruction.
  
    for (const auto image_id : missing_image_ids) {
      auto reg_image = reconstruction.Image(image_id);
      reg_image.SetRegistered(false);
      AddImage(reg_image);
      RegisterImage(image_id);
      if (!ExistsCamera(reg_image.CameraId())) {
        AddCamera(reconstruction.Camera(reg_image.CameraId()));
      }
      auto& image = Image(image_id);
      tform.TransformPose(&image.Qvec(), &image.Tvec());
    }
  
    // Merge the two point clouds using the following two rules:
    //    - copy points to this reconstruction with non-conflicting tracks,
    //      i.e. points that do not have an already triangulated observation
    //      in this reconstruction.
    //    - merge tracks that are unambiguous, i.e. only merge points in the two
    //      reconstructions if they have a one-to-one mapping.
    // Note that in both cases no cheirality or reprojection test is performed.
  
    for (const auto& point3D : reconstruction.Points3D()) {
      Track new_track;
      Track old_track;
      std::set<point3D_t> old_point3D_ids;
      for (const auto& track_el : point3D.second.Track().Elements()) {
        if (common_image_ids.count(track_el.image_id) > 0) {
          const auto& point2D =
              Image(track_el.image_id).Point2D(track_el.point2D_idx);
          if (point2D.HasPoint3D()) {
            old_track.AddElement(track_el);
            old_point3D_ids.insert(point2D.Point3DId());
          } else {
            new_track.AddElement(track_el);
          }
        } else if (missing_image_ids.count(track_el.image_id) > 0) {
          Image(track_el.image_id).ResetPoint3DForPoint2D(track_el.point2D_idx);
          new_track.AddElement(track_el);
        }
      }
  
      const bool create_new_point = new_track.Length() >= 2;
      const bool merge_new_and_old_point =
          (new_track.Length() + old_track.Length()) >= 2 &&
          old_point3D_ids.size() == 1;
      if (create_new_point || merge_new_and_old_point) {
        Eigen::Vector3d xyz = point3D.second.XYZ();
        tform.TransformPoint(&xyz);
        const auto point3D_id =
            AddPoint3D(xyz, new_track, point3D.second.Color());
        if (old_point3D_ids.size() == 1) {
          MergePoints3D(point3D_id, *old_point3D_ids.begin());
        }
      }
    }
  
    FilterPoints3DWithLargeReprojectionError(max_reproj_error, Point3DIds());
  
    return true;
  }
  
  bool Reconstruction::Align(const std::vector<std::string>& image_names,
                             const std::vector<Eigen::Vector3d>& locations,
                             const int min_common_images) {
    CHECK_GE(min_common_images, 3);
    CHECK_EQ(image_names.size(), locations.size());
  
    // Find out which images are contained in the reconstruction and get the
    // positions of their camera centers.
    std::set<image_t> common_image_ids;
    std::vector<Eigen::Vector3d> src;
    std::vector<Eigen::Vector3d> dst;
    for (size_t i = 0; i < image_names.size(); ++i) {
      const class Image* image = FindImageWithName(image_names[i]);
      if (image == nullptr) {
        continue;
      }
  
      if (!IsImageRegistered(image->ImageId())) {
        continue;
      }
  
      // Ignore duplicate images.
      if (common_image_ids.count(image->ImageId()) > 0) {
        continue;
      }
  
      common_image_ids.insert(image->ImageId());
      src.push_back(image->ProjectionCenter());
      dst.push_back(locations[i]);
    }
  
    // Only compute the alignment if there are enough correspondences.
    if (common_image_ids.size() < static_cast<size_t>(min_common_images)) {
      return false;
    }
  
    SimilarityTransform3 tform;
    tform.Estimate(src, dst);
    Transform(tform);
  
    return true;
  }
  
  bool Reconstruction::AlignRobust(const std::vector<std::string>& image_names,
                                   const std::vector<Eigen::Vector3d>& locations,
                                   const int min_common_images,
                                   const RANSACOptions& ransac_options) {
    CHECK_GE(min_common_images, 3);
    CHECK_EQ(image_names.size(), locations.size());
  
    // Find out which images are contained in the reconstruction and get the
    // positions of their camera centers.
    std::set<image_t> common_image_ids;
    std::vector<Eigen::Vector3d> src;
    std::vector<Eigen::Vector3d> dst;
    for (size_t i = 0; i < image_names.size(); ++i) {
      const class Image* image = FindImageWithName(image_names[i]);
      if (image == nullptr) {
        continue;
      }
  
      if (!IsImageRegistered(image->ImageId())) {
        continue;
      }
  
      // Ignore duplicate images.
      if (common_image_ids.count(image->ImageId()) > 0) {
        continue;
      }
  
      common_image_ids.insert(image->ImageId());
      src.push_back(image->ProjectionCenter());
      dst.push_back(locations[i]);
    }
  
    // Only compute the alignment if there are enough correspondences.
    if (common_image_ids.size() < static_cast<size_t>(min_common_images)) {
      return false;
    }
  
    LORANSAC<SimilarityTransformEstimator<3>, SimilarityTransformEstimator<3>>
        ransac(ransac_options);
  
    const auto report = ransac.Estimate(src, dst);
  
    if (report.support.num_inliers < static_cast<size_t>(min_common_images)) {
      return false;
    }
  
    Transform(SimilarityTransform3(report.model));
  
    return true;
  }
  
  const class Image* Reconstruction::FindImageWithName(
      const std::string& name) const {
    for (const auto& elem : images_) {
      if (elem.second.Name() == name) {
        return &elem.second;
      }
    }
    return nullptr;
  }
  
  std::vector<image_t> Reconstruction::FindCommonRegImageIds(
      const Reconstruction& reconstruction) const {
    std::vector<image_t> common_reg_image_ids;
    for (const auto image_id : reg_image_ids_) {
      if (reconstruction.ExistsImage(image_id) &&
          reconstruction.IsImageRegistered(image_id)) {
        CHECK_EQ(Image(image_id).Name(), reconstruction.Image(image_id).Name());
        common_reg_image_ids.push_back(image_id);
      }
    }
    return common_reg_image_ids;
  }
  
  size_t Reconstruction::FilterPoints3D(
      const double max_reproj_error, const double min_tri_angle,
      const std::unordered_set<point3D_t>& point3D_ids) {
    size_t num_filtered = 0;
    num_filtered +=
        FilterPoints3DWithLargeReprojectionError(max_reproj_error, point3D_ids);
    num_filtered +=
        FilterPoints3DWithSmallTriangulationAngle(min_tri_angle, point3D_ids);
    return num_filtered;
  }
  
  size_t Reconstruction::FilterPoints3DInImages(
      const double max_reproj_error, const double min_tri_angle,
      const std::unordered_set<image_t>& image_ids) {
    std::unordered_set<point3D_t> point3D_ids;
    for (const image_t image_id : image_ids) {
      const class Image& image = Image(image_id);
      for (const Point2D& point2D : image.Points2D()) {
        if (point2D.HasPoint3D()) {
          point3D_ids.insert(point2D.Point3DId());
        }
      }
    }
    return FilterPoints3D(max_reproj_error, min_tri_angle, point3D_ids);
  }
  
  size_t Reconstruction::FilterAllPoints3D(const double max_reproj_error,
                                           const double min_tri_angle) {
    // Important: First filter observations and points with large reprojection
    // error, so that observations with large reprojection error do not make
    // a point stable through a large triangulation angle.
    const std::unordered_set<point3D_t>& point3D_ids = Point3DIds();
    size_t num_filtered = 0;
    num_filtered +=
        FilterPoints3DWithLargeReprojectionError(max_reproj_error, point3D_ids);
    num_filtered +=
        FilterPoints3DWithSmallTriangulationAngle(min_tri_angle, point3D_ids);
    return num_filtered;
  }
  
  size_t Reconstruction::FilterObservationsWithNegativeDepth() {
    size_t num_filtered = 0;
    for (const auto image_id : reg_image_ids_) {
      const class Image& image = Image(image_id);
      const Eigen::Matrix3x4d proj_matrix = image.ProjectionMatrix();
      for (point2D_t point2D_idx = 0; point2D_idx < image.NumPoints2D();
           ++point2D_idx) {
        const Point2D& point2D = image.Point2D(point2D_idx);
        if (point2D.HasPoint3D()) {
          const class Point3D& point3D = Point3D(point2D.Point3DId());
          if (!HasPointPositiveDepth(proj_matrix, point3D.XYZ())) {
            DeleteObservation(image_id, point2D_idx);
            num_filtered += 1;
          }
        }
      }
    }
    return num_filtered;
  }
  
  std::vector<image_t> Reconstruction::FilterImages(
      const double min_focal_length_ratio, const double max_focal_length_ratio,
      const double max_extra_param) {
    std::vector<image_t> filtered_image_ids;
    for (const image_t image_id : RegImageIds()) {
      const class Image& image = Image(image_id);
      const class Camera& camera = Camera(image.CameraId());
      if (image.NumPoints3D() == 0) {
        filtered_image_ids.push_back(image_id);
      } else if (camera.HasBogusParams(min_focal_length_ratio,
                                       max_focal_length_ratio, max_extra_param)) {
        filtered_image_ids.push_back(image_id);
      }
    }
  
    // Only de-register after iterating over reg_image_ids_ to avoid
    // simultaneous iteration and modification of the vector.
    for (const image_t image_id : filtered_image_ids) {
      DeRegisterImage(image_id);
    }
  
    return filtered_image_ids;
  }
  
  size_t Reconstruction::ComputeNumObservations() const {
    size_t num_obs = 0;
    for (const image_t image_id : reg_image_ids_) {
      num_obs += Image(image_id).NumPoints3D();
    }
    return num_obs;
  }
  
  double Reconstruction::ComputeMeanTrackLength() const {
    if (points3D_.empty()) {
      return 0.0;
    } else {
      return ComputeNumObservations() / static_cast<double>(points3D_.size());
    }
  }
  
  double Reconstruction::ComputeMeanObservationsPerRegImage() const {
    if (reg_image_ids_.empty()) {
      return 0.0;
    } else {
      return ComputeNumObservations() /
             static_cast<double>(reg_image_ids_.size());
    }
  }
  
  double Reconstruction::ComputeMeanReprojectionError() const {
    double error_sum = 0.0;
    size_t num_valid_errors = 0;
    for (const auto& point3D : points3D_) {
      if (point3D.second.HasError()) {
        error_sum += point3D.second.Error();
        num_valid_errors += 1;
      }
    }
  
    if (num_valid_errors == 0) {
      return 0.0;
    } else {
      return error_sum / num_valid_errors;
    }
  }
  
  // @lin comment从不使用二进制文件，因为其不是二进制文件不直观
  // @kai never use the binary format that is not very intuitive
  void Reconstruction::Read(const std::string& path) {
  //   if (ExistsFile(JoinPaths(path, "cameras.bin")) &&
  //       ExistsFile(JoinPaths(path, "images.bin")) &&
  //       ExistsFile(JoinPaths(path, "points3D.bin"))) {
  //     ReadBinary(path);
  //   } else if (ExistsFile(JoinPaths(path, "cameras.txt")) &&
  //              ExistsFile(JoinPaths(path, "images.txt")) &&
  //              ExistsFile(JoinPaths(path, "points3D.txt"))) {
  //     ReadText(path);
  //   } else {
  //     LOG(FATAL) << "cameras, images, points3D files do not exist at " << path;
  //   }
  
    if (ExistsFile(JoinPaths(path, "cameras.txt")) &&
               ExistsFile(JoinPaths(path, "images.txt")) &&
               ExistsFile(JoinPaths(path, "points3D.txt"))) {
      ReadText(path);
    } else {
      LOG(FATAL) << "cameras, images, points3D files do not exist at " << path;
    }
  }
  
  // @lin 写入文本参数
  // @kai
  void Reconstruction::Write(const std::string& path) const { WriteText(path); }
  
  // @lin comment 读取相机内外参数以及3D点参数(是一种统一的参数)
  void Reconstruction::ReadText(const std::string& path) {
    ReadCamerasText(JoinPaths(path, "cameras.txt"));
    ReadImagesText(JoinPaths(path, "images.txt"));
    ReadPoints3DText(JoinPaths(path, "points3D.txt"));
  }
  // @lin comment 读取二进制文件
  void Reconstruction::ReadBinary(const std::string& path) {
    ReadCamerasBinary(JoinPaths(path, "cameras.bin"));
    ReadImagesBinary(JoinPaths(path, "images.bin"));
    ReadPoints3DBinary(JoinPaths(path, "points3D.bin"));
  }
  
  void Reconstruction::WriteText(const std::string& path) const {
    WriteCamerasText(JoinPaths(path, "cameras.txt"));
    WriteImagesText(JoinPaths(path, "images.txt"));
    WritePoints3DText(JoinPaths(path, "points3D.txt"));
  }
  
  void Reconstruction::WriteBinary(const std::string& path) const {
    WriteCamerasBinary(JoinPaths(path, "cameras.bin"));
    WriteImagesBinary(JoinPaths(path, "images.bin"));
    WritePoints3DBinary(JoinPaths(path, "points3D.bin"));
  }
  // @lin comment 转换到PLY文件
  std::vector<PlyPoint> Reconstruction::ConvertToPLY() const {
    std::vector<PlyPoint> ply_points;
    ply_points.reserve(points3D_.size());
  
    for (const auto& point3D : points3D_) {
      PlyPoint ply_point;
      ply_point.x = point3D.second.X();
      ply_point.y = point3D.second.Y();
      ply_point.z = point3D.second.Z();
      ply_point.r = point3D.second.Color(0);
      ply_point.g = point3D.second.Color(1);
      ply_point.b = point3D.second.Color(2);
      ply_points.push_back(ply_point);
    }
  
    return ply_points;
  }
  
  void Reconstruction::ImportPLY(const std::string& path) {
    points3D_.clear();
  
    const auto ply_points = ReadPly(path);
  
    points3D_.reserve(ply_points.size());
  
    for (const auto& ply_point : ply_points) {
      AddPoint3D(Eigen::Vector3d(ply_point.x, ply_point.y, ply_point.z), Track(),
                 Eigen::Vector3ub(ply_point.r, ply_point.g, ply_point.b));
    }
  }
  
  bool Reconstruction::ExportNVM(const std::string& path) const {
    std::ofstream file(path, std::ios::trunc);
    CHECK(file.is_open()) << path;
  
    // White space added for compatibility with Meshlab.
    file << "NVM_V3 " << std::endl << " " << std::endl;
    file << reg_image_ids_.size() << "  " << std::endl;
  
    std::unordered_map<image_t, size_t> image_id_to_idx_;
    size_t image_idx = 0;
  
    for (const auto image_id : reg_image_ids_) {
      const class Image& image = Image(image_id);
      const class Camera& camera = Camera(image.CameraId());
  
      if (camera.ModelId() != SimpleRadialCameraModel::model_id) {
        std::cout << "WARNING: NVM only supports `SIMPLE_RADIAL` camera model."
                  << std::endl;
        return false;
      }
  
      const double f =
          camera.Params(SimpleRadialCameraModel::focal_length_idxs[0]);
      const double k =
          -1 * camera.Params(SimpleRadialCameraModel::extra_params_idxs[0]);
      const Eigen::Vector3d proj_center = image.ProjectionCenter();
  
      file << image.Name() << " ";
      file << f << " ";
      file << image.Qvec(0) << " ";
      file << image.Qvec(1) << " ";
      file << image.Qvec(2) << " ";
      file << image.Qvec(3) << " ";
      file << proj_center(0) << " ";
      file << proj_center(1) << " ";
      file << proj_center(2) << " ";
      file << k << " ";
      file << 0 << std::endl;
  
      image_id_to_idx_[image_id] = image_idx;
      image_idx += 1;
    }
  
    file << std::endl << points3D_.size() << std::endl;
  
    for (const auto& point3D : points3D_) {
      file << point3D.second.XYZ()(0) << " ";
      file << point3D.second.XYZ()(1) << " ";
      file << point3D.second.XYZ()(2) << " ";
      file << static_cast<int>(point3D.second.Color(0)) << " ";
      file << static_cast<int>(point3D.second.Color(1)) << " ";
      file << static_cast<int>(point3D.second.Color(2)) << " ";
  
      std::ostringstream line;
  
      std::unordered_set<image_t> image_ids;
      for (const auto& track_el : point3D.second.Track().Elements()) {
        // Make sure that each point only has a single observation per image,
        // since VisualSfM does not support with multiple observations.
        if (image_ids.count(track_el.image_id) == 0) {
          const class Image& image = Image(track_el.image_id);
          const Point2D& point2D = image.Point2D(track_el.point2D_idx);
          line << image_id_to_idx_[track_el.image_id] << " ";
          line << track_el.point2D_idx << " ";
          line << point2D.X() << " ";
          line << point2D.Y() << " ";
          image_ids.insert(track_el.image_id);
        }
      }
  
      std::string line_string = line.str();
      line_string = line_string.substr(0, line_string.size() - 1);
  
      file << image_ids.size() << " ";
      file << line_string << std::endl;
    }
  
    return true;
  }
  
  bool Reconstruction::ExportBundler(const std::string& path,
                                     const std::string& list_path) const {
    std::ofstream file(path, std::ios::trunc);
    CHECK(file.is_open()) << path;
  
    std::ofstream list_file(list_path, std::ios::trunc);
    CHECK(list_file.is_open()) << list_path;
  
    file << "# Bundle file v0.3" << std::endl;
  
    file << reg_image_ids_.size() << " " << points3D_.size() << std::endl;
  
    std::unordered_map<image_t, size_t> image_id_to_idx_;
    size_t image_idx = 0;
  
    for (const image_t image_id : reg_image_ids_) {
      const class Image& image = Image(image_id);
      const class Camera& camera = Camera(image.CameraId());
  
      double f;
      double k1;
      double k2;
      if (camera.ModelId() == SimplePinholeCameraModel::model_id ||
          camera.ModelId() == PinholeCameraModel::model_id) {
        f = camera.MeanFocalLength();
        k1 = 0.0;
        k2 = 0.0;
      } else if (camera.ModelId() == SimpleRadialCameraModel::model_id) {
        f = camera.Params(SimpleRadialCameraModel::focal_length_idxs[0]);
        k1 = camera.Params(SimpleRadialCameraModel::extra_params_idxs[0]);
        k2 = 0.0;
      } else if (camera.ModelId() == RadialCameraModel::model_id) {
        f = camera.Params(RadialCameraModel::focal_length_idxs[0]);
        k1 = camera.Params(RadialCameraModel::extra_params_idxs[0]);
        k2 = camera.Params(RadialCameraModel::extra_params_idxs[1]);
      } else {
        std::cout << "WARNING: Bundler only supports `SIMPLE_RADIAL` and "
                     "`RADIAL` camera models."
                  << std::endl;
        return false;
      }
  
      file << f << " " << k1 << " " << k2 << std::endl;
  
      const Eigen::Matrix3d R = image.RotationMatrix();
      file << R(0, 0) << " " << R(0, 1) << " " << R(0, 2) << std::endl;
      file << -R(1, 0) << " " << -R(1, 1) << " " << -R(1, 2) << std::endl;
      file << -R(2, 0) << " " << -R(2, 1) << " " << -R(2, 2) << std::endl;
  
      file << image.Tvec(0) << " ";
      file << -image.Tvec(1) << " ";
      file << -image.Tvec(2) << std::endl;
  
      list_file << image.Name() << std::endl;
  
      image_id_to_idx_[image_id] = image_idx;
      image_idx += 1;
    }
  
    for (const auto& point3D : points3D_) {
      file << point3D.second.XYZ()(0) << " ";
      file << point3D.second.XYZ()(1) << " ";
      file << point3D.second.XYZ()(2) << std::endl;
  
      file << static_cast<int>(point3D.second.Color(0)) << " ";
      file << static_cast<int>(point3D.second.Color(1)) << " ";
      file << static_cast<int>(point3D.second.Color(2)) << std::endl;
  
      std::ostringstream line;
  
      line << point3D.second.Track().Length() << " ";
  
      for (const auto& track_el : point3D.second.Track().Elements()) {
        const class Image& image = Image(track_el.image_id);
        const class Camera& camera = Camera(image.CameraId());
  
        // Bundler output assumes image coordinate system origin
        // in the lower left corner of the image with the center of
        // the lower left pixel being (0, 0). Our coordinate system
        // starts in the upper left corner with the center of the
        // upper left pixel being (0.5, 0.5).
  
        const Point2D& point2D = image.Point2D(track_el.point2D_idx);
  
        line << image_id_to_idx_.at(track_el.image_id) << " ";
        line << track_el.point2D_idx << " ";
        line << point2D.X() - camera.PrincipalPointX() << " ";
        line << camera.PrincipalPointY() - point2D.Y() << " ";
      }
  
      std::string line_string = line.str();
      line_string = line_string.substr(0, line_string.size() - 1);
  
      file << line_string << std::endl;
    }
  
    return true;
  }
  
  void Reconstruction::ExportPLY(const std::string& path) const {
    const auto ply_points = ConvertToPLY();
  
    const bool kWriteNormal = false;
    const bool kWriteRGB = true;
    WriteBinaryPlyPoints(path, ply_points, kWriteNormal, kWriteRGB);
  }
  
  void Reconstruction::ExportVRML(const std::string& images_path,
                                  const std::string& points3D_path,
                                  const double image_scale,
                                  const Eigen::Vector3d& image_rgb) const {
    std::ofstream images_file(images_path, std::ios::trunc);
    CHECK(images_file.is_open()) << images_path;
  
    const double six = image_scale * 0.15;
    const double siy = image_scale * 0.1;
  
    std::vector<Eigen::Vector3d> points;
    points.emplace_back(-six, -siy, six * 1.0 * 2.0);
    points.emplace_back(+six, -siy, six * 1.0 * 2.0);
    points.emplace_back(+six, +siy, six * 1.0 * 2.0);
    points.emplace_back(-six, +siy, six * 1.0 * 2.0);
    points.emplace_back(0, 0, 0);
    points.emplace_back(-six / 3.0, -siy / 3.0, six * 1.0 * 2.0);
    points.emplace_back(+six / 3.0, -siy / 3.0, six * 1.0 * 2.0);
    points.emplace_back(+six / 3.0, +siy / 3.0, six * 1.0 * 2.0);
    points.emplace_back(-six / 3.0, +siy / 3.0, six * 1.0 * 2.0);
  
    for (const auto& image : images_) {
      if (!image.second.IsRegistered()) {
        continue;
      }
  
      images_file << "Shape{\n";
      images_file << " appearance Appearance {\n";
      images_file << "  material DEF Default-ffRffGffB Material {\n";
      images_file << "  ambientIntensity 0\n";
      images_file << "  diffuseColor "
                  << " " << image_rgb(0) << " " << image_rgb(1) << " "
                  << image_rgb(2) << "\n";
      images_file << "  emissiveColor 0.1 0.1 0.1 } }\n";
      images_file << " geometry IndexedFaceSet {\n";
      images_file << " solid FALSE \n";
      images_file << " colorPerVertex TRUE \n";
      images_file << " ccw TRUE \n";
  
      images_file << " coord Coordinate {\n";
      images_file << " point [\n";
  
      Eigen::Transform<double, 3, Eigen::Affine> transform;
      transform.matrix().topLeftCorner<3, 4>() =
          image.second.InverseProjectionMatrix();
  
      // Move camera base model to camera pose.
      for (size_t i = 0; i < points.size(); i++) {
        const Eigen::Vector3d point = transform * points[i];
        images_file << point(0) << " " << point(1) << " " << point(2) << "\n";
      }
  
      images_file << " ] }\n";
  
      images_file << "color Color {color [\n";
      for (size_t p = 0; p < points.size(); p++) {
        images_file << " " << image_rgb(0) << " " << image_rgb(1) << " "
                    << image_rgb(2) << "\n";
      }
  
      images_file << "\n] }\n";
  
      images_file << "coordIndex [\n";
      images_file << " 0, 1, 2, 3, -1\n";
      images_file << " 5, 6, 4, -1\n";
      images_file << " 6, 7, 4, -1\n";
      images_file << " 7, 8, 4, -1\n";
      images_file << " 8, 5, 4, -1\n";
      images_file << " \n] \n";
  
      images_file << " texCoord TextureCoordinate { point [\n";
      images_file << "  1 1,\n";
      images_file << "  0 1,\n";
      images_file << "  0 0,\n";
      images_file << "  1 0,\n";
      images_file << "  0 0,\n";
      images_file << "  0 0,\n";
      images_file << "  0 0,\n";
      images_file << "  0 0,\n";
      images_file << "  0 0,\n";
  
      images_file << " ] }\n";
      images_file << "} }\n";
    }
  
    // Write 3D points
  
    std::ofstream points3D_file(points3D_path, std::ios::trunc);
    CHECK(points3D_file.is_open()) << points3D_path;
  
    points3D_file << "#VRML V2.0 utf8\n";
    points3D_file << "Background { skyColor [1.0 1.0 1.0] } \n";
    points3D_file << "Shape{ appearance Appearance {\n";
    points3D_file << " material Material {emissiveColor 1 1 1} }\n";
    points3D_file << " geometry PointSet {\n";
    points3D_file << " coord Coordinate {\n";
    points3D_file << "  point [\n";
  
    for (const auto& point3D : points3D_) {
      points3D_file << point3D.second.XYZ()(0) << ", ";
      points3D_file << point3D.second.XYZ()(1) << ", ";
      points3D_file << point3D.second.XYZ()(2) << std::endl;
    }
  
    points3D_file << " ] }\n";
    points3D_file << " color Color { color [\n";
  
    for (const auto& point3D : points3D_) {
      points3D_file << point3D.second.Color(0) / 255.0 << ", ";
      points3D_file << point3D.second.Color(1) / 255.0 << ", ";
      points3D_file << point3D.second.Color(2) / 255.0 << std::endl;
    }
  
    points3D_file << " ] } } }\n";
  }
  
  bool Reconstruction::ExtractColorsForImage(const image_t image_id,
                                             const std::string& path) {
    const class Image& image = Image(image_id);
  
    Bitmap bitmap;
    if (!bitmap.Read(JoinPaths(path, image.Name()))) {
      return false;
    }
  
    const Eigen::Vector3ub kBlackColor(0, 0, 0);
    for (const Point2D point2D : image.Points2D()) {
      if (point2D.HasPoint3D()) {
        class Point3D& point3D = Point3D(point2D.Point3DId());
        if (point3D.Color() == kBlackColor) {
          BitmapColor<float> color;
          // COLMAP assumes that the upper left pixel center is (0.5, 0.5).
          if (bitmap.InterpolateBilinear(point2D.X() - 0.5, point2D.Y() - 0.5,
                                         &color)) {
            const BitmapColor<uint8_t> color_ub = color.Cast<uint8_t>();
            point3D.SetColor(
                Eigen::Vector3ub(color_ub.r, color_ub.g, color_ub.b));
          }
        }
      }
    }
  
    return true;
  }
  
  void Reconstruction::ExtractColorsForAllImages(const std::string& path) {
    EIGEN_STL_UMAP(point3D_t, Eigen::Vector3d) color_sums;
    std::unordered_map<point3D_t, size_t> color_counts;
  
    for (size_t i = 0; i < reg_image_ids_.size(); ++i) {
      const class Image& image = Image(reg_image_ids_[i]);
      const std::string image_path = JoinPaths(path, image.Name());
  
      Bitmap bitmap;
      if (!bitmap.Read(image_path)) {
        std::cout << StringPrintf("Could not read image %s at path %s.",
                                  image.Name().c_str(), image_path.c_str())
                  << std::endl;
        continue;
      }
  
      for (const Point2D point2D : image.Points2D()) {
        if (point2D.HasPoint3D()) {
          BitmapColor<float> color;
          // COLMAP assumes that the upper left pixel center is (0.5, 0.5).
          if (bitmap.InterpolateBilinear(point2D.X() - 0.5, point2D.Y() - 0.5,
                                         &color)) {
            if (color_sums.count(point2D.Point3DId())) {
              Eigen::Vector3d& color_sum = color_sums[point2D.Point3DId()];
              color_sum(0) += color.r;
              color_sum(1) += color.g;
              color_sum(2) += color.b;
              color_counts[point2D.Point3DId()] += 1;
            } else {
              color_sums.emplace(point2D.Point3DId(),
                                 Eigen::Vector3d(color.r, color.g, color.b));
              color_counts.emplace(point2D.Point3DId(), 1);
            }
          }
        }
      }
    }
  
    const Eigen::Vector3ub kBlackColor = Eigen::Vector3ub::Zero();
    for (auto& point3D : points3D_) {
      if (color_sums.count(point3D.first)) {
        Eigen::Vector3d color =
            color_sums[point3D.first] / color_counts[point3D.first];
        color.unaryExpr(std::ptr_fun<double, double>(std::round));
        point3D.second.SetColor(color.cast<uint8_t>());
      } else {
        point3D.second.SetColor(kBlackColor);
      }
    }
  }
  
  void Reconstruction::CreateImageDirs(const std::string& path) const {
    std::set<std::string> image_dirs;
    for (const auto& image : images_) {
      const std::vector<std::string> name_split =
          StringSplit(image.second.Name(), "/");
      if (name_split.size() > 1) {
        std::string dir = path;
        for (size_t i = 0; i < name_split.size() - 1; ++i) {
          dir = JoinPaths(dir, name_split[i]);
          image_dirs.insert(dir);
        }
      }
    }
    for (const auto& dir : image_dirs) {
      CreateDirIfNotExists(dir);
    }
  }
  
  size_t Reconstruction::FilterPoints3DWithSmallTriangulationAngle(
      const double min_tri_angle,
      const std::unordered_set<point3D_t>& point3D_ids) {
    // Number of filtered points.
    size_t num_filtered = 0;
  
    // Minimum triangulation angle in radians.
    const double min_tri_angle_rad = DegToRad(min_tri_angle);
  
    // Cache for image projection centers.
    EIGEN_STL_UMAP(image_t, Eigen::Vector3d) proj_centers;
  
    for (const auto point3D_id : point3D_ids) {
      if (!ExistsPoint3D(point3D_id)) {
        continue;
      }
  
      const class Point3D& point3D = Point3D(point3D_id);
  
      // Calculate triangulation angle for all pairwise combinations of image
      // poses in the track. Only delete point if none of the combinations
      // has a sufficient triangulation angle.
      bool keep_point = false;
      for (size_t i1 = 0; i1 < point3D.Track().Length(); ++i1) {
        const image_t image_id1 = point3D.Track().Element(i1).image_id;
  
        Eigen::Vector3d proj_center1;
        if (proj_centers.count(image_id1) == 0) {
          const class Image& image1 = Image(image_id1);
          proj_center1 = image1.ProjectionCenter();
          proj_centers.emplace(image_id1, proj_center1);
        } else {
          proj_center1 = proj_centers.at(image_id1);
        }
  
        for (size_t i2 = 0; i2 < i1; ++i2) {
          const image_t image_id2 = point3D.Track().Element(i2).image_id;
          const Eigen::Vector3d proj_center2 = proj_centers.at(image_id2);
  
          const double tri_angle = CalculateTriangulationAngle(
              proj_center1, proj_center2, point3D.XYZ());
  
          if (tri_angle >= min_tri_angle_rad) {
            keep_point = true;
            break;
          }
        }
  
        if (keep_point) {
          break;
        }
      }
  
      if (!keep_point) {
        num_filtered += 1;
        DeletePoint3D(point3D_id);
      }
    }
  
    return num_filtered;
  }
  
  size_t Reconstruction::FilterPoints3DWithLargeReprojectionError(
      const double max_reproj_error,
      const std::unordered_set<point3D_t>& point3D_ids) {
    const double max_squared_reproj_error = max_reproj_error * max_reproj_error;
  
    // Number of filtered points.
    size_t num_filtered = 0;
  
    for (const auto point3D_id : point3D_ids) {
      if (!ExistsPoint3D(point3D_id)) {
        continue;
      }
  
      class Point3D& point3D = Point3D(point3D_id);
  
      if (point3D.Track().Length() < 2) {
        DeletePoint3D(point3D_id);
        num_filtered += point3D.Track().Length();
        continue;
      }
  
      double reproj_error_sum = 0.0;
  
      std::vector<TrackElement> track_els_to_delete;
  
      for (const auto& track_el : point3D.Track().Elements()) {
        const class Image& image = Image(track_el.image_id);
        const class Camera& camera = Camera(image.CameraId());
        const Point2D& point2D = image.Point2D(track_el.point2D_idx);
        const double squared_reproj_error = CalculateSquaredReprojectionError(
            point2D.XY(), point3D.XYZ(), image.Qvec(), image.Tvec(), camera);
        if (squared_reproj_error > max_squared_reproj_error) {
          track_els_to_delete.push_back(track_el);
        } else {
          reproj_error_sum += std::sqrt(squared_reproj_error);
        }
      }
  
      if (track_els_to_delete.size() >= point3D.Track().Length() - 1) {
        num_filtered += point3D.Track().Length();
        DeletePoint3D(point3D_id);
      } else {
        num_filtered += track_els_to_delete.size();
        for (const auto& track_el : track_els_to_delete) {
          DeleteObservation(track_el.image_id, track_el.point2D_idx);
        }
        point3D.SetError(reproj_error_sum / point3D.Track().Length());
      }
    }
  
    return num_filtered;
  }
  
  // @lin 获取每一个三维点所对应的track的反投影的平均几何误差
  // @kai
  void Reconstruction::UpdateReprojErr() {
  	const std::unordered_set<point3D_t>& point3D_ids = this->Point3DIds();
  	for (const auto point3D_id : point3D_ids) {
  		if (!ExistsPoint3D(point3D_id)) {
  			continue;
  		}
  
  		class Point3D& point3D = this->Point3D(point3D_id);
  
  		double reproj_error_sum = 0.0;
  	    for (const auto& track_el : point3D.Track().Elements()) {
  	      const class Image& image = this->Image(track_el.image_id);
  	      const class Camera& camera = this->Camera(image.CameraId());
  	      const Point2D& point2D = image.Point2D(track_el.point2D_idx);
  	      const double squared_reproj_error = CalculateSquaredReprojectionError(
  	          point2D.XY(), point3D.XYZ(), image.Qvec(), image.Tvec(), camera);
  	      reproj_error_sum += std::sqrt(squared_reproj_error);
  	    }
  	    point3D.SetError(reproj_error_sum / point3D.Track().Length());
  	}
  }
  // @lin 读取相机文本，按照固定的格式进行读取
  /// ID MODEL WIDTH HEIGHT PARAMS(fx fy cx cy s)
  void Reconstruction::ReadCamerasText(const std::string& path) {
    cameras_.clear();
  
    std::ifstream file(path);
    CHECK(file.is_open()) << path;
  
    std::string line;
    std::string item;
  
    while (std::getline(file, line)) {
      StringTrim(&line);
  
      if (line.empty() || line[0] == '#') {
        continue;
      }
      // 使用C++文本流读取字符串
      std::stringstream line_stream(line);
  
      class Camera camera;
  
      // ID
      std::getline(line_stream, item, ' ');
      camera.SetCameraId(std::stoul(item));
  
      // MODEL
      std::getline(line_stream, item, ' ');
      camera.SetModelIdFromName(item);
  
      // WIDTH
      std::getline(line_stream, item, ' ');
      camera.SetWidth(std::stoll(item));
  
      // HEIGHT
      std::getline(line_stream, item, ' ');
      camera.SetHeight(std::stoll(item));
  
      // PARAMS
      camera.Params().clear();
      while (!line_stream.eof()) {
        std::getline(line_stream, item, ' ');
        camera.Params().push_back(std::stold(item));
      }
  
      CHECK(camera.VerifyParams());
  
      cameras_.emplace(camera.CameraId(), camera);
    }
  }
  
  void Reconstruction::ReadImagesText(const std::string& path) {
    images_.clear();
  
    std::ifstream file(path);
    CHECK(file.is_open()) << path;
  
    std::string line;
    std::string item;
  
    while (std::getline(file, line)) {
      StringTrim(&line);
  
      if (line.empty() || line[0] == '#') {
        continue;
      }
  
      std::stringstream line_stream1(line);
  
      // ID
      std::getline(line_stream1, item, ' ');
      const image_t image_id = std::stoul(item);
  
      class Image image;
      image.SetImageId(image_id);
  
      image.SetRegistered(true);
      reg_image_ids_.push_back(image_id);
  
      // QVEC (qw, qx, qy, qz)
      std::getline(line_stream1, item, ' ');
      image.Qvec(0) = std::stold(item);
  
      std::getline(line_stream1, item, ' ');
      image.Qvec(1) = std::stold(item);
  
      std::getline(line_stream1, item, ' ');
      image.Qvec(2) = std::stold(item);
  
      std::getline(line_stream1, item, ' ');
      image.Qvec(3) = std::stold(item);
  
      image.NormalizeQvec();
  
      // TVEC
      std::getline(line_stream1, item, ' ');
      image.Tvec(0) = std::stold(item);
  
      std::getline(line_stream1, item, ' ');
      image.Tvec(1) = std::stold(item);
  
      std::getline(line_stream1, item, ' ');
      image.Tvec(2) = std::stold(item);
  
      // CAMERA_ID
      std::getline(line_stream1, item, ' ');
      image.SetCameraId(std::stoul(item));
  
      // NAME
      std::getline(line_stream1, item, ' ');
      image.SetName(item);
  
      // POINTS2D
      if (!std::getline(file, line)) {
        break;
      }
  
      StringTrim(&line);
      std::stringstream line_stream2(line);
  
      std::vector<Eigen::Vector2d> points2D;
      std::vector<point3D_t> point3D_ids;
  
      if (!line.empty()) {
        while (!line_stream2.eof()) {
          Eigen::Vector2d point;
  
          std::getline(line_stream2, item, ' ');
          point.x() = std::stold(item);
  
          std::getline(line_stream2, item, ' ');
          point.y() = std::stold(item);
  
          points2D.push_back(point);
  
          std::getline(line_stream2, item, ' ');
          if (item == "-1") {
            point3D_ids.push_back(kInvalidPoint3DId);
          } else {
            point3D_ids.push_back(std::stoll(item));
          }
        }
      }
  
      image.SetUp(Camera(image.CameraId()));
      image.SetPoints2D(points2D);
  
      for (point2D_t point2D_idx = 0; point2D_idx < image.NumPoints2D();
           ++point2D_idx) {
        if (point3D_ids[point2D_idx] != kInvalidPoint3DId) {
          image.SetPoint3DForPoint2D(point2D_idx, point3D_ids[point2D_idx]);
        }
      }
  
      images_.emplace(image.ImageId(), image);
    }
  }
  
  void Reconstruction::ReadPoints3DText(const std::string& path) {
    points3D_.clear();
  
    std::ifstream file(path);
    CHECK(file.is_open()) << path;
  
    std::string line;
    std::string item;
  
    while (std::getline(file, line)) {
      StringTrim(&line);
  
      if (line.empty() || line[0] == '#') {
        continue;
      }
  
      std::stringstream line_stream(line);
  
      // ID
      std::getline(line_stream, item, ' ');
      const point3D_t point3D_id = std::stoll(item);
  
      // Make sure, that we can add new 3D points after reading 3D points
      // without overwriting existing 3D points.
      num_added_points3D_ = std::max(num_added_points3D_, point3D_id);
  
      class Point3D point3D;
  
      // XYZ
      std::getline(line_stream, item, ' ');
      point3D.XYZ(0) = std::stold(item);
  
      std::getline(line_stream, item, ' ');
      point3D.XYZ(1) = std::stold(item);
  
      std::getline(line_stream, item, ' ');
      point3D.XYZ(2) = std::stold(item);
  
      // Color
      std::getline(line_stream, item, ' ');
      point3D.Color(0) = static_cast<uint8_t>(std::stoi(item));
  
      std::getline(line_stream, item, ' ');
      point3D.Color(1) = static_cast<uint8_t>(std::stoi(item));
  
      std::getline(line_stream, item, ' ');
      point3D.Color(2) = static_cast<uint8_t>(std::stoi(item));
  
      // ERROR
      std::getline(line_stream, item, ' ');
      point3D.SetError(std::stold(item));
  
      // TRACK
      while (!line_stream.eof()) {
        TrackElement track_el;
  
        std::getline(line_stream, item, ' ');
        StringTrim(&item);
        if (item.empty()) {
          break;
        }
        track_el.image_id = std::stoul(item);
  
        std::getline(line_stream, item, ' ');
        track_el.point2D_idx = std::stoul(item);
  
        point3D.Track().AddElement(track_el);
      }
  
      point3D.Track().Compress();
  
      points3D_.emplace(point3D_id, point3D);
    }
  }
  
  void Reconstruction::ReadCamerasBinary(const std::string& path) {
    std::ifstream file(path, std::ios::binary);
    CHECK(file.is_open()) << path;
  
    const size_t num_cameras = ReadBinaryLittleEndian<uint64_t>(&file);
    for (size_t i = 0; i < num_cameras; ++i) {
      class Camera camera;
      camera.SetCameraId(ReadBinaryLittleEndian<camera_t>(&file));
      camera.SetModelId(ReadBinaryLittleEndian<int>(&file));
      camera.SetWidth(ReadBinaryLittleEndian<uint64_t>(&file));
      camera.SetHeight(ReadBinaryLittleEndian<uint64_t>(&file));
      ReadBinaryLittleEndian<double>(&file, &camera.Params());
      CHECK(camera.VerifyParams());
      cameras_.emplace(camera.CameraId(), camera);
    }
  }
  
  void Reconstruction::ReadImagesBinary(const std::string& path) {
    std::ifstream file(path, std::ios::binary);
    CHECK(file.is_open()) << path;
  
    const size_t num_reg_images = ReadBinaryLittleEndian<uint64_t>(&file);
    for (size_t i = 0; i < num_reg_images; ++i) {
      class Image image;
  
      image.SetImageId(ReadBinaryLittleEndian<image_t>(&file));
  
      image.Qvec(0) = ReadBinaryLittleEndian<double>(&file);
      image.Qvec(1) = ReadBinaryLittleEndian<double>(&file);
      image.Qvec(2) = ReadBinaryLittleEndian<double>(&file);
      image.Qvec(3) = ReadBinaryLittleEndian<double>(&file);
      image.NormalizeQvec();
  
      image.Tvec(0) = ReadBinaryLittleEndian<double>(&file);
      image.Tvec(1) = ReadBinaryLittleEndian<double>(&file);
      image.Tvec(2) = ReadBinaryLittleEndian<double>(&file);
  
      image.SetCameraId(ReadBinaryLittleEndian<camera_t>(&file));
  
      char name_char;
      do {
        file.read(&name_char, 1);
        if (name_char != '\0') {
          image.Name() += name_char;
        }
      } while (name_char != '\0');
  
      const size_t num_points2D = ReadBinaryLittleEndian<uint64_t>(&file);
  
      std::vector<Eigen::Vector2d> points2D;
      points2D.reserve(num_points2D);
      std::vector<point3D_t> point3D_ids;
      point3D_ids.reserve(num_points2D);
      for (size_t j = 0; j < num_points2D; ++j) {
        const double x = ReadBinaryLittleEndian<double>(&file);
        const double y = ReadBinaryLittleEndian<double>(&file);
        points2D.emplace_back(x, y);
        point3D_ids.push_back(ReadBinaryLittleEndian<point3D_t>(&file));
      }
  
      image.SetUp(Camera(image.CameraId()));
      image.SetPoints2D(points2D);
  
      for (point2D_t point2D_idx = 0; point2D_idx < image.NumPoints2D();
           ++point2D_idx) {
        if (point3D_ids[point2D_idx] != kInvalidPoint3DId) {
          image.SetPoint3DForPoint2D(point2D_idx, point3D_ids[point2D_idx]);
        }
      }
  
      image.SetRegistered(true);
      reg_image_ids_.push_back(image.ImageId());
  
      images_.emplace(image.ImageId(), image);
    }
  }
  
  void Reconstruction::ReadPoints3DBinary(const std::string& path) {
    std::ifstream file(path, std::ios::binary);
    CHECK(file.is_open()) << path;
  
    const size_t num_points3D = ReadBinaryLittleEndian<uint64_t>(&file);
    for (size_t i = 0; i < num_points3D; ++i) {
      class Point3D point3D;
  
      const point3D_t point3D_id = ReadBinaryLittleEndian<point3D_t>(&file);
      num_added_points3D_ = std::max(num_added_points3D_, point3D_id);
  
      point3D.XYZ()(0) = ReadBinaryLittleEndian<double>(&file);
      point3D.XYZ()(1) = ReadBinaryLittleEndian<double>(&file);
      point3D.XYZ()(2) = ReadBinaryLittleEndian<double>(&file);
      point3D.Color(0) = ReadBinaryLittleEndian<uint8_t>(&file);
      point3D.Color(1) = ReadBinaryLittleEndian<uint8_t>(&file);
      point3D.Color(2) = ReadBinaryLittleEndian<uint8_t>(&file);
      point3D.SetError(ReadBinaryLittleEndian<double>(&file));
  
      const size_t track_length = ReadBinaryLittleEndian<uint64_t>(&file);
      for (size_t j = 0; j < track_length; ++j) {
        const image_t image_id = ReadBinaryLittleEndian<image_t>(&file);
        const point2D_t point2D_idx = ReadBinaryLittleEndian<point2D_t>(&file);
        point3D.Track().AddElement(image_id, point2D_idx);
      }
      point3D.Track().Compress();
  
      points3D_.emplace(point3D_id, point3D);
    }
  }
  
  void Reconstruction::WriteCamerasText(const std::string& path) const {
    std::ofstream file(path, std::ios::trunc);
    CHECK(file.is_open()) << path;
  
      // set fulll precision
    file << std::setprecision(PRECISION);
    
    file << "# Camera list with one line of data per camera:" << std::endl;
    file << "#   CAMERA_ID, MODEL, WIDTH, HEIGHT, PARAMS[]" << std::endl;
    file << "# Number of cameras: " << cameras_.size() << std::endl;
  
    for (const auto& camera : cameras_) {
      std::ostringstream line;
  
      // set fulll precision
      line << std::setprecision(PRECISION);
        
      line << camera.first << " ";
      line << camera.second.ModelName() << " ";
      line << camera.second.Width() << " ";
      line << camera.second.Height() << " ";
  
      for (const double param : camera.second.Params()) {
        line << param << " ";
      }
  
      std::string line_string = line.str();
      line_string = line_string.substr(0, line_string.size() - 1);
  
      file << line_string << std::endl;
    }
  }
  
  void Reconstruction::WriteImagesText(const std::string& path) const {
    std::ofstream file(path, std::ios::trunc);
    CHECK(file.is_open()) << path;
    
      // set fulll precision
    file << std::setprecision(PRECISION);
      
    file << "# Image list with two lines of data per image:" << std::endl;
    file << "#   IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, "
            "NAME"
         << std::endl;
    file << "#   POINTS2D[] as (X, Y, POINT3D_ID)" << std::endl;
    file << "# Number of images: " << reg_image_ids_.size()
         << ", mean observations per image: "
         << ComputeMeanObservationsPerRegImage() << std::endl;
  
    for (const auto& image : images_) {
      if (!image.second.IsRegistered()) {
        continue;
      }
  
      std::ostringstream line;
      std::string line_string;
      
      // set fulll precision
      line << std::setprecision(PRECISION);
      
      line << image.first << " ";
  
      // QVEC (qw, qx, qy, qz)
      const Eigen::Vector4d normalized_qvec =
          NormalizeQuaternion(image.second.Qvec());
      line << normalized_qvec(0) << " ";
      line << normalized_qvec(1) << " ";
      line << normalized_qvec(2) << " ";
      line << normalized_qvec(3) << " ";
  
      // TVEC
      line << image.second.Tvec(0) << " ";
      line << image.second.Tvec(1) << " ";
      line << image.second.Tvec(2) << " ";
  
      line << image.second.CameraId() << " ";
  
      line << image.second.Name();
  
      file << line.str() << std::endl;
  
      line.str("");
      line.clear();
  
      for (const Point2D& point2D : image.second.Points2D()) {
        line << point2D.X() << " ";
        line << point2D.Y() << " ";
        if (point2D.HasPoint3D()) {
          line << point2D.Point3DId() << " ";
        } else {
          line << -1 << " ";
        }
      }
      line_string = line.str();
      line_string = line_string.substr(0, line_string.size() - 1);
      file << line_string << std::endl;
    }
  }
  
  void Reconstruction::WritePoints3DText(const std::string& path) const {
    std::ofstream file(path, std::ios::trunc);
    CHECK(file.is_open()) << path;
  
    // set fulll precision
    file << std::setprecision(PRECISION);
    
    file << "# 3D point list with one line of data per point:" << std::endl;
    file << "#   POINT3D_ID, X, Y, Z, R, G, B, ERROR, "
            "TRACK[] as (IMAGE_ID, POINT2D_IDX)"
         << std::endl;
    file << "# Number of points: " << points3D_.size()
         << ", mean track length: " << ComputeMeanTrackLength() << std::endl;
  
    for (const auto& point3D : points3D_) {
      file << point3D.first << " ";
      file << point3D.second.XYZ()(0) << " ";
      file << point3D.second.XYZ()(1) << " ";
      file << point3D.second.XYZ()(2) << " ";
      file << static_cast<int>(point3D.second.Color(0)) << " ";
      file << static_cast<int>(point3D.second.Color(1)) << " ";
      file << static_cast<int>(point3D.second.Color(2)) << " ";
      file << point3D.second.Error() << " ";
  
      std::ostringstream line;
      // set fulll precision
      line << std::setprecision(PRECISION);
      
      for (const auto& track_el : point3D.second.Track().Elements()) {
        line << track_el.image_id << " ";
        line << track_el.point2D_idx << " ";
      }
  
      std::string line_string = line.str();
      line_string = line_string.substr(0, line_string.size() - 1);
  
      file << line_string << std::endl;
    }
  }
  
  void Reconstruction::WriteCamerasBinary(const std::string& path) const {
    std::ofstream file(path, std::ios::trunc | std::ios::binary);
    CHECK(file.is_open()) << path;
  
    WriteBinaryLittleEndian<uint64_t>(&file, cameras_.size());
  
    for (const auto& camera : cameras_) {
      WriteBinaryLittleEndian<camera_t>(&file, camera.first);
      WriteBinaryLittleEndian<int>(&file, camera.second.ModelId());
      WriteBinaryLittleEndian<uint64_t>(&file, camera.second.Width());
      WriteBinaryLittleEndian<uint64_t>(&file, camera.second.Height());
      for (const double param : camera.second.Params()) {
        WriteBinaryLittleEndian<double>(&file, param);
      }
    }
  }
  
  void Reconstruction::WriteImagesBinary(const std::string& path) const {
    std::ofstream file(path, std::ios::trunc | std::ios::binary);
    CHECK(file.is_open()) << path;
  
    WriteBinaryLittleEndian<uint64_t>(&file, reg_image_ids_.size());
  
    for (const auto& image : images_) {
      if (!image.second.IsRegistered()) {
        continue;
      }
  
      WriteBinaryLittleEndian<image_t>(&file, image.first);
  
      const Eigen::Vector4d normalized_qvec =
          NormalizeQuaternion(image.second.Qvec());
      WriteBinaryLittleEndian<double>(&file, normalized_qvec(0));
      WriteBinaryLittleEndian<double>(&file, normalized_qvec(1));
      WriteBinaryLittleEndian<double>(&file, normalized_qvec(2));
      WriteBinaryLittleEndian<double>(&file, normalized_qvec(3));
  
      WriteBinaryLittleEndian<double>(&file, image.second.Tvec(0));
      WriteBinaryLittleEndian<double>(&file, image.second.Tvec(1));
      WriteBinaryLittleEndian<double>(&file, image.second.Tvec(2));
  
      WriteBinaryLittleEndian<camera_t>(&file, image.second.CameraId());
  
      const std::string name = image.second.Name() + '\0';
      file.write(name.c_str(), name.size());
  
      WriteBinaryLittleEndian<uint64_t>(&file, image.second.NumPoints2D());
      for (const Point2D& point2D : image.second.Points2D()) {
        WriteBinaryLittleEndian<double>(&file, point2D.X());
        WriteBinaryLittleEndian<double>(&file, point2D.Y());
        WriteBinaryLittleEndian<point3D_t>(&file, point2D.Point3DId());
      }
    }
  }
  
  void Reconstruction::WritePoints3DBinary(const std::string& path) const {
    std::ofstream file(path, std::ios::trunc | std::ios::binary);
    CHECK(file.is_open()) << path;
  
    WriteBinaryLittleEndian<uint64_t>(&file, points3D_.size());
  
    for (const auto& point3D : points3D_) {
      WriteBinaryLittleEndian<point3D_t>(&file, point3D.first);
      WriteBinaryLittleEndian<double>(&file, point3D.second.XYZ()(0));
      WriteBinaryLittleEndian<double>(&file, point3D.second.XYZ()(1));
      WriteBinaryLittleEndian<double>(&file, point3D.second.XYZ()(2));
      WriteBinaryLittleEndian<uint8_t>(&file, point3D.second.Color(0));
      WriteBinaryLittleEndian<uint8_t>(&file, point3D.second.Color(1));
      WriteBinaryLittleEndian<uint8_t>(&file, point3D.second.Color(2));
      WriteBinaryLittleEndian<double>(&file, point3D.second.Error());
  
      WriteBinaryLittleEndian<uint64_t>(&file, point3D.second.Track().Length());
      for (const auto& track_el : point3D.second.Track().Elements()) {
        WriteBinaryLittleEndian<image_t>(&file, track_el.image_id);
        WriteBinaryLittleEndian<point2D_t>(&file, track_el.point2D_idx);
      }
    }
  }
  
  void Reconstruction::SetObservationAsTriangulated(
      const image_t image_id, const point2D_t point2D_idx,
      const bool is_continued_point3D) {
    if (correspondence_graph_ == nullptr) {
      return;
    }
  
    const class Image& image = Image(image_id);
    const Point2D& point2D = image.Point2D(point2D_idx);
    const std::vector<CorrespondenceGraph::Correspondence>& corrs =
        correspondence_graph_->FindCorrespondences(image_id, point2D_idx);
  
    CHECK(image.IsRegistered());
    CHECK(point2D.HasPoint3D());
  
    for (const auto& corr : corrs) {
      class Image& corr_image = Image(corr.image_id);
      const Point2D& corr_point2D = corr_image.Point2D(corr.point2D_idx);
      corr_image.IncrementCorrespondenceHasPoint3D(corr.point2D_idx);
      // Update number of shared 3D points between image pairs and make sure to
      // only count the correspondences once (not twice forward and backward).
      if (point2D.Point3DId() == corr_point2D.Point3DId() &&
          (is_continued_point3D || image_id < corr.image_id)) {
        const image_pair_t pair_id =
            Database::ImagePairToPairId(image_id, corr.image_id);
        image_pairs_[pair_id].first += 1;
        CHECK_LE(image_pairs_[pair_id].first, image_pairs_[pair_id].second)
            << "The correspondence graph graph must not contain duplicate "
               "matches";
      }
    }
  }
  
  void Reconstruction::ResetTriObservations(const image_t image_id,
                                            const point2D_t point2D_idx,
                                            const bool is_deleted_point3D) {
    if (correspondence_graph_ == nullptr) {
      return;
    }
  
    const class Image& image = Image(image_id);
    const Point2D& point2D = image.Point2D(point2D_idx);
    const std::vector<CorrespondenceGraph::Correspondence>& corrs =
        correspondence_graph_->FindCorrespondences(image_id, point2D_idx);
  
    CHECK(image.IsRegistered());
    CHECK(point2D.HasPoint3D());
  
    for (const auto& corr : corrs) {
      class Image& corr_image = Image(corr.image_id);
      const Point2D& corr_point2D = corr_image.Point2D(corr.point2D_idx);
      corr_image.DecrementCorrespondenceHasPoint3D(corr.point2D_idx);
      // Update number of shared 3D points between image pairs and make sure to
      // only count the correspondences once (not twice forward and backward).
      if (point2D.Point3DId() == corr_point2D.Point3DId() &&
          (!is_deleted_point3D || image_id < corr.image_id)) {
        const image_pair_t pair_id =
            Database::ImagePairToPairId(image_id, corr.image_id);
        image_pairs_[pair_id].first -= 1;
        CHECK_GE(image_pairs_[pair_id].first, 0)
            << "The scene graph graph must not contain duplicate matches";
      }
    }
  }
  
  }  // namespace colmap
  