| 547 | } |
| 548 | |
| 549 | void FactorGraph::removeFactor(const FactorType CurrentFactorType, const double Timestamp) |
| 550 | { |
| 551 | if (_Structure.checkFactor(CurrentFactorType, Timestamp)) |
| 552 | { |
| 553 | /** loop over factors */ |
| 554 | ceres::ResidualBlockId CeresID; |
| 555 | for (int FactorNumber = _Structure.countFactor(CurrentFactorType, Timestamp); FactorNumber > 0; FactorNumber--) |
| 556 | { |
| 557 | FactorID Factor(CurrentFactorType, Timestamp, FactorNumber - 1); |
| 558 | _Structure.getResidualID(Factor, CeresID); |
| 559 | |
| 560 | /** remove factor in ceres */ |
| 561 | _Graph.RemoveResidualBlock(CeresID); |
| 562 | |
| 563 | /** remove factor in libRSF */ |
| 564 | _Structure.removeFactor(Factor); |
| 565 | } |
| 566 | } |
| 567 | else |
| 568 | { |
| 569 | PRINT_ERROR("Factor doesn't exist: ", CurrentFactorType, " at ", Timestamp); |
| 570 | } |
| 571 | } |
| 572 | |
| 573 | void FactorGraph::removeFactorsOutsideWindow(const FactorType CurrentFactorType, const double TimeWindow, const double CurrentTime) |
| 574 | { |
no test coverage detected