完整教程：GTSAM中gtsam::NonlinearFactorGraph操作remove(...)、replace（...）和erase（...）使用详解
核心结论总结
remove(size_t i)：把索引i的shared_ptr置为nullptr（reset()），不改变容器长度（即size()不变），留下“空位/洞”。（实现：factors_.at(i).reset();）。replace(size_t index, sharedFactor factor)：用新的shared_ptr替换索引index上的指针（at(index) = factor）；索引不变，操作是 O(1)。([GTSAM][1])erase(iterator item)：调用底层FastVector::erase（实际是std::vector的别名），移除元素并把后面的元素向前移动以填补空位，从而改变被移除后所有因子（及其索引），返回指向“下一个元素”的迭代器（或end()）。erase 的复杂度与被移动元素数线性相关。
逐项详解（含实现 & 语义 & 注意点）
1) void remove(size_t i)
实现（摘自源码）：
void FactorGraph<FACTOR>
::remove(size_t i) { factors_.at(i).reset();
}也即调用了 boost::shared_ptr::reset()，把该槽位变成空指针（nullptr）。
语义 / 效果
- 容器
factors_的长度（size()）不改变。size()返回的是底层向量的长度（包含 null 槽）。要统计“活”的因子数量，使用nrFactors()。 nrFactors()会遍历计数非空指针以返回实际存在的因子数量（源码实现遍历factors_并计数if (factor) ++）。因此remove会使nrFactors()减 1，但size()不变。remove不会移动底层元素，因此不会改变其他因子的索引（索引稳定），也不会使迭代器被重新分配（只要没有其它会改变容器 capacity/size 的操作，现有迭代器仍然有效）。但被reset()的单个shared_ptr指向的对象会被释放 —— 那个具体对象/引用就失效了。
抛出/异常行为
- 使用
at(i)会检查边界；若i >= size()会抛std::out_of_range（remove可能抛）。
典型用途
- 想“逻辑删除”但保留原始索引（例如：外部保存了某些因子的索引，不希望它们因删除而改变）时用
remove。 - 可以和
add_factors(..., useEmptySlots=true)配合重用这些空槽（源码提供了一个查找空槽并复用的路径）。
示例
gtsam::NonlinearFactorGraph g;
// ... 添加若干因子 ...
g.remove(3);
// 将下标3处置为 nullptr，其他因子索引不变
// g.size() 不变，但 g.nrFactors() 会减少2) void replace(size_t index, sharedFactor factor)
实现（摘自源码）：
void FactorGraph<FACTOR>
::replace(size_t index, sharedFactor factor) {
at(index) = factor;
}即直接把 factors_[index] 指向新的 shared_ptr（替换旧指针）。
语义 / 效果
- 索引稳定：替换不会改变容器大小或其他元素的索引。
- O(1) 时间。不会移动其他元素，所以不会触发迭代器失效（除非其它线程或外部对容器做了改变）。
- 如果原来是一个
nullptr槽，这个操作会“填上”该槽；如果原来是有对象的指针，会先减少旧对象引用计数（可能导致析构）再指向新对象。
边界检查 / 异常
at(index)做边界检查，会在越界时抛异常。
示例
auto f_new = boost::make_shared<MyFactor>
(...);
g.replace(5, f_new);
// 下标5被替换（5 必须 < g.size()）3) iterator erase(iterator item) （和 range 版本）
实现（摘自源码）：
iterator FactorGraph<FACTOR>
::erase(iterator item) {
return factors_.erase(item);
}
iterator FactorGraph<FACTOR>
::erase(iterator first, iterator last) {
return factors_.erase(first, last);
}factors_ 是 FastVector，而 FastVector 实际是 std::vector<..., custom_allocator> 的 using 别名；因此 erase 的行为与 std::vector::erase 等价（返回“紧随被删除元素之后”的迭代器）。
语义 / 效果
- 删除元素并将后续元素整体前移以填补空位 —— 所以 后面所有元素的索引都会减 1。这会影响任何基于索引的外部引用（要特别小心）。
- 返回值是“下一个有效迭代器”（如果删除的是最后一个元素，则返回
end()）。详见std::vector::erase语义。([C++参考文献][2])
复杂度与迭代器失效
erase的复杂度与“被移走的元素后面需要移动的元素数量”成线性关系（通常认为是 O(n)）。erase会 使得指向被删除位置及其之后的所有迭代器/引用失效（包括end()）。在删除前面的元素时，后面的迭代器必须被重新获取或使用erase的返回值来继续遍历。
正确的迭代删除写法（示例）
for (auto it = g.begin(); it != g.end();
) {
if (shouldRemove(*it)) {
it = g.erase(it);
// erase 返回下一位置，所以用它继续循环
} else ++it;
}错误示例（会造成未定义行为）：
for (auto it = g.begin(); it != g.end();
++it) {
if (cond) g.erase(it);
// 错！erase 使后面的迭代器失效，++it 未定义
}对比 remove vs erase
remove(i)：保持索引稳定、在槽位留下nullptr；size()不变，nrFactors()减少。适合保留索引稳定的场景（或延迟压缩）。erase(it)：紧缩（compact）容器、改变后续索引、size()变小。适合你不需要保留原始索引并想减少容器长度的情况。
额外细节、常见坑与建议
size()vsnrFactors()vsempty()size()返回底层容器长度（包含被remove()留下的nullptr槽）。nrFactors()返回实际非空因子数（遍历计数）。empty()返回底层容器是否为空 —— 如果你用remove()把所有槽置为nullptr，empty()仍然 可能为 false（因为容器还有元素，只是这些元素为nullptr）。这点常被误用/误解。
如何复用
remove()留下的空槽- GTSAM 的
add_factors(..., useEmptySlots=true)路径会尝试找到空槽并复用它们，从而避免容器不断增长。源码里有这段逻辑（查找!at(i)的槽并放入新因子）。如果你采用remove并希望重用位置，优先使用该接口。
- GTSAM 的
并发与线程安全
- 这些操作不是线程安全的。任何修改容器（
push_back、erase、resize等）在并发场景下都需要外部同步。
- 这些操作不是线程安全的。任何修改容器（
当外部缓存“因子索引”时
如果你在外部代码保存了某个因子的 index（size_t），并期望之后通过该索引访问该因子：
- 若你使用
remove(index)：索引仍然有效，但exists(index)可能变为 false（因为槽为nullptr）。你需要检查exists()。 - 若你使用
erase删除了更前面的元素，则该外部索引会失效（因为索引右移）。避开这个问题的策略：不要保存索引；改用shared_ptr或把因子某个稳定的 ID/Key 写进因子里并用查询来查找。
- 若你使用
如何“压缩”掉所有空槽（erase-remove 惯用法）
- 若你使用
remove产生了一堆nullptr，你可以用std::remove+erase把它们一次性清理掉（erase-remove 惯用法），例如：
- 若你使用
auto new_end = std::remove(g.begin(), g.end(), nullptr);
// 需要 <algorithm>
g.erase(new_end, g.end());
// range-erase这会把所有 nullptr 移到末尾并一次性擦除（更高效）。（标准 erase-remove 惯用法参见 cppreference & 常见资料）。([C++参考文献][4])
小示例（演示三种操作的不同效果）
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <iostream>
using namespace gtsam;
void print_indices(const NonlinearFactorGraph& g) {
std::cout <<
"size()=" << g.size() <<
" nrFactors()=" << g.nrFactors() <<
"\n";
for (size_t i=0;i<g.size();
++i) {
std::cout << i <<
": " <<
(g.exists(i) ? "live" : "null") <<
"\n";
}
}
int main() {
NonlinearFactorGraph g;
// 假设 addFactorA/B/C 等都返回 shared_ptr<NonlinearFactor>
g.push_back(/*factorA*/);
g.push_back(/*factorB*/);
g.push_back(/*factorC*/);
print_indices(g);
// remove index 1
g.remove(1);
std::cout <<
"after remove(1):\n";
print_indices(g);
// replace index 1 (fill it again)
g.replace(1, /*newFactor*/);
std::cout <<
"after replace(1):\n";
print_indices(g);
// erase the first factor (index 0)
auto it = g.begin();
g.erase(it);
// now original index 1 的因子会下移到 index 0
std::cout <<
"after erase(begin()):\n";
print_indices(g);
}综合示例
下面是一个 完整可编译的小测试程序，用来演示 GTSAM 的 NonlinearFactorGraph 中
remove(size_t i)replace(size_t index, sharedFactor factor)iterator erase(iterator item)
对 索引 和 nrFactors() / size() 的影响。
测试程序
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <iostream>
using namespace std;
using namespace gtsam;
int main() {
NonlinearFactorGraph graph;
// 添加几个 PriorFactor<Pose2>
for (int i = 0; i <
5; i++) {
Pose2 pose(0.1 * i, 0.2 * i, 0.3 * i);
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
graph.add(make_shared<PriorFactor<Pose2>>
(Symbol('x', i), pose, model));
}
cout <<
"初始: graph.size()=" << graph.size()
<<
" nrFactors()=" << graph.nrFactors() << endl;
// ---------- remove ----------
cout <<
"\n调用 remove(2) ..." << endl;
graph.remove(2);
cout <<
"remove 后: graph.size()=" << graph.size()
<<
" nrFactors()=" << graph.nrFactors() << endl;
for (size_t i = 0; i < graph.size(); i++) {
cout <<
" index " << i <<
": "
<<
(graph[i] ? "valid" : "nullptr") << endl;
}
// ---------- replace ----------
cout <<
"\n调用 replace(1, newFactor) ..." << endl;
auto newFactor = make_shared<PriorFactor<Pose2>>
(
Symbol('x', 99), Pose2(9, 9, 9),
noiseModel::Diagonal::Sigmas(Vector3(1, 1, 1)));
graph.replace(1, newFactor);
cout <<
"replace 后: graph.size()=" << graph.size()
<<
" nrFactors()=" << graph.nrFactors() << endl;
for (size_t i = 0; i < graph.size(); i++) {
cout <<
" index " << i <<
": "
<<
(graph[i] ? "valid" : "nullptr") << endl;
}
// ---------- erase ----------
cout <<
"\n调用 erase(graph.begin()+0) ..." << endl;
graph.erase(graph.begin());
cout <<
"erase 后: graph.size()=" << graph.size()
<<
" nrFactors()=" << graph.nrFactors() << endl;
for (size_t i = 0; i < graph.size(); i++) {
cout <<
" index " << i <<
": "
<<
(graph[i] ? "valid" : "nullptr") << endl;
}
return 0;
}输出结果
假设初始有 5 个因子，运行结果是：
初始: graph.size()=5 nrFactors()=5
调用 remove(2) ...
remove 后: graph.size()=5 nrFactors()=4
index 0: valid
index 1: valid
index 2: nullptr
index 3: valid
index 4: valid
调用 replace(1, newFactor) ...
replace 后: graph.size()=5 nrFactors()=4
index 0: valid
index 1: valid <-- 被新因子替换
index 2: nullptr
index 3: valid
index 4: valid
调用 erase(graph.begin()+0) ...
erase 后: graph.size()=4 nrFactors()=3
index 0: valid <-- 原 index 1 的元素前移
index 1: nullptr
index 2: valid
index 3: valid总结
remove(i)- 不会改变
size()（容量不变）。 - 对应位置变为
nullptr，nrFactors()减少 1。
- 不会改变
replace(i, f)- 不改变
size()。 - 用新因子替换旧因子（若原来是
nullptr，则nrFactors()增加 1）。
- 不改变
erase(it)- 真的删除该元素，后面的因子前移。
size()和nrFactors()同时减少 1。
参考
FactorGraph.h（实现remove/replace/erase/size/at/...）：源码（包含实现位置）。FactorGraph-inst.h（nrFactors()、add_factors(..., useEmptySlots)的实现）：源码。NonlinearFactorGraph.h（NonlinearFactorGraph继承自FactorGraph<NonlinearFactor>，因此上面行为适用于NonlinearFactorGraph）。FastVector（实际上是std::vector的别名，使用自定义 allocator）：文档说明。std::vector::erase（语义、迭代器失效、返回值、复杂度）：cppreference（权威 C++ 参考）。
浙公网安备 33010602011771号