Theia源码剖析——GlobalSFM

TheiaSFM——GlobalSFM

global_sfm

估计相机位姿和结构的管道如下：
1）通过在形成三元组的旋转上强制使用循环约束来过滤不好的成对几何体
2）初始化焦距
3）估计每个摄像机的整体旋转
4）删除相对旋转与整体旋转不一致的所有成对几何
5）在已知旋转的情况下优化相对平移
6）过滤潜在的不良相对平移
7）估计位置
8）估算结构
9）Bundle Adjustment
10）重新三角化，并捆绑调整

在每个过滤步骤之后，我们将删除不再连接到视图图中最大连接的组件的所有视图。

class GlobalReconstructionEstimator : public ReconstructionEstimator {
 public:
  GlobalReconstructionEstimator(
      const ReconstructionEstimatorOptions& options);

  ReconstructionEstimatorSummary Estimate(ViewGraph* view_graph,
                                          Reconstruction* reconstruction);

 private:
  bool FilterInitialViewGraph();
  void CalibrateCameras();
  bool EstimateGlobalRotations();
  void FilterRotations();
  void OptimizePairwiseTranslations();
  void FilterRelativeTranslation();
  bool EstimatePosition();
  void EstimateStructure();
  bool BundleAdjustment();
  // Bundle adjust only the camera positions and points. The camera orientations
  // and intrinsics are held constant.
  bool BundleAdjustCameraPositionsAndPoints();

  ViewGraph* view_graph_;
  Reconstruction* reconstruction_;

  ReconstructionEstimatorOptions options_;
  FilterViewPairsFromRelativeTranslationOptions translation_filter_options_;
  BundleAdjustmentOptions bundle_adjustment_options_;
  RansacParameters ransac_params_;

  std::unordered_map<ViewId, Eigen::Vector3d> orientations_;
  std::unordered_map<ViewId, Eigen::Vector3d> positions_;

  DISALLOW_COPY_AND_ASSIGN(GlobalReconstructionEstimator);
};

来分析其中最重要的函数

ReconstructionEstimatorSummary GlobalReconstructionEstimator::Estimate(
    ViewGraph* view_graph, Reconstruction* reconstruction)

初始化相关参数

reconstruction_ = reconstruction;
view_graph_ = view_graph;
orientations_.clear();
positions_.clear();

STEP 1.过滤初始视图图形，并删除任何不好的两个视图几何跳转

if (!FilterInitialViewGraph()) {
  LOG(INFO) << "Insufficient view pairs to perform estimation.";
  return;
}

STEP 2.校准所有未校准的摄像机跳转
```
CalibrateCameras();
```

STEP 3.估计全局旋转跳转

if (!EstimateGlobalRotations()){
  return;
}

STEP 4.过滤不好的旋转跳转
```
FilterRotations();
```
STEP 5.优化相对位移跳转
```
OptimizePairwiseTranslations();
```
STEP 6.过滤不好的相对位移跳转
```
FilterRelativeTranslation();
```
STEP 7.估计全局位置跳转
```
if (!EstimatePosition()) {
  return;
}
```

在重建对象中设置位姿跳转

SetReconstructionFromEstimatedPoses(orientations_,
                                    positions_,
                                    reconstruction_);

始终进行一次三角剖分，然后根据重建估计量选项进行三角剖分并删除异常值

for (int i = 0; i < options_.num_retriangulation_iterations + 1; i++) {

STEP 8. 三角剖分特征跳转#在重建对象中设置位姿-SetReconstructionFromEstimatedPoses)
```
EstimateStructure();
SetUnderconstrainedAsUnestimated(reconstruction_);
```

进行一步BA调整，仅调整摄像机位置和3D点。仅在第一次BA调整迭代时才这样做跳转

if (i == 0 && options_.refine_camera_positions_and_points_after_position_estimation) 
{
	BundleAdjustCameraPositionsAndPoints();
}

STEP 9. BundleAdjustment 跳转
```
if (!BundleAdjustment()) {
  return;
}
```

剔除效果不好的轨道跳转

int num_points_removed = SetOutlierTracksToUnestimated(
    options_.max_reprojection_error_in_pixels,
    options_.min_triangulation_angle_degrees,
    reconstruction_);

来看相关函数具体是怎么实现的：

FilterInitialViewGraph

删除所有没有足够数量的内部视图的视图对

bool GlobalReconstructionEstimator::FilterInitialViewGraph() {
  
  std::unordered_set<ViewIdPair> view_pairs_to_remove;
  // 得到视图图的所有边的视图匹配对
  const auto& view_pairs = view_graph_->GetAllEdges();
  for (const auto& view_pair : view_pairs) {
    // 如果小于两视图的最小lnliers，则放入移除队列
    if (view_pair.second.num_verified_matches <
        options_.min_num_two_view_inliers) {
      view_pairs_to_remove.insert(view_pair.first);
    }
  }
  // 将移除队列的视图匹配在视图图中对应的边删除
  for (const ViewIdPair view_id_pair : view_pairs_to_remove) {
    view_graph_->RemoveEdge(view_id_pair.first, view_id_pair.second);
  }

  // 仅重建最大的连接组件
  RemoveDisconnectedViewPairs(view_graph_);
  // 经过以上操作，删除了很多视图，所以view_graph 的边和点发生了变化
  return view_graph_->NumEdges() >= 1;
}

RemoveDisconnectedViewPairs

删除所有不属于最大连接组件的视图对

std::unordered_set<ViewId> RemoveDisconnectedViewPairs(ViewGraph* view_graph) {
  CHECK_NOTNULL(view_graph);
  std::unordered_set<ViewId> removed_views;

  // 提取所有连接组件
  ConnectedComponents<ViewId> cc_extractor;
  // 得到视图图的所有边
  const auto& view_pairs = view_graph->GetAllEdges();
  // 将两视图的id加入到cc_extractor
  for (const auto& view_pair : view_pairs) {
    cc_extractor.AddEdge(view_pair.first.first, view_pair.first.second);
  }
  std::unordered_map<ViewId, std::unordered_set<ViewId> > connected_components;
  cc_extractor.Extract(&connected_components);

  // 查找最大的连接组件.
  int max_cc_size = 0;
  ViewId largest_cc_root_id = kInvalidViewId;
  // 遍历所有的连接组件
  for (const auto& connected_component : connected_components) {
    // 如果组件的孩子个数比最大的连接组件个数大
    if (connected_component.second.size() > max_cc_size) {
      // 更新最大连接组件个数
      max_cc_size = connected_component.second.size();
      // 将根节点设置为largest_cc_root_id
      largest_cc_root_id = connected_component.first;
    }
  }

  // 删除所有包含要删除的视图的视图对（即不在最大连接组件中的视图对）.
  const int num_view_pairs_before_filtering = view_graph->NumEdges();
  for (const auto& connected_component : connected_components) {
    // 跳过最大连接组件id
    if (connected_component.first == largest_cc_root_id) {
      continue;
    }

    // 注意：连接的组件也将包含根ID，因此我们不必明确删除connected_component.first，因为它将存在于connected_components.second中。
    for (const ViewId view_id2 : connected_component.second) {
      // 从视图图中删除不在最大连接组件中的view
      view_graph->RemoveView(view_id2);
      removed_views.insert(view_id2);
    }
  }

  const int num_removed_view_pairs =
      num_view_pairs_before_filtering - view_graph->NumEdges();
  LOG_IF(INFO, num_removed_view_pairs > 0)
      << num_removed_view_pairs
      << " view pairs were disconnected from the largest connected component "
         "of the view graph and were removed.";
  // 返回移除的视图
  return removed_views;
}

以上函数的流程是：

view_graph指一个视图的图，节点表示视图，边表示两个节点的连接关系（TwoViewInfo），获取view_graph的所有边即为获取整个重建的所有视图的匹配关系对。

将这些匹配关系对加入到连接组件即cc_extractor中，比如（1，3）、（2，4）、（3，5)..等匹配对，加入之后，会将所有有匹配关系的归为一个序列，即变为（1，3，5）、（2，4）..那么什么是最大的连接组件呢？通俗易懂的话讲即为有公共匹配关系对最多的视图序列，如果（1，3）（3，5）（5，7）为三个匹配视图对，那么（1，3，5，7）为一个匹配序列对，说明这四个视图匹配组有公共关系，为一个连接组件。那么最大连接组件就是找到以上连接组件的最长组件，即含有最多视图的组件。那么非最多视图的组件里的视图即为匹配关系较少的bad view，需要被删除。还是上面的例子，（1，3）（3，5）（5，7）（2，4），那么会产生两个组件，最长连接组件（1，3，5，7）和组件（2，4），可以看出视图2和视图4只有一个配对关系，与其他视图均无配对关系，所以这两个视图含有信息量较少，将这些非最长连接组件的视图全部删除，剩下的最长连接组件即为所求。

