科研
我的工作

代码分析

# 多子图重建结果合并

# 合并框架

void MergeClusters(
    const SceneClustering::Cluster& cluster,
    std::unordered_map<const SceneClustering::Cluster*, ReconstructionManager>*
        reconstruction_managers) {
  // Extract all reconstructions from all child clusters.
  std::vector<Reconstruction*> reconstructions;
  for (const auto& child_cluster : cluster.child_clusters) {
    if (!child_cluster.child_clusters.empty()) {
      MergeClusters(child_cluster, reconstruction_managers);
    }

    auto& reconstruction_manager = reconstruction_managers->at(&child_cluster);
    for (size_t i = 0; i < reconstruction_manager.Size(); ++i) {
      reconstructions.push_back(&reconstruction_manager.Get(i));
    }
  }

  // Try to merge all child cluster reconstruction.
  while (reconstructions.size() > 1) {
    bool merge_success = false;
    for (size_t i = 0; i < reconstructions.size(); ++i) {
      for (size_t j = 0; j < i; ++j) {
        const double kMaxReprojError = 8.0;
        if (reconstructions[i]->Merge(*reconstructions[j], kMaxReprojError)) {
          reconstructions.erase(reconstructions.begin() + j);
          merge_success = true;
          break;
        }
      }

      if (merge_success) {
        break;
      }
    }

    if (!merge_success) {
      break;
    }
  }

  // Insert a new reconstruction manager for merged cluster.
  auto& reconstruction_manager = (*reconstruction_managers)[&cluster];
  for (const auto& reconstruction : reconstructions) {
    reconstruction_manager.Add();
    reconstruction_manager.Get(reconstruction_manager.Size() - 1) =
        *reconstruction;
  }

  // Delete all merged child cluster reconstruction managers.
  for (const auto& child_cluster : cluster.child_clusters) {
    reconstruction_managers->erase(&child_cluster);
  }
}

该函数实现了将一个聚类中的所有子聚类的重建结果进行合并的功能。具体实现步骤如下：

遍历所有子聚类，将子聚类中的所有重建结果加入到一个 vector 中。
对于 vector 中的所有重建结果，尝试进行合并。合并的条件是重投影误差小于一个阈值 kMaxReprojError。
如果成功合并了两个重建结果，则将其中一个从 vector 中删除。
将合并后的重建结果加入到新的聚类中。
删除所有已经合并的子聚类的重建结果。

该函数使用了一个 unordered_map 来保存每个聚类对应的 ReconstructionManager。在遍历子聚类时，递归调用 MergeClusters 函数，将子聚类的重建结果加入到 unordered_map 中。在合并重建结果时，从 unordered_map 中获取对应的 ReconstructionManager。最后，将合并后的重建结果再次加入到 unordered_map 中，以便后续的合并操作。

# 采用LO-RANSAC计算相似变换

bool ComputeAlignmentBetweenReconstructions(
    const Reconstruction& src_reconstruction,
    const Reconstruction& ref_reconstruction,
    const double min_inlier_observations, const double max_reproj_error,
    Eigen::Matrix3x4d* alignment) {
  CHECK_GE(min_inlier_observations, 0.0);
  CHECK_LE(min_inlier_observations, 1.0);

  RANSACOptions ransac_options;
  ransac_options.max_error = 1.0 - min_inlier_observations;
  ransac_options.min_inlier_ratio = 0.2;

  LORANSAC<ReconstructionAlignmentEstimator, ReconstructionAlignmentEstimator>
      ransac(ransac_options);
  ransac.estimator.SetMaxReprojError(max_reproj_error);
  ransac.estimator.SetReconstructions(&src_reconstruction, &ref_reconstruction);
  ransac.local_estimator.SetMaxReprojError(max_reproj_error);
  ransac.local_estimator.SetReconstructions(&src_reconstruction,
                                            &ref_reconstruction);

  const auto& common_image_ids =
      src_reconstruction.FindCommonRegImageIds(ref_reconstruction);

  if (common_image_ids.size() < 3) {
    return false;
  }

  std::vector<const Image*> src_images(common_image_ids.size());
  std::vector<const Image*> ref_images(common_image_ids.size());
  for (size_t i = 0; i < common_image_ids.size(); ++i) {
    src_images[i] = &src_reconstruction.Image(common_image_ids[i]);
    ref_images[i] = &ref_reconstruction.Image(common_image_ids[i]);
  }

  const auto report = ransac.Estimate(src_images, ref_images);

  if (report.success) {
    *alignment = report.model;
  }

  return report.success;
}

该函数实现了计算两个重建结果之间的相对位姿的功能。具体实现步骤如下：

根据两个重建结果中共同观测到的图像，构建两个 vector，分别保存两个重建结果中对应的 Image 对象。
使用 LORANSAC 算法，通过这些共同的图像来计算两个重建结果之间的相对位姿。其中，使用 ReconstructionAlignmentEstimator 类来估计相对位姿。
如果计算成功，则将计算得到的相对位姿保存在 alignment 参数中。

