1、函数签名

template <typename CONTAINER, typename = HasDerivedElementType<CONTAINER>>
FactorIndices add_factors(const CONTAINER& factors,
                          bool useEmptySlots = false);

关键类型与模板约束

• FactorIndices：FastVector<FactorIndex>（本质上是 std::vector<size_t>），表示新加入因子的下标列表，顺序与输入容器一致。
• CONTAINER 要求：容器的元素类型需是指向因子派生类的 shared_ptr，由约束 HasDerivedElementType<CONTAINER> 保证（例如 std::vector<NonlinearFactor::shared_ptr>）。

注意：add_factors 只接受“指针容器”。如果你手里是“因子对象容器”（非指针），应改用 add()/push_back() 相应重载。

2、函数做什么

把一批因子一次性加入因子图，并返回这些新因子在图中的索引列表（FactorIndices）。可选地复用之前通过 remove() 产生的“空槽位（nullptr）”。

3、参数

• factors：待加入的一批因子（容器，元素为 shared_ptr<DerivedFactor>）。

• useEmptySlots（默认 false）：

  • false：直接尾部追加所有因子；返回的索引是一段连续的新下标。
  • true：优先填充此前 remove() 留下的空槽；如果空槽不够会扩容；返回的索引可能不连续。

4、内部流程（源码要点）

• 预先分配 newFactorIndices(num_factors)；
• 若 useEmptySlots==true：从当前索引 i 向前找空位（at(i)==nullptr），找到就放；找不到时 resize() 扩容以容纳剩余因子；每放入一个，把目标位记录到 newFactorIndices[j]。
• 若 useEmptySlots==false：直接计算每个新因子将落在的位置 i + size()，然后一次性 push_back(factors)。
• 返回 newFactorIndices。

5、与 remove() / erase() 的关系

• remove(i)：把第 i 个因子置为 nullptr，保留索引不变 → 产生“空槽”，便于 useEmptySlots=true 时复用；size() 不变，nrFactors()（非空因子数）会减少。
• erase()：直接删除并压紧索引，后续因子索引全部左移；这会改变已保存的索引。

6、复杂度与返回值特性

• useEmptySlots=false：近似 O(k) 追加（k 为新因子数），返回连续区间索引。
• useEmptySlots=true：可能需要扫描空位，最坏 O(N + k)（N 为当前容量），返回索引不保证连续/单调。

7、使用场景

1. 批量添加因子

   std::vector<gtsam::BetweenFactor<Pose3>> newFactors;
   // ... 填充 newFactors ...
   auto indices = factorGraph.add_factors(newFactors);
   

   • 批量向因子图中添加回环闭合或里程计约束因子，并得到它们的索引。

2. 复用空槽位

   factorGraph.add_factors(newFactors, true);
   

   • 当一些因子被删除或失效后，选择复用这些空槽位，提高效率。

3. 动态 SLAM 系统

   • 在在线 SLAM 或增量式优化中，因子图不断添加新因子，这个函数提供了统一接口。

8、实用示例（以 NonlinearFactorGraph 为例）

using gtsam::NonlinearFactorGraph;
using gtsam::NonlinearFactor;
using gtsam::BetweenFactor;
using gtsam::noiseModel;

NonlinearFactorGraph graph;
// ... 图中已有因子，且之前可能有 graph.remove(i) 产生过空槽

std::vector<NonlinearFactor::shared_ptr> batch;
auto model = noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1,1,1,1,1,1).finished());
batch.push_back(boost::make_shared<BetweenFactor<gtsam::Pose3>>(gtsam::Symbol('x',0), gtsam::Symbol('x',1), gtsam::Pose3(), model));
batch.push_back(boost::make_shared<BetweenFactor<gtsam::Pose3>>(gtsam::Symbol('x',1), gtsam::Symbol('x',2), gtsam::Pose3(), model));

// 1) 直接尾部追加，索引连续
gtsam::FactorIndices idxs1 = graph.add_factors(batch, /*useEmptySlots=*/false);

// 2) 复用空槽（如果之前用过 remove()）
gtsam::FactorIndices idxs2 = graph.add_factors(batch, /*useEmptySlots=*/true);

9、常见坑位

1. 容器元素类型不对：传了非指针对象容器给 add_factors 会匹配失败；改用 add(container)/push_back(container)。
2. 索引假设错误：开启 useEmptySlots 后返回索引不再连续，别假设是 [old_size, old_size+k)。
3. 混用 erase 与索引缓存：erase 会改变后续索引；需要稳定索引时用 remove + useEmptySlots=true。

10、源码注释

template <class FACTOR>
template <typename CONTAINER, typename>
FactorIndices FactorGraph<FACTOR>::add_factors(
    const CONTAINER& factors,
    bool useEmptySlots) {

  const size_t num_factors = factors.size();              // 获取要添加的因子数量
  FactorIndices newFactorIndices(num_factors);           // 创建索引列表，用于存储新因子的位置

  if (useEmptySlots) {                                   // 如果选择复用空槽位
    size_t i = 0;                                        // 初始化索引 i，用于遍历因子图
    for (size_t j = 0; j < num_factors; ++j) {          // 遍历每一个要添加的因子
      do {                                               // 循环寻找可用槽位
        if (i >= size())                                 // 如果 i 超过当前因子图大小
          resize(size() + num_factors - j);             // 扩容一次，为剩余因子预留空间
        else if (at(i))                                  // 如果当前位置已被占用
          ++i;                                          // 移动到下一个槽位
        else                                            // 找到空槽位
          break;                                        // 跳出循环，准备填充因子
      } while (true);

      at(i) = factors[j];                                // 将因子填入空槽位 i
      newFactorIndices[j] = i;                           // 记录新因子的索引
    }
  } else {                                               // 如果不复用空槽位
    for (size_t i = 0; i < num_factors; ++i)            // 计算新因子索引
      newFactorIndices[i] = i + size();                 // 索引从因子图末尾开始
    push_back(factors);                                  // 直接将所有因子追加到因子图末尾
  }

  return newFactorIndices;                               // 返回新因子的索引列表
}

参考资料