注意，这里需要特别讲一下ConnectedComponents。

ConnectedComponents可以理解为一个含有根节点和子节点的组合。

比如：看下面这段程序：

// 创建一个ConnectedComponents，内部存储int型数据
ConnectedComponents<int> connected_components;
// 为connected_components添加边
for(int i = 0; i < 9; i++)
{
    connected_components.AddEdge(i, i + 1);
}
// 建立一个set用于提取connected_components，类型为unordered_map<int, unordered_set<int>>
// int指根节点的值， unordered_set<int>是父节点和子节点的值
unordered_map<int, unordered_set<int>> disjoint_sets;
// 提取connected_components
connected_components.Extract(&disjoint_sets);

// 遍历每一个disjoint_sets
for(const auto& cc : disjoint_sets)
{
    // 输出根节点的值
    cout << cc.first << " ";
    // 依次输出子节点的值
    for(auto i : cc.second)
    {
        cout << i << " ";
    }
}
/*
1
0 0 1 2 3 4 5 6 7 8 9 
*/

可以看到，由于添加的边为（0，1）（1，2）（2，3）…（8，9），所以（1，2）的1可以作为（0，1）的子节点，同理（2，3）的2可以作为（1，2）的子节点，因此只有一个序列，父节点为0，子节点依次是1-9，需要特殊注意的是，unordered_set也存放父节点的值。

同理，看第二个程序：这个程序把上面的i+1变成了i+5

connected_components.AddEdge(i, i + 5);
/*
5
4 9 4 
2 12 7 2 
1 11 6 1 
0 10 5 0 
3 8 3 13 
*/

可以看到结果为5个序列。

由此我们可以发现规律，即ConnectedComponents添加新边时，检查添加的两个节点值，如果在已有序列中存在，则遵循以下规律

如果两个节点都不存在，则新开辟一个序列，如：

（3，5）、（1，7），新添加（2，4），则序列变为（3，5）、（1，7）、（2，4）
如果其中一个节点存在，一个节点不存在，则将其添加到存在的节点序列中，如：

（3，5）、（1，7），新添加（5，10），则序列变为（3，5，10）、（1，7）
如果两个节点都存在
- 如果两个节点存在在同一个序列中，如：
  
  （3，5）、（1，7），新添加（3，5），则原序列不变。
- 如果两个节点存在在不同序列中，则将两个序列合并，如：
  
  （3，5）、（1，7），新添加（5，7），则原序列变为（3，5，1，7）

CalibrateCameras

校准所有未校准的摄像机，具体介绍在增量式sfm中。

void GlobalReconstructionEstimator::CalibrateCameras() {
  SetCameraIntrinsicsFromPriors(reconstruction_);
}

SetCameraIntrinsicsFromPriors

从每个视图的CameraIntrinsicsPrior设置相机内部函数。事先没有焦距的视图将设置一个值，该值对应于中间视角。先验没有提供的主要点被简单地初始化为相应图像尺寸尺寸的一半。

void SetCameraIntrinsicsFromPriors(Reconstruction* reconstruction) {
  // 一次从一组先验设置摄像机内参
  const std::unordered_set<CameraIntrinsicsGroupId>
      camera_intrinsics_group_ids = reconstruction->CameraIntrinsicsGroupIds();
  for (const CameraIntrinsicsGroupId intrinsics_group_id :
       camera_intrinsics_group_ids) {
    // 获取此相机内在函数组中的所有视图
    const std::unordered_set<ViewId> views_in_intrinsics_group =
        reconstruction->GetViewsInCameraIntrinsicGroup(intrinsics_group_id);

    // 我们为内在函数组选择一个“代表性视图”。 我们从先验中为该视图设置内在函数，然后将同一内在函数组中所有其他视图的内在函数设置为指向代表性内在函数。 	由于使用了shared_ptrs，因此共享的内在函数将保持活动状态，直到该组中的所有摄影机都脱离上下文为止。
    const ViewId representative_view_id = InitializeRepresentativeCameraInGroup(
        views_in_intrinsics_group, reconstruction);
    // 设置代表性的view
    View* representative_view =
        reconstruction->MutableView(representative_view_id);
    Camera* representative_camera = representative_view->MutableCamera();
    representative_camera->SetFromCameraIntrinsicsPriors(
        representative_view->CameraIntrinsicsPrior());
    CHECK_NOTNULL(representative_camera->CameraIntrinsics().get());

    // 设置该组的所有内部函数
    for (const ViewId view_in_intrinsics_group : views_in_intrinsics_group) {
      View* view = reconstruction->MutableView(view_in_intrinsics_group);
      // 如果重建中不存在该视图或该视图是代表性视图，请跳过此视图。
      if (view == nullptr ||
          representative_view_id == view_in_intrinsics_group) {
        continue;
      }

      // 将视图的内在函数设置为指向共享的内在函数。 这包括估计的视图，这些视图可能具有与共享的内部函数不同的估计内部函数参数
      view->MutableCamera()->MutableCameraIntrinsics() =
          representative_camera->CameraIntrinsics();
      view->MutableCamera()->SetImageSize(representative_camera->ImageWidth(),
                                          representative_camera->ImageHeight());
    }
  }
}

未命名文件 (2)

EstimateGlobalRotations

估计全局旋转

bool GlobalReconstructionEstimator::EstimateGlobalRotations() {
  // 得到所有匹配对关系
  const auto& view_pairs = view_graph_->GetAllEdges();

  // 选择全局旋转估计类型
  // * ROBUST_L1L2
  // * NONLINEAR
  // * LINEAR
  std::unique_ptr<RotationEstimator> rotation_estimator;
  switch (options_.global_rotation_estimator_type) {
    // 根据给定的相对旋转和对全局方向的初始猜测来计算全局旋转。
    /*
    Chatterjee和Govindu提出的鲁棒算法“高效和大规模旋转平均”（ICCV 2013）用于获得对异常值鲁棒的精确解决方案。

	该算法的一般策略是先使L1最小化，然后再加权最小二乘，以最小化相对旋转误差（使用相对旋转和相应的全局旋转之间的差）。
    L1最小化速度相对较慢，但为异常值提供了出色的鲁棒性。 然后，L2最小化（速度更快）可以将解决方案精炼为非常准确的。
    */
    case GlobalRotationEstimatorType::ROBUST_L1L2: {
      // 通过沿着最大生成树行走来初始化方向估计
      OrientationsFromMaximumSpanningTree(*view_graph_, &orientations_);
      RobustRotationEstimator::Options robust_rotation_estimator_options;
      rotation_estimator.reset(
          new RobustRotationEstimator(robust_rotation_estimator_options));
      break;
    }
    case GlobalRotationEstimatorType::NONLINEAR: {
      // Initialize the orientation estimations by walking along the maximum
      // spanning tree.
      OrientationsFromMaximumSpanningTree(*view_graph_, &orientations_);
      rotation_estimator.reset(new NonlinearRotationEstimator());
      break;
    }
    case GlobalRotationEstimatorType::LINEAR: {
      // Set the constructor variable to true to weigh each term by the inlier
      // count.
      rotation_estimator.reset(new LinearRotationEstimator());
      break;
    }
    default: {
      LOG(FATAL) << "Invalid type of global rotation estimation chosen.";
      break;
    }
  }

  return rotation_estimator->EstimateRotations(view_pairs, &orientations_);
}

OrientationsFromMaximumSpanningTree(*view_graph_, &orientations_)：

通过计算最大生成树（按边缘权重）并通过链接旋转求解全局方向来计算视图图中每个视图的方向。仅针对视图图中最大的连接部分估计方向。

一、什么是最大生成树：

　　在一个图的所有生成树中边权值和最大的生成树即为最大生成树。

二、怎么生成：

　　1、将图中所有边的边权变为相反数，再跑一遍最小生成树算法。相反数最小，原数就最大。

　　2、修改一下最小生成树算法：对于kruskal，将“从小到大排序”改为“从大到小排序”；

　　　　　　　　　　　　　　　对于prim，将“每次选到所有蓝点代价最小的白点”改为“每次选到所有蓝点代价最大的点”。

orientations 即对应每个视图的方向，即为相机方向，也即全局旋转。

这个函数比较难懂，来逐一分析：

定义一个unordered_set类型的largest_cc作为最大连接组件，最大连接组件的定义在上个函数中有详细讲解。

从传入的view_graph中得到最大的连接组件id传入largest_cc中。

定义一个最大连接组件子图，从视图图view_graph中，选取最大连接组件相关的子图。（通俗来讲，视图图中存储的顶点为所有的视图id，边为两者之间的视图关系TwoViewInfo，通过最大连接组件得到最大连接的视图id序列，将序列中的id相关的边和点的子图提取出来，舍弃非最大连接组件的视图点和匹配关系边，仅保留最大连接组件的子视图）。