该函数使用了 RANSACOptions 结构体来设置 LORANSAC 算法的参数，包括最大误差和最小内点比例。在计算相对位姿时，使用了 ReconstructionAlignmentEstimator 类来估计相对位姿。该类中实现了一个 ComputeError 函数，用于计算重建结果之间的重投影误差。如果重投影误差小于一个阈值 max_reproj_error，则将对应的观测点视为内点。在计算相对位姿时，使用了两个重建结果中共同观测到的图像。如果共同图像的数量小于 3，则无法计算相对位姿，返回 false。

# 计算两个点云的相似变换

# 具体融合操作

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::unordered_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;
}

这段代码实现了两个重建结果的合并操作。具体来说，它首先计算两个重建结果之间的对齐变换，然后将缺失的图像和点云添加到当前重建结果中。接下来，它根据一些规则将两个点云合并起来，包括将没有冲突轨迹的点复制到当前重建结果中，以及合并那些具有一对一映射的点。最后，它使用重投影误差过滤掉一些重建点。

# 场景图划分操作

cppvoid SceneClustering::PartitionFlatCluster(
    const std::vector<std::pair<int, int>>& edges,
    const std::vector<int>& weights) {
  CHECK_EQ(edges.size(), weights.size());

  // Partition the cluster using a normalized cut on the scene graph.
  const auto labels =
      ComputeNormalizedMinGraphCut(edges, weights, options_.branching);

  // Assign the images to the clustered child clusters.
  root_cluster_->child_clusters.resize(options_.branching);
  for (const auto image_id : root_cluster_->image_ids) {
    if (labels.count(image_id)) {
      auto& child_cluster =
          root_cluster_->child_clusters.at(labels.at(image_id));
      child_cluster.image_ids.push_back(image_id);
    }
  }

  // Sort child clusters by descending size of images and secondarily by lowest
  // image id.
  std::sort(root_cluster_->child_clusters.begin(),
            root_cluster_->child_clusters.end(),
            [](const Cluster& first, const Cluster& second) {
              return first.image_ids.size() >= second.image_ids.size() &&
                     *std::min_element(first.image_ids.begin(),
                                       first.image_ids.end()) <
                         *std::min_element(second.image_ids.begin(),
                                           second.image_ids.end());
            });

  // For each image find all related images with their weights
  std::unordered_map<int, std::vector<std::pair<int, int>>> related_images;
  for (size_t i = 0; i < edges.size(); ++i) {
    related_images[edges[i].first].emplace_back(edges[i].second, weights[i]);
    related_images[edges[i].second].emplace_back(edges[i].first, weights[i]);
  }

  // Sort related images by decreasing weights
  for (auto& image : related_images) {
    std::sort(image.second.begin(), image.second.end(),
              [](const std::pair<int, int>& first,
                 const std::pair<int, int>& second) {
                return first.second > second.second;
              });
  }

  // For each cluster add as many of the needed matching images up to
  // the max image overal allowance
  // We do the process sequentially for each image to ensure that at
  // least we get the best matches firat
  for (int i = 0; i < options_.branching; ++i) {
    auto& orig_image_ids = root_cluster_->child_clusters[i].image_ids;
    std::set<int> cluster_images(
        root_cluster_->child_clusters[i].image_ids.begin(),
        root_cluster_->child_clusters[i].image_ids.end());
    const size_t max_size = cluster_images.size() + options_.image_overlap;
    // check up to all the desired matches
    for (size_t j = 0; j < static_cast<size_t>(options_.num_image_matches) &&
                       cluster_images.size() < max_size;
         ++j) {
      for (const image_t image_id : orig_image_ids) {
        const auto& images = related_images[image_id];
        if (j >= images.size()) {
          continue;
        }
        // image not exists in cluster so we add it in the overlap set
        const int related_id = images[j].first;
        if (cluster_images.count(related_id) == 0) {
          cluster_images.insert(related_id);
        }
        if (cluster_images.size() >= max_size) {
          break;
        }
      }
    }
    orig_image_ids.clear();
    orig_image_ids.insert(orig_image_ids.end(), cluster_images.begin(),
                          cluster_images.end());
  }
}

这段代码实现了场景聚类中的子聚类划分操作。具体来说，它首先调用 ComputeNormalizedMinGraphCut 函数对场景图进行归一化切割，得到每个图像所属的子聚类标签。然后，它将每个图像分配给对应的子聚类，并按照子聚类中图像数量从大到小、图像 ID 从小到大的顺序对子聚类进行排序。接着，它对每个子聚类中的图像进行匹配，以确保每个子聚类中的图像都与其他子聚类中的图像有一定的重叠。具体来说，它首先根据场景图中的边和权重构建一个图像之间的关系图，然后对于每个子聚类，它从该子聚类中的图像出发，按照关系图中的权重从大到小的顺序，依次添加与该图像有关系的其他图像，直到该子聚类中的图像数量达到一定的阈值。最后，它将每个子聚类中的图像重新赋值给对应的子聚类。

← hierarchical 实现流程→