46 const std::optional<
FastList<Key> >& keysToMove,
const std::optional< FactorIndices >& removeFactorIndices) {
51 const bool debug =
false;
53 if(
debug) std::cout <<
"ConcurrentIncrementalFilter::update Begin" << std::endl;
61 if(removeFactorIndices){
62 removedFactors.insert(removedFactors.end(), removeFactorIndices->begin(), removeFactorIndices->end());
66 std::optional<FastMap<Key,int> > orderingConstraints = {};
67 if(keysToMove && keysToMove->size() > 0) {
73 orderingConstraints->operator[](
key) = group;
78 for(
const auto key: newTheta.
keys()) {
79 orderingConstraints->operator[](
key) = group;
83 orderingConstraints->operator[](
key) = 0;
89 std::optional<FastList<Key> > noRelinKeys = {};
92 std::optional<FastList<Key> > additionalKeys = {};
93 if(keysToMove && keysToMove->size() > 0) {
94 std::set<Key> markedKeys;
95 for(
Key key: *keysToMove) {
99 while(*key_iter !=
key) {
100 markedKeys.insert(*key_iter);
108 additionalKeys =
FastList<Key>(markedKeys.begin(), markedKeys.end());
113 ISAM2Result isam2Result =
isam2_.
update(newFactors, newTheta, removedFactors, orderingConstraints, noRelinKeys, additionalKeys);
116 if(keysToMove && keysToMove->size() > 0) {
118 gttic(cache_smoother_factors);
123 for(
size_t slot: removedFactorSlots) {
131 for(
Key key: *keysToMove) {
134 gttoc(cache_smoother_factors);
141 for(
size_t index: deletedFactorsIndices) {
159 if(
debug) std::cout <<
"ConcurrentIncrementalFilter::update End" << std::endl;
179 const bool debug =
false;
181 if(
debug) std::cout <<
"ConcurrentIncrementalFilter::synchronize Begin" << std::endl;
195 values.insert(smootherSummarizationValues);
196 for(
Key key: newSeparatorKeys) {
205 noRelinKeys.push_back(
key);
221 if(
debug) std::cout <<
"ConcurrentIncrementalFilter::synchronize End" << std::endl;
229 gttic(get_summarized_factors);
239 gttoc(get_summarized_factors);
245 gttic(get_smoother_factors);
251 gttoc(get_smoother_factors);
271 if(std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(),
key) != clique->conditional()->endParents()) {
274 for(
Key idx: clique->conditional()->frontals()) {
275 additionalKeys.insert(idx);
294 const auto& slots = variableIndex[
key];
295 removedFactorSlots.insert(removedFactorSlots.end(), slots.begin(), slots.end());
299 std::sort(removedFactorSlots.begin(), removedFactorSlots.end());
300 removedFactorSlots.erase(std::unique(removedFactorSlots.begin(), removedFactorSlots.end()), removedFactorSlots.end());
303 for(
size_t index: factorsToIgnore) {
304 removedFactorSlots.erase(std::remove(removedFactorSlots.begin(), removedFactorSlots.end(), index), removedFactorSlots.end());
307 return removedFactorSlots;
319 shortcutKeys.insert(factor->begin(), factor->end());
323 shortcutKeys.insert(
key);
328 graph.push_back(removedFactors);
349 std::set<ISAM2Clique::shared_ptr> separatorCliques;
350 for(
Key key: separatorKeys) {
352 separatorCliques.insert( clique );
358 for(
Key key: clique->conditional()->frontals()) {
359 cliqueKeys.push_back(
key);
362 std::sort(cliqueKeys.begin(), cliqueKeys.end());
365 std::set<size_t> cliqueFactorSlots;
366 for(
Key key: cliqueKeys) {
370 std::set<Key> factorKeys(factor->begin(), factor->end());
371 if(std::includes(cliqueKeys.begin(), cliqueKeys.end(), factorKeys.begin(), factorKeys.end())) {
372 cliqueFactorSlots.insert(slot);
380 cliqueFactorSlots.erase(slot);
385 for(
size_t slot: cliqueFactorSlots) {
390 std::set<ISAM2Clique::shared_ptr> childCliques;
393 childCliques.insert(clique->children.begin(), clique->children.end());
397 childCliques.erase(clique);
403 graph.push_back( factor );
410 return filterSummarization;