得到最大连接子图的所有边值（所有的TwoViewInfo），定义一个克鲁斯卡尔算法的最小生成树mst_extractor。

遍历最大连接子图的所有边all_edges，注意这里的edge的类型为<ViewIdPair, TwoViewInfo>，那么两视图的id分别为edge.first.first和edge.first.second，将其作为顶点加入到mst_extractor中，由于是最大生成树，所以权重取两视图的匹配数的相反数。

定义一个存放ViewIdPair的集合mst，从mst_extractor中提取最小生成树存放在mst中。注意，之前的最大连接组件虽然视图是唯一的，但是其中的匹配关系不是唯一的，通过最小生成树提取，获得一个唯一的视图匹配关系。

建立一个最小生成树子图，存放唯一视图匹配关系的id和两视图信息TwoViewInfo。（这里就一共涉及到了三个图，1、视图图，2、最大连接子图，3、最大连接子图的最小生成树图）

定义一个heap，类型为vector，这里的HeapElement实际上是typedef pair<TwoViewInfo, ViewIdPair>，注意和前面的edge的类型<ViewIdPair, TwoViewInfo>区分，一个是TwoViewInfo做键ViewIdPair做值，一个正好相反。

设置根视图id，也就是初始视图的id为mst.begin()->first。将根视图的方向（即选做第一个视图的摄像机的旋转）设置为零矩阵。

将view_id的所有边缘添加到堆中。仅添加尚未具有方向估计的边缘。步骤为：得到view_id的邻居视图id，遍历所有的邻居id，如果邻居视图已经有方向了，跳过，否则将view 和邻居两个视图的信息和两个视图id添加到heap中，然后调整堆为小顶堆。

while循环，当堆不为空时，得到堆顶元素，因为堆顶元素已经刚刚设置了方向，所以将其pop掉，然后第二个视图的方向，直到全部计算完。

bool OrientationsFromMaximumSpanningTree(
    const ViewGraph& view_graph,
    std::unordered_map<ViewId, Eigen::Vector3d>* orientations) {
  CHECK_NOTNULL(orientations);

  // 因为MST仅在单个连接的组件上有效，所以计算输入视图图中最大的连接的组件
  std::unordered_set<theia::ViewId> largest_cc;
  // 得到最大连接组件
  view_graph.GetLargestConnectedComponentIds(&largest_cc);
  ViewGraph largest_cc_subgraph;
  // 从view_graph中提取最大连接组件largest_cc的子图largest_cc_subgraph（最大连接子图）
  view_graph.ExtractSubgraph(largest_cc, &largest_cc_subgraph);

  // 计算最大生成树
    
  // 得到最大连接子图的所有边
  //  const std::unordered_map<ViewIdPair, TwoViewInfo>& GetAllEdges() const;
  const auto& all_edges = largest_cc_subgraph.GetAllEdges();
  
  // 使用Kruskal的贪婪算法提取图的最小生成树的类。 最小生成树是一个子图，它包含图中的所有节点，并且仅包含以最小边权重总和连接这些节点的边。 该算法在O（E * log（V））中运行，其中E是边数，V是图中的节点数。
  MinimumSpanningTree<ViewId, int> mst_extractor;
  // 遍历所有的边值
  for (const auto& edge : all_edges) {
    // 由于我们需要最大生成树，因此可以在最小生成树提取器中取消所有边缘权重 
    /*
    void AddEdge(const T& node1, const T& node2, const V& weight) {
    	edges_.emplace_back(weight, std::pair<T, T>(node1, node2));
  	}
    */
    // 给mst_extractor添加边
    mst_extractor.AddEdge(
        edge.first.first, edge.first.second, -edge.second.num_verified_matches);
  }

  std::unordered_set<ViewIdPair> mst;
  // 提取最小生成树。 成功返回true，失败返回false。 如果返回true，则输出变量包含最小生成树的边缘列表。
  if (!mst_extractor.Extract(&mst)) {
    VLOG(2)
        << "Could not extract the maximum spanning tree from the view graph";
    return false;
  }

  // 创建一个MST视图图
  ViewGraph mst_view_graph;
  for (const ViewIdPair& edge : mst) {
    mst_view_graph.AddEdge(
        edge.first,
        edge.second,
        *largest_cc_subgraph.GetEdge(edge.first, edge.second));
  }

  // 将相对旋转链接在一起以计算方向。 我们使用堆来确定要添加到最小生成树的下一条边
    
  // typedef std::pair<TwoViewInfo, ViewIdPair> HeapElement;
  std::vector<HeapElement> heap;

  // 设置根值
  const ViewId root_view_id = mst.begin()->first;
  (*orientations)[root_view_id] = Eigen::Vector3d::Zero();
  
  // 将view_id的所有边缘添加到堆中。 仅添加尚未具有方向估计的边缘
  AddEdgesToHeap(mst_view_graph, *orientations, root_view_id, &heap);

  while (!heap.empty()) {
    const HeapElement next_edge = heap.front();
    // 去除最佳边缘
      
    // 将堆顶(所给范围的最前面)元素移动到所给范围的最后，并且将新的最大值置于所给范围的最前面
    std::pop_heap(heap.begin(), heap.end(), SortHeapElement);
    heap.pop_back();

    // 如果边缘包含两个已经添加的顶点，则不执行任何操作
    if (ContainsKey(*orientations, next_edge.second.second)) {
      continue;
    }

    // 计算顶点的方向
    (*orientations)[next_edge.second.second] =
        ComputeOrientation(FindOrDie(*orientations, next_edge.second.first),
                           next_edge.first,
                           next_edge.second.first,
                           next_edge.second.second);

     // 将所有边缘添加到堆中
    AddEdgesToHeap(
        mst_view_graph, *orientations, next_edge.second.second, &heap);
  }
  return true;
}

void AddEdgesToHeap(
    const ViewGraph& view_graph,
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientations,
    const ViewId view_id,
    std::vector<HeapElement>* heap) {
  // 得到view_id的邻居视图id
  const std::unordered_set<ViewId>* edge_ids =
      view_graph.GetNeighborIdsForView(view_id);
  // 遍历邻居视图id
  for (const ViewId edge_id : *edge_ids) {
    // 如果邻居视图id已经有方向了，即邻居视图的摄像机有旋转了，则跳过。
    if (ContainsKey(orientations, edge_id)) {
      continue;
    }

    // 将view_id和邻居id(edge_id)和两者的信息ViewInfo添加到heap中
    heap->emplace_back(*view_graph.GetEdge(view_id, edge_id),
                       ViewIdPair(view_id, edge_id));
    // push_heap()是向堆中插入一个元素
    /*
    bool SortHeapElement(const HeapElement& h1, const HeapElement& h2) {
      return h1.first.num_verified_matches > h2.first.num_verified_matches;
    }
    */
    std::push_heap(heap->begin(), heap->end(), SortHeapElement);
  }
}

rotation_estimator->EstimateRotations(view_pairs, &orientations_);

根据初始猜测估计所有视图的整体方向。成功估算时返回true，否则返回false。

bool RobustRotationEstimator::EstimateRotations(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    std::unordered_map<ViewId, Eigen::Vector3d>* global_orientations) {
  // 遍历所有的view_pairs
  for (const auto& view_pair : view_pairs) {
    // view_pair.second.rotation_2是视图对第二个视图的旋转
    AddRelativeRotationConstraint(view_pair.first, view_pair.second.rotation_2);
  }
  return EstimateRotations(global_orientations);
}

AddRelativeRotationConstraint

一个替代接口是使用AddRelativeRotationConstraint逐个添加相对旋转约束，然后调用下面的EstimateRotations接口。这允许调用者为同一视图id对添加多个约束，这可能导致更准确的旋转估算。

"Parallel Structure from Motion from Local Increment to Global Averaging" by Zhu et al (Arxiv 2017). https://arxiv.org/abs/1702.08601

void RobustRotationEstimator::AddRelativeRotationConstraint(
    const ViewIdPair& view_id_pair, const Eigen::Vector3d& relative_rotation) {
  // 存储相对方向约束
  relative_rotations_.emplace_back(view_id_pair, relative_rotation);
}

bool RobustRotationEstimator::EstimateRotations(
    std::unordered_map<ViewId, Eigen::Vector3d>* global_orientations) {
  
  // 用于计算整体旋转的成对相对旋转
  CHECK_GT(relative_rotations_.size(), 0)
      << "Relative rotation constraints must be added to the robust rotation "
         "solver before estimating global rotations.";
  global_orientations_ = CHECK_NOTNULL(global_orientations);

  // 计算视图id到线性系统中索引的映射。 第一个旋转的索引为-1，不会添加到线性系统中。 这将消除测量仪的自由度（有效地将一台摄像机作为identity rotation）
  int index = -1;
  view_id_to_index_.reserve(global_orientations->size());
  for (const auto& orientation : *global_orientations) {
    // 给每个视图一个索引，第一个视图的索引为-1，不加入系统中
    view_id_to_index_[orientation.first] = index;
    ++index;
  }

  // 定义稀疏矩阵
  Eigen::SparseMatrix<double> sparse_mat;
  
  // 设置稀疏线性系统，使dR_ij = dR_j-dR_i。 这是角轴旋转的一阶近似值。 这仅应调用一次。
  SetupLinearSystem();

  // 执行L1鲁棒损耗最小化
  if (!SolveL1Regression()) {
    LOG(ERROR) << "Could not solve the L1 regression step.";
    return false;
  }

  // 执行迭代加权的最小二乘
  if (!SolveIRLS()) {
    LOG(ERROR) << "Could not solve the least squares error step.";
    return false;
  }

  return true;
}

SetupLinearSystem

设置稀疏线性系统

void RobustRotationEstimator::SetupLinearSystem() {
  // 旋转变化比全局旋转数少一，因为我们保持第一旋转不变
  tangent_space_step_.resize((global_orientations_->size() - 1) * 3);
  tangent_space_residual_.resize(relative_rotations_.size() * 3);
  sparse_matrix_.resize(relative_rotations_.size() * 3,
                        (global_orientations_->size() - 1) * 3);

  // 对于每个相对旋转约束，将一个条目添加到稀疏矩阵。 我们使用角度轴的一阶近似值，使得：R_ij = R_j-R_i。 这使得稀疏矩阵只是一堆identity矩阵。
  int rotation_error_index = 0;
  std::vector<Eigen::Triplet<double> > triplet_list;
  //   std::vector<std::pair<ViewIdPair, Eigen::Vector3d> > relative_rotations_;
  for (const auto& relative_rotation : relative_rotations_) {
    // 第个个视图的索引
    const int view1_index =
        FindOrDie(view_id_to_index_, relative_rotation.first.first);
    // 如果第一个视图的索引不为初始视图索引-1 
    // 我们将其中一个旋转保持不变，以消除线性系统的歧义   static const int kConstantRotationIndex = -1;
    if (view1_index != kConstantRotationIndex) {
      triplet_list.emplace_back(3 * rotation_error_index,
                                3 * view1_index,
                                -1.0);
      triplet_list.emplace_back(3 * rotation_error_index + 1,
                                3 * view1_index + 1,
                                -1.0);
      triplet_list.emplace_back(3 * rotation_error_index + 2,
                                3 * view1_index + 2,
                                -1.0);
    }

    const int view2_index =
        FindOrDie(view_id_to_index_, relative_rotation.first.second);
    if (view2_index != kConstantRotationIndex) {
      triplet_list.emplace_back(3 * rotation_error_index + 0,
                                3 * view2_index + 0,
                                1.0);
      triplet_list.emplace_back(3 * rotation_error_index + 1,
                                3 * view2_index + 1,
                                1.0);
      triplet_list.emplace_back(3 * rotation_error_index + 2,
                                3 * view2_index + 2,
                                1.0);
    }
    ++rotation_error_index;
  }
  // 构造稀疏矩阵
  sparse_matrix_.setFromTriplets(triplet_list.begin(), triplet_list.end());
}

这里需要着重讲一下：

首先定义一个稀疏矩阵 Eigen::SparseMatrix sparse_matrix_;

然后将其的大小设置为 sparse_matrix_.resize(relative_rotations_.size() * 3, (global_orientations_->size() - 1) * 3);

接着我们设置一个int rotation_error_index = 0;

然后定义vector<Eigen::Triplet<double> > triplet_list;

遍历relative_rotations_，分别得到两个视图的索引。

注意这个语句：triplet_list.emplace_back(3 * rotation_error_index, 3 * view1_index, -1.0);，意思是将传入三个值，分别为3 * rotation_error_index， 3 * view1_index，和-1.0，那么传入这三个值有什么意义呢？

首先来看，由于每次循环后，rotation_error_index都会自增1，所以下一次循环时3 * rotation_error_index + 0/1/2 都不会与上一次循环的值重复，且3 * view1_index + 0/1/2也是如此。这个triplet_list的用处是用于构造稀疏矩阵，第一个值为稀疏矩阵的行，第二个值为稀疏矩阵的列，第三个值为对应行列里填如的数。那么就可以明显的看出，每次循环都通过rotation_error_index 来控制稀疏矩阵的行，第一次为第0行，第1行和第2行，第二次循环为第3行，第4行和第5行，通过两个视图的索引来控制列，第一个视图的索引为-1，不加入系统，从第二个视图开始，假设其索引为1，与其配对的视图索引为2，那么这次循环填入的值分别为

(0，3，-1)、（1，4，-1）、（2，5，-1）、（0，6，1）、（1，7，1）、（2，8，1）

那么构造出的稀疏矩阵即为

[left[ egin{matrix} 0 & 0 & 0 & -1 & 0 & 0 & 1 & 0 & 0\ 0 & 0 & 0 & 0 & -1 & 0 & 0 & 1 & 0\ 0 & 0 & 0 & 0 & 0 & -1 & 0 & 0 & 1\ end{matrix} ight] ]

这个稀疏矩阵每三行为一个组，大部分值都为0，为-1的值其列为3 * view1_index + 0/1/2，行为3 * rotation_error_index + 0/1/2，rotation_error_index 是依次增大的，所以每一行都会有一个-1和1的值，剩下值都为0。同理，为1的值其列为3 * view2_index + 0/1/2，行为3 * rotation_error_index + 0/1/2。

FilterRotations

void GlobalReconstructionEstimator::FilterRotations() {
  // 根据相对旋转和估计的总体方向过滤视图对
  FilterViewPairsFromOrientation(
      orientations_,
      options_.rotation_filtering_max_difference_degrees,
      view_graph_);
  // 删除不属于最大连接组件的所有视图对，并返回已删除视图的ViewId(这个函数前面有讲过，因为上一步删除了一些视图对，所以需要重新删除不属于最大连接组件的所有视图对)
  const std::unordered_set<ViewId> removed_views =
      RemoveDisconnectedViewPairs(view_graph_);
  for (const ViewId removed_view : removed_views) {
    orientations_.erase(removed_view);
  }
}

FilterViewPairsFromOrientation

根据方向估计值过滤视图对。如果从两个视图匹配获得的相对旋转（即TwoViewInfo.rotation_2）与定向估计所形成的相对旋转相差超过阈值，则该视图对将被视为“bad”并被删除。
视图ID假定ID较低的视图为ViewIdPair中的第一项。

具体而言，如果满足以下条件，我们仅保留(R_ {i，j}：|| R_ {i，j}-R_j * R_i ^ t || <tau)，其中(|| A-B ||) 是A和B之间的角距离
注意：此功能将删除任何视图对，这些视图对包含在方向上没有条目的视图，并将注释输出到LOG（WARNING）。

void FilterViewPairsFromOrientation(
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientations,
    const double max_relative_rotation_difference_degrees,
    ViewGraph* view_graph) {
  CHECK_NOTNULL(view_graph);
  CHECK_GE(max_relative_rotation_difference_degrees, 0.0);

  // 预先计算弧度的平方阈值
  const double max_relative_rotation_difference_radians =
      DegToRad(max_relative_rotation_difference_degrees);
  const double sq_max_relative_rotation_difference_radians =
      max_relative_rotation_difference_radians *
      max_relative_rotation_difference_radians;

  std::unordered_set<ViewIdPair> view_pairs_to_remove;
  const auto& view_pairs = view_graph->GetAllEdges();
  for (const auto& view_pair : view_pairs) {
    const Eigen::Vector3d* orientation1 =
        FindOrNull(orientations, view_pair.first.first);
    const Eigen::Vector3d* orientation2 =
        FindOrNull(orientations, view_pair.first.second);

    // 如果视图对包含没有方向的视图，则将其删除
    if (orientation1 == nullptr || orientation2 == nullptr) {
      LOG(WARNING)
          << "View pair (" << view_pair.first.first << ", "
          << view_pair.first.second
          << ") contains a view that does not exist! Removing the view pair.";
      view_pairs_to_remove.insert(view_pair.first);
      continue;
    }

    // 如果相对旋转估计值不在公差范围内，请删除视图对
    if (!AngularDifferenceIsAcceptable(
            *orientation1,
            *orientation2,
            view_pair.second.rotation_2,
            sq_max_relative_rotation_difference_radians)) {
      view_pairs_to_remove.insert(view_pair.first);
    }
  }

  // 删除所有“bad”相对位姿
  for (const ViewIdPair view_id_pair : view_pairs_to_remove) {
    view_graph->RemoveEdge(view_id_pair.first, view_id_pair.second);
  }
  VLOG(1) << "Removed " << view_pairs_to_remove.size()
          << " view pairs by rotation filtering.";
}

优化相对位移 OptimizePairwiseTranslations

void GlobalReconstructionEstimator::OptimizePairwiseTranslations() {
  // 在计算绝对旋转后，细化相对平移估计。 这可以帮助提高位置估计的准确性
  if (options_.refine_relative_translations_after_rotation_estimation) {
    RefineRelativeTranslationsWithKnownRotations(*reconstruction_,
                                                 orientations_,
                                                 options_.num_threads,
                                                 view_graph_);
  }
}

RefineRelativeTranslationsWithKnownRotations

在已知旋转估计的情况下，通过优化对极约束来细化视图对之间的相对平移估计

void RefineRelativeTranslationsWithKnownRotations(
    const Reconstruction& reconstruction,
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientations,
    const int num_threads,
    ViewGraph* view_graph) {
  CHECK_GE(num_threads, 1);
  const auto& view_pairs = view_graph->GetAllEdges();

  ThreadPool pool(num_threads);
  // 优化每个视图对的位移估计
  for (const auto& view_pair : view_pairs) {
    // 获取两个视图共有的所有特征对应关系
    std::vector<FeatureCorrespondence> matches;
    const View* view1 = reconstruction.View(view_pair.first.first);
    const View* view2 = reconstruction.View(view_pair.first.second);
    GetNormalizedFeatureCorrespondences(*view1, *view2, &matches);

    TwoViewInfo* info = view_graph->GetMutableEdge(view_pair.first.first,
                                                   view_pair.first.second);
    pool.Add(OptimizeRelativePositionWithKnownRotation,
             matches,
             FindOrDie(orientations, view_pair.first.first),
             FindOrDie(orientations, view_pair.first.second),
             &info->position_2);
  }
}

OptimizeRelativePositionWithKnownRotation

使用已知的相对旋转，针对所有对应关系优化使对极约束x2'* [t] _x * R * x1 = 0最小的相对位置。注意：位置为-R'* t，旋转方向对应于摄像机1和2的绝对方向。

此算法基于Onur Ozyesil和Amit Singer撰写的"Robust Camera Location Estimation by Convex Programming"中的稳健的成对平移估计算法（CVPR 2015）

// 以后有时间再研究。。。

过滤不好的位移 FilterRelativeTranslation

void GlobalReconstructionEstimator::FilterRelativeTranslation() {
  
  // options_.extract_maximal_rigid_subgraph如果为true，则仅将条件良好的摄像头用于位置估计，以进行全局位置估计
  if (options_.extract_maximal_rigid_subgraph) {
    LOG(INFO) << "Extracting maximal rigid component of viewing graph to "
                 "determine which cameras are well-constrained for position "
                 "estimation.";
    ExtractMaximallyParallelRigidSubgraph(orientations_, view_graph_);
  }

  // 过滤可能不好的相对位移
  // 使用1DSfM算法过滤相对平移估计，以潜在地去除异常的相对姿势以进行位置估计
  if (options_.filter_relative_translations_with_1dsfm) {
    LOG(INFO) << "Filtering relative translations with 1DSfM filter.";
    FilterViewPairsFromRelativeTranslation(translation_filter_options_,
                                           orientations_,
                                           view_graph_);
  }
  // 从估算中删除所有未连接的视图
  const std::unordered_set<ViewId> removed_views =
      RemoveDisconnectedViewPairs(view_graph_);
  for (const ViewId removed_view : removed_views) {
    orientations_.erase(removed_view);
  }
}

估计全局位置 EstimatePosition

bool GlobalReconstructionEstimator::EstimatePosition() {
  // 获得所有边信息
  const auto& view_pairs = view_graph_->GetAllEdges();
  // 定义全局位置估算方法接口的通用类。 这些方法将每个摄像机估计的（全局/绝对）方向以及成对摄像机之间的成对平移方向作为输入。 还可以根据需要传递其他信息，例如跟踪/对应，但这些信息将特定于子类实现。
  std::unique_ptr<PositionEstimator> position_estimator;

  // 选择全局位置估计类型
  switch (options_.global_position_estimator_type) {
    case GlobalPositionEstimatorType::LEAST_UNSQUARED_DEVIATION: {
      position_estimator.reset(new LeastUnsquaredDeviationPositionEstimator(
          options_.least_unsquared_deviation_position_estimator_options));
      break;
    }
    // 以此为例
    case GlobalPositionEstimatorType::NONLINEAR: {
      position_estimator.reset(new NonlinearPositionEstimator(
          options_.nonlinear_position_estimator_options, *reconstruction_));
      break;
    }
    case GlobalPositionEstimatorType::LINEAR_TRIPLET: {
      position_estimator.reset(new LinearPositionEstimator(
          options_.linear_triplet_position_estimator_options,
          *reconstruction_));
      break;
    }
    default: {
      LOG(FATAL) << "Invalid type of global position estimation chosen.";
      break;
    }
  }

  return position_estimator->EstimatePositions(view_pairs,
                                               orientations_,
                                               &positions_);
}

NonlinearPositionEstimator

根据成对的相对姿势和相机的绝对方向，估计视图的相机位置。使用具有鲁棒成本函数的非线性求解器估算位置。该解决方案策略紧密遵循Wilson和Snavely在“ Robust Global Translations with 1DSfM”（ECCV 2014）中概述的方法。

bool NonlinearPositionEstimator::EstimatePositions(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
    
  // 注意这里positions非空，尽管之前对其没有任何赋值操作
  CHECK_NOTNULL(positions);
  // 如果视图对和方向为空，返回
  if (view_pairs.empty() || orientations.empty()) {
    VLOG(2) << "Number of view_pairs = " << view_pairs.size()
            << " Number of orientations = " << orientations.size();
    return false;
  }
  triangulated_points_.clear();
  problem_.reset(new ceres::Problem());
  view_pairs_ = &view_pairs;

  // 仅当问题足够大时才使用迭代schur，否则使用稀疏schur
  static const int kMinNumCamerasForIterativeSolve = 1000;

  // 将位置初始化为随机
  InitializeRandomPositions(orientations, positions);

  // 将约束添加到problem
  AddCameraToCameraConstraints(orientations, positions);
  if (options_.min_num_points_per_view > 0) {
    AddPointToCameraConstraints(orientations, positions);
    AddCamerasAndPointsToParameterGroups(positions);
  }

  // 将一台摄像机设为原点，以消除原点的歧义
  positions->begin()->second.setZero();
  // 设定对应的参数模块在优化过程中保持不变
  problem_->SetParameterBlockConstant(positions->begin()->second.data());

  // 设置求解器选项
  ceres::Solver::Summary summary;
  solver_options_.num_threads = options_.num_threads;
  solver_options_.max_num_iterations = options_.max_num_iterations;

  // 选择线性求解器的类型。 对于足够大的问题，我们想使用迭代方法（例如，共轭梯度或迭代Schur）
  // 但是，如果在优化中使用了3D点，我们只想使用Schur求解器。
  if (positions->size() > kMinNumCamerasForIterativeSolve) {
    if (options_.min_num_points_per_view > 0) {
      solver_options_.linear_solver_type = ceres::ITERATIVE_SCHUR;
      solver_options_.preconditioner_type = ceres::SCHUR_JACOBI;
    } else {
      solver_options_.linear_solver_type = ceres::CGNR;
      solver_options_.preconditioner_type = ceres::JACOBI;
    }
  } else {
    if (options_.min_num_points_per_view > 0) {
      solver_options_.linear_solver_type = ceres::SPARSE_SCHUR;
    } else {
      solver_options_.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
    }
  }

  ceres::Solve(solver_options_, problem_.get(), &summary);
  LOG(INFO) << summary.FullReport();
  return summary.IsSolutionUsable();
}

InitializeRandomPositions

初始化随机位置，估计相机位置需要首先将位置初始化为随机值。

具体步骤为：得到所有的视图对并遍历，如果orientations中包含该视图对，则初始化视图对的随机位置（视图对的方向和位置即为摄像机之间的旋转平移）

void NonlinearPositionEstimator::InitializeRandomPositions(
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  std::unordered_set<ViewId> constrained_positions;
  constrained_positions.reserve(orientations.size());
  // 将视图对的每对视图id插入到constrained_positions中
  for (const auto& view_pair : *view_pairs_) {
    constrained_positions.insert(view_pair.first.first);
    constrained_positions.insert(view_pair.first.second);
  }
  // 为positions预存空间
  positions->reserve(orientations.size());
  // 遍历所有的视图方向
  for (const auto& orientation : orientations) {
    // 如果两视图方向的视图为constrained_positions中的两个视图
    if (ContainsKey(constrained_positions, orientation.first)) {
       // 随机化位置，注意这里的下标是orientation.first即视图对的第一个视图
      (*positions)[orientation.first] = 100.0 * rng_->RandVector3d();
    }
  }
}

AddCameraToCameraConstraints

void NonlinearPositionEstimator::AddCameraToCameraConstraints(
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  for (const auto& view_pair : *view_pairs_) {
    // 得到视图id和对应位置
    const ViewId view_id1 = view_pair.first.first;
    const ViewId view_id2 = view_pair.first.second;
    Vector3d* position1 = FindOrNull(*positions, view_id1);
    Vector3d* position2 = FindOrNull(*positions, view_id2);

    // 如果一个或两个位置都不存在，请不要添加此视图对
    if (position1 == nullptr || position2 == nullptr) {
      continue;
    }

    // 旋转相对平移，使其与全局方向框架对齐
    /*
    Vector3d GetRotatedTranslation(const Vector3d& rotation_angle_axis,
                               const Vector3d& translation) {
      Matrix3d rotation;
      ceres::AngleAxisToRotationMatrix(
          rotation_angle_axis.data(),
          ceres::ColumnMajorAdapter3x3(rotation.data()));
      return rotation.transpose() * translation;
    }
    */

    // 将相对位移转换到世界方向框架
    const Vector3d translation_direction = GetRotatedTranslation(
        FindOrDie(orientations, view_id1), view_pair.second.position_2);

    // ceres解决problem
    ceres::CostFunction* cost_function =
        PairwiseTranslationError::Create(translation_direction, 1.0);
    
    // 问题加入残差项
    problem_->AddResidualBlock(cost_function,
                               new ceres::HuberLoss(options_.robust_loss_width),
                               position1->data(),
                               position2->data());
  }

  VLOG(2) << problem_->NumResidualBlocks()
          << " camera to camera constraints "
             "were added to the position "
             "estimation problem.";
}

来具体看Ceres Solver是如何实现位移的：

首先我们需要将位移转移到世界方向框架上去——将旋转向量转换为旋转矩阵，然后得到旋转矩阵的转置 x 位移。

// 将相对位移转换到世界方向框架
const Vector3d translation_direction = GetRotatedTranslation(
    FindOrDie(orientations, view_id1), view_pair.second.position_2);

/*
Vector3d GetRotatedTranslation(const Vector3d& rotation_angle_axis,
                               const Vector3d& translation) {
    Matrix3d rotation;
   	// 将旋转向量转换为旋转矩阵
    ceres::AngleAxisToRotationMatrix(
        rotation_angle_axis.data(),
        ceres::ColumnMajorAdapter3x3(rotation.data()));
    return rotation.transpose() * translation;
}
*/

计算平移方向和从两个位置形成的方向之间的误差，以使(（c_j-c_i）)-标量* (t_{ij})最小。

struct PairwiseTranslationError {
  // 传入位移方向 和 权重(1.0)
  PairwiseTranslationError(const Eigen::Vector3d& translation_direction,
                           const double weight);

  // 该误差由上述位置误差给出
  template <typename T>
  bool operator()(const T* position1, const T* position2, T* residuals) const;

  static ceres::CostFunction* Create(
      const Eigen::Vector3d& translation_direction, const double weight);

  const Eigen::Vector3d translation_direction_;
  const double weight_;
};

template <typename T>

// 重写（），传入两个视图的坐标position
bool PairwiseTranslationError::operator() (const T* position1,
                                           const T* position2,
                                           T* residuals) const {
  // 定义范数容忍度
  const T kNormTolerance = T(1e-12);

  // translation = position2 - position1
  T translation[3];
  translation[0] = position2[0] - position1[0];
  translation[1] = position2[1] - position1[1];
  translation[2] = position2[2] - position1[2];
    
  // 计算norm = sqrt(x^2 + y^2 + z^2)
  T norm =
      sqrt(translation[0] * translation[0] + translation[1] * translation[1] +
           translation[2] * translation[2]);

  // 如果范数很小，则位置非常接近。 在这种情况下，请避免除以极小数，否则将导致残差项的权重急剧上升。
  if (T(norm) < kNormTolerance) {
    norm = T(1.0);
  }
    
  // 定义计算残差
  residuals[0] = weight_ * (translation[0] / norm - translation_direction_[0]);
  residuals[1] = weight_ * (translation[1] / norm - translation_direction_[1]);
  residuals[2] = weight_ * (translation[2] / norm - translation_direction_[2]);
  return true;
}

使用ceres解决问题

// ceres解决problem
ceres::CostFunction* cost_function = PairwiseTranslationError::Create(translation_direction, 1.0);
    

// 问题加入残差项
problem_->AddResidualBlock(cost_function,
                           new ceres::HuberLoss(options_.robust_loss_width),
                           position1->data(),
                           position2->data());

在重建对象中设置位姿 SetReconstructionFromEstimatedPoses

void SetReconstructionFromEstimatedPoses(
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientations,
    const std::unordered_map<ViewId, Eigen::Vector3d>& positions,
    Reconstruction* reconstruction) {
  // 遍历所有的position
  for (const auto& position : positions) {
    // 得到对应的视图
    View* view = reconstruction->MutableView(position.first);
    // 跳过空视图
    if (view == nullptr) {
      LOG(WARNING) << "Trying to set the pose of View " << position.first
                   << " which does not exist in the reconstruction.";
      continue;
    }
    // 如果视图已经被估计
    CHECK(!view->IsEstimated()) << "Cannot set the pose of a view that has "
                                   "already been estimated. View Id "
                                << position.first;

    // 找到位置视图对应的方向
    const Eigen::Vector3d* orientation =
        FindOrNull(orientations, position.first);
    if (orientation == nullptr) {
      LOG(WARNING) << "Cannot add View " << position.first
                   << " to the reconstruction because it does nto contain an "
                      "orientation estimation.";
      continue;
    }
  
    // 为视图的相机设置位置和旋转
    view->MutableCamera()->SetPosition(position.second);
    view->MutableCamera()->SetOrientationFromAngleAxis(*orientation);
    // 将视图设置为已估计
    view->SetEstimated(true);
  }
}

三角剖分特征 EstimateStructure

void GlobalReconstructionEstimator::EstimateStructure() {
  // 估计所有的轨道
  TrackEstimator::Options triangulation_options;
  triangulation_options.max_acceptable_reprojection_error_pixels =
      options_.triangulation_max_reprojection_error_in_pixels;
  triangulation_options.min_triangulation_angle_degrees =
      options_.min_triangulation_angle_degrees;
  triangulation_options.bundle_adjustment = options_.bundle_adjust_tracks;
  triangulation_options.ba_options = SetBundleAdjustmentOptions(options_, 0);
  triangulation_options.ba_options.num_threads = 1;
  triangulation_options.ba_options.verbose = false;
  triangulation_options.num_threads = options_.num_threads;
  TrackEstimator track_estimator(triangulation_options, reconstruction_);
  const TrackEstimator::Summary summary = track_estimator.EstimateAllTracks();
}

SetUnderconstrainedAsUnestimated

详解见增量式SFM

BA调整 BundleAdjustCameraPositionsAndPoints

BA仅调整相机的位置和点。相机的方向和内在特性保持不变。

通过仅选择3d点的子集进行优化，可以大大提高大规模BA调整的效率，因为3d点往往具有增加的场景冗余。如果以适当限制非线性优化的方式选择点，则与优化所有轨迹相比，可以观察到类似的质量结果。
选择3d点以使其符合以下条件：
a）高置信度（即低重投影误差）。
b）最好是长轨道。
c）用于优化的轨道在每个图像中提供了良好的空间覆盖率。
d）每个视图至少观察K条优化轨迹。
首先，将每个图像中的轨道散列到具有图像网格的空间仓中，其中每个图像网格单元格是提供的宽度。在每个网格单元中，根据轨道的轨道长度对轨道进行排序，然后根据平均重投影误差进行排序。轨道长度被截断为不大于long_track_length_threshold，以便在长轨道中选择低重投影误差的轨道进行BA调整。
我们建议将网格像元大小设置为100像素，将长轨道长度阈值设置为10，将每个视图的最小优化轨道数设置为100。

// view_ids是从reconstruction获得已估计的view
bool SelectGoodTracksForBundleAdjustment(
    const Reconstruction& reconstruction,
    const std::unordered_set<ViewId>& view_ids,
    const int long_track_length_threshold,
    const int image_grid_cell_size_pixels,
    const int min_num_optimized_tracks_per_view,
    std::unordered_set<TrackId>* tracks_to_optimize) {
  // 计算轨迹平均重投影误差
  std::unordered_map<TrackId, TrackStatistics> track_statistics;
  ComputeTrackStatistics(reconstruction,
                         view_ids,
                         long_track_length_threshold,
                         &track_statistics);

  // 对于每个图像，将图像划分为一个网格，并从每个网格单元格中选择最高质量的轨道。这有助于在每张图像中对轨迹进行良好的空间覆盖。
  for (const ViewId view_id : view_ids) {
    const View* view = reconstruction.View(view_id);

    // 从图像中的每个网格单元格中选择最好的轨道，并将它们添加到要优化的轨道容器中。
    SelectBestTracksFromEachImageGridCell(reconstruction,
                                          *view,
                                          image_grid_cell_size_pixels,
                                          track_statistics,
                                          tracks_to_optimize);
  }

  // 到目前为止，我们只添加了在每张图像中具有尽可能完整的空间覆盖的特征，但我们不能确保每张图像都被至少K个特征约束。因此，我们再次循环遍历所有视图并添加还没有添加的顶部M轨道
  for (const ViewId view_id : view_ids) {
    const View* view = reconstruction.View(view_id);

    // 如果这个视图没有被足够优化的轨道所约束，添加顶级的特性，直到有足够的轨道约束该视图
    SelectTopRankedTracksInView(reconstruction,
                                track_statistics,
                                *view,
                                min_num_optimized_tracks_per_view,
                                tracks_to_optimize);
  }

  return true;
}

计算平均重投影误差和每个轨道的截断轨道长度。我们基于以下观察来截断轨道长度：较大的轨道长度为束调整提供了更好的约束，而较大的轨道在我们的经验中也更有可能包含离群值。截断轨道长度会强制选择重投影误差最小的长轨道。

typedef std::pair<int, double> TrackStatistics;

void ComputeTrackStatistics(
    const Reconstruction& reconstruction,
    const std::unordered_set<ViewId>& view_ids,
    const int long_track_length_threshold,
    std::unordered_map<TrackId, TrackStatistics>* track_statistics) {
  // 遍历所有视图并为我们计算的每个轨道计算轨道统计信息
  for (const ViewId view_id : view_ids) {
    const View* view = reconstruction.View(view_id);
    const auto& tracks_in_view = view->TrackIds();
    // 计算此视图中我们尚未为其计算统计信息的每个轨道的统计信息
    for (const TrackId track_id : tracks_in_view) {
      const Track* track = reconstruction.Track(track_id);
      // 如果已经计算出统计信息，或者尚未估算出轨道，请跳过此轨道
      if (ContainsKey(*track_statistics, track_id) || track == nullptr ||
          !track->IsEstimated()) {
        continue;
      }

      // 计算轨道统计信息并将其添加到输出map
      const TrackStatistics& statistics_for_this_track =
          ComputeStatisticsForTrack(reconstruction,
                                    track_id,
                                    long_track_length_threshold);
      track_statistics->emplace(track_id, statistics_for_this_track);
    }
  }
}

TrackStatistics ComputeStatisticsForTrack(
    const Reconstruction& reconstruction,
    const TrackId track_id,
    const int long_track_length_threshold) {
  // 保证达到此功能的任何轨道都将存在并被估计，因此无需在此处进行检查
  const Track* track = reconstruction.Track(track_id);
  const auto& views_observing_track = track->ViewIds();

  double sq_reprojection_error_sum = 0.0;
  int num_valid_reprojections = 0;
  // 计算观察轨迹的每个视图的平方重投影误差，并将其计算为累加和
  for (const ViewId view_id : views_observing_track) {
    const View* view = reconstruction.View(view_id);
    if (view == nullptr || !view->IsEstimated()) {
      continue;
    }
    sq_reprojection_error_sum +=
        ComputeSqReprojectionError(*view, *view->GetFeature(track_id), *track);
    ++num_valid_reprojections;
  }

  // 计算并返回轨道统计信息
  const int truncated_track_length =
      std::min(num_valid_reprojections, long_track_length_threshold);
  const double mean_sq_reprojection_error =
      sq_reprojection_error_sum / static_cast<double>(num_valid_reprojections);
  // typedef std::pair<int, double> TrackStatistics;
  return TrackStatistics(truncated_track_length, mean_sq_reprojection_error);
}

SelectBestTracksFromEachImageGridCell

从图像中选择轨迹，确保图像有良好的空间覆盖。为此，我们首先将轨迹放入图像网格中的网格单元格中。然后，在每个单元格中，我们找到排名最好的轨道，并将其添加到轨道列表中进行优化。

typedef std::pair<TrackId, TrackStatistics> GridCellElement;

void SelectBestTracksFromEachImageGridCell(
    const Reconstruction& reconstruction,
    const View& view,
    const int grid_cell_size,
    const std::unordered_map<TrackId, TrackStatistics>& track_statistics,
    std::unordered_set<TrackId>* tracks_to_optimize) {
    
  static const double inv_grid_cell_size =  1.0 / grid_cell_size;

  // 将每个feature散列到一个网格单元格中
  ImageGrid image_grid;
  const auto& track_ids = view.TrackIds();
  for (const TrackId track_id : track_ids) {
    const Track* track = reconstruction.Track(track_id);
    if (track == nullptr || !track->IsEstimated()) {
      continue;
    }
      
    const Feature& feature = *view.GetFeature(track_id);
    const TrackStatistics& current_track_statistics =
        FindOrDieNoPrint(track_statistics, track_id);
    // 计算网格下标
    const Eigen::Vector2i grid_cell =
        (feature * inv_grid_cell_size).cast<int>();
    
    image_grid[grid_cell].emplace_back(track_id, current_track_statistics);
  }

  // 从每个网格单元中选择最好的feature，并将其添加到要优化的轨道中
  for (auto& grid_cell : image_grid) {
    // 先按轨道长度对每个单元的特征进行排序，再按平均重投影误差排序。
    const GridCellElement& grid_cell_element =
        *std::min_element(grid_cell.second.begin(),
                          grid_cell.second.end(),
                          // 根据轨迹统计信息对网格单元格元素进行排序，该统计信息将首先根据(截断的)轨迹长度排序，然后根据平均重投影误差排序
                          /*
                          bool CompareGridCellElements(
                             const std::pair<TrackId, TrackStatistics>& element1,
                             const std::pair<TrackId, TrackStatistics>& element2) {
                             return element1.second < element2.second;
                          }
                          */
                          CompareGridCellElements);

    // 将轨道id插入到轨道中进行优化
    tracks_to_optimize->emplace(grid_cell_element.first);
  }
}

SelectTopRankedTracksInView

在视图观察到优化轨道的最小数量之前，选择还没有被选择的排名最高的轨道。

void SelectTopRankedTracksInView(
    const Reconstruction& reconstruction,
    const std::unordered_map<TrackId, TrackStatistics>& track_statistics,
    const View& view,
    const int min_num_optimized_tracks_per_view,
    std::unordered_set<TrackId>* tracks_to_optimize) {
  int num_optimized_tracks = 0;
  int num_estimated_tracks = 0;

  const auto& tracks_in_view = view.TrackIds();
  std::vector<GridCellElement> ranked_candidate_tracks;
  for (const TrackId track_id : tracks_in_view) {
    const Track* track = reconstruction.Track(track_id);
    if (track == nullptr || !track->IsEstimated()) {
      continue;
    }
    // 我们只有在估计了轨迹后才能到达这个点
    ++num_estimated_tracks;

    // 如果轨道已经安排优化，增加优化特性的数量
    if (ContainsKey(*tracks_to_optimize, track_id)) {
      ++num_optimized_tracks;
      // 如果optimized_tracks的数量大于最小值，那么我们可以提前返回，因为我们知道不需要为这个视图添加更多的feature
      if (num_optimized_tracks >= min_num_optimized_tracks_per_view) {
        return;
      }
    } else {
      // 如果轨道尚未设置为优化，则将其添加到候选轨道列表中
      ranked_candidate_tracks.emplace_back(
          track_id, FindOrDieNoPrint(track_statistics, track_id));
    }
  }

  // 我们只有在优化轨迹的数量小于最小值时才能达到这一点。如果是这种情况，那么我们添加最上边的候选特征，直到满足观察到的特征的最小数量
  if (num_optimized_tracks != num_estimated_tracks) {
    // 选择添加多少轨道。如果我们需要比估计更多的轨道，那么我们只需添加所有剩余的特性
    const int num_optimized_tracks_needed =
        std::min(min_num_optimized_tracks_per_view - num_optimized_tracks,
                 num_estimated_tracks - num_optimized_tracks);
    // 排序num_optimized_tracks_needed个轨道
    std::partial_sort(
        ranked_candidate_tracks.begin(),
        ranked_candidate_tracks.begin() + num_optimized_tracks_needed,
        ranked_candidate_tracks.end());
    // 将候选轨道添加到要优化的轨道列表中
    for (int i = 0; i < num_optimized_tracks_needed; i++) {
      tracks_to_optimize->emplace(ranked_candidate_tracks[i].first);
    }
  }
}

Bundle Adjustment

bool GlobalReconstructionEstimator::BundleAdjustment() {
  // BA选项
  bundle_adjustment_options_ =
      SetBundleAdjustmentOptions(options_, positions_.size());

  // 如果需要，选择好的轨道优化BA。这大大减少了bundle平差中参数的数量，并且在过滤可能减慢非线性优化的异常值轨道方面做得很好
  std::unordered_set<TrackId> tracks_to_optimize;
  if (options_.subsample_tracks_for_bundle_adjustment &&
      SelectGoodTracksForBundleAdjustment(
          *reconstruction_,
          options_.track_subset_selection_long_track_length_threshold,
          options_.track_selection_image_grid_cell_size_pixels,
          options_.min_num_optimized_tracks_per_view,
          &tracks_to_optimize)) {
    // 将所有未为BA选择的轨道设置为未估计，以免影响bundle调整优化
    const auto& view_ids = reconstruction_->ViewIds();
    SetTracksInViewsToUnestimated(view_ids,
                                  tracks_to_optimize,
                                  reconstruction_);
  } else {
    // 得到重构中所有已估计轨道的轨道id
    GetEstimatedTracksFromReconstruction(*reconstruction_, &tracks_to_optimize);
  }
  LOG(INFO) << "Selected " << tracks_to_optimize.size()
            << " tracks to optimize.";

  std::unordered_set<ViewId> views_to_optimize;
  // 得到重构中所有已估计视图的视图id
  GetEstimatedViewsFromReconstruction(*reconstruction_,
                                      &views_to_optimize);
  // Bundle调整指定的视图和轨迹
  const auto& bundle_adjustment_summary =
      BundleAdjustPartialReconstruction(bundle_adjustment_options_,
                                        views_to_optimize,
                                        tracks_to_optimize,
                                        reconstruction_);
  return bundle_adjustment_summary.success;
}

BundleAdjustPartialReconstruction

BundleAdjustmentSummary BundleAdjustPartialReconstruction(
    const BundleAdjustmentOptions& options,
    const std::unordered_set<ViewId>& view_ids,
    const std::unordered_set<TrackId>& track_ids,
    Reconstruction* reconstruction) {
  CHECK_NOTNULL(reconstruction);

  // 定义BundleAdjuster类，向此类中添加所有视图和轨道
  BundleAdjuster bundle_adjuster(options, reconstruction);
  for (const ViewId view_id : view_ids) {
    bundle_adjuster.AddView(view_id);
  }
  for (const TrackId track_id : track_ids) {
    bundle_adjuster.AddTrack(track_id);
  }
  // BA优化
  return bundle_adjuster.Optimize();
}

BundleAdjuster类建立了BA的非线性优化问题。通过添加要优化的视图和轨迹来设置Bundle调整问题。只有视图和跟踪提供的AddView和AddTrack将被优化。所有其他参数保持不变。

注意:如果任何视图被优化，必须在AddTracks之前调用AddViews。

BundleAdjustmentSummary Optimize();

在AddView和AddTrack被调用后，使用bundle adjustment优化提供的视图和跟踪。

BundleAdjustmentSummary BundleAdjuster::Optimize() {
  // 设置相机位姿的extrinsics参数化。如果需要，这将设置方向和/或位置为常量
  SetCameraExtrinsicsParameterization();

  // 设置内在属性组参数化。这将控制哪些内在参数或优化或保持不变。注意，每个相机内含特性组可能是一个不同的相机和/或一个不同的相机内含特性模型。
  SetCameraIntrinsicsParameterization();

  // NOTE: 在Ceres求解器email group中发现了一个线程，该线程指出使用反向BA顺序(即，先使用相机然后使用点)对于内部迭代来说是一个好主意
  if (solver_options_.use_inner_iterations) {
    solver_options_.inner_iteration_ordering.reset(
        new ceres::ParameterBlockOrdering(*parameter_ordering_));
    // 反向BA
    solver_options_.inner_iteration_ordering->Reverse();
  }

  // 解决问题
  ceres::Solver::Summary solver_summary;
  ceres::Solve(solver_options_, problem_.get(), &solver_summary);
  LOG_IF(INFO, options_.verbose) << solver_summary.FullReport();

  // 设置BundleAdjustmentSummary
  BundleAdjustmentSummary summary;
  summary.initial_cost = solver_summary.initial_cost;
  summary.final_cost = solver_summary.final_cost;

  // 这只表明优化是否成功运行，并不保证质量或收敛
  summary.success = solver_summary.IsSolutionUsable();

  return summary;
}

剔除效果不好的轨道 SetOutlierTracksToUnestimated

注意：里面的相机方向ray_directions 表示每一个点的向量坐标 - 相机的向量坐标，表示相机到点的方向向量。

流程为：设置最大重投影误差，遍历每一个轨道id，如果轨道没有被估计，则跳过。得到当前轨道对应的视图组，遍历每一个视图

int SetOutlierTracksToUnestimated(const std::unordered_set<TrackId>& track_ids,
                                  const double max_inlier_reprojection_error,
                                  const double min_triangulation_angle_degrees,
                                  Reconstruction* reconstruction) {
  // 最大sq重投影误差（sq??)
  const double max_sq_reprojection_error =
      max_inlier_reprojection_error * max_inlier_reprojection_error;

  int num_estimated_tracks = 0;
  int num_bad_reprojections = 0;
  int num_insufficient_viewing_angles = 0;

  // 得到每一个轨道id
  for (const TrackId track_id : track_ids) {
    // 得到id对应track
    Track* track = reconstruction->MutableTrack(track_id);
    // 如果轨道没有被估计， 则跳过
    if (!track->IsEstimated()) {
      continue;
    }
    // 已经估计的轨道数+1
    ++num_estimated_tracks;
    // 射线方向
    std::vector<Eigen::Vector3d> ray_directions;
    // 轨道对应的视图组
    const auto& view_ids = track->ViewIds();
    int num_projections = 0;
    double mean_sq_reprojection_error = 0;
    // 遍历每一个视图
    for (const ViewId view_id : view_ids) {
      // 得id对应的视图，并跳过没有估计的视图
      const View* view = CHECK_NOTNULL(reconstruction->View(view_id));
      if (!view->IsEstimated()) {
        continue;
      }
      // 视图内相机
      const Camera& camera = view->Camera();
      // 相机射线方向
      const Eigen::Vector3d ray_direction =
          track->Point().hnormalized() - camera.GetPosition();
      ray_directions.push_back(ray_direction.normalized());

      // 得到track在view上的特征点
      const Feature* feature = view->GetFeature(track_id);
      // 重新投影观察值
      Eigen::Vector2d projection;
      // 得到点的深度
      const double depth = camera.ProjectPoint(track->Point(), &projection);
      // 如果点在相机后面，则移除
      if (depth < 0) {
        ++num_bad_reprojections;
        track->SetEstimated(false);
        break;
      }
      mean_sq_reprojection_error += (projection - *feature).squaredNorm();
      ++num_projections;
    }
    // 平均重投影误差
    mean_sq_reprojection_error /= static_cast<double>(num_projections);
    // 如果轨道已经被估计，且平均重投影误差大于最大重投影误差，则放弃这个轨道（设置为未估计）
    if (track->IsEstimated() &&
        mean_sq_reprojection_error > max_sq_reprojection_error) {
      ++num_bad_reprojections;
      track->SetEstimated(false);
    }

    // 如果重投影误差都很好，则轨迹将保持估计。 然后，我们通过至少两个摄像机以足够的视角对其进行测试，以测试轨道是否受到适当约束。
      
    /*
    // 如果任意两个观测值之间的三角测量角度足够，则返回true
    bool SufficientTriangulationAngle(
    const std::vector<Eigen::Vector3d>& ray_directions,
    const double min_triangulation_angle_degrees) 
    {
    	const double cos_of_min_angle = cos(DegToRad(min_triangulation_angle_degrees));
    	for (int i = 0; i < ray_directions.size(); i++) 
      		for (int j = i + 1; j < ray_directions.size(); j++) 
        		if (ray_directions[i].dot(ray_directions[j]) < cos_of_min_angle) return true;
    	return false;
    }
    */
      
    if (track->IsEstimated() &&
        !SufficientTriangulationAngle(ray_directions,
                                      min_triangulation_angle_degrees)) {
      ++num_insufficient_viewing_angles;
      track->SetEstimated(false);
    }
  }
  // 输出信息
  LOG_IF(INFO, num_bad_reprojections > 0 || num_insufficient_viewing_angles > 0)
      << num_bad_reprojections
      << " points were removed because of bad reprojection errors. "
      << num_insufficient_viewing_angles
      << " points were removed because they had insufficient viewing angles "
         "and were poorly constrained.";
  // 返回不好的投影数 + 角度不够充足的视图数
  return num_bad_reprojections + num_insufficient_viewing_angles;
}