12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Re-linearization

release/4.3a0
Frank Dellaert 2025-02-01 02:29:08 -05:00
parent ea27bac018
commit 7ee548ddd9
1 changed files with 22 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
examples/Hybrid_City10000.cpp
View File

@ -68,12 +68,12 @@ class Experiment {
size_t maxNrHypotheses = 10; size_t maxNrHypotheses = 10;
size_t reLinearizationFrequency = 1; size_t reLinearizationFrequency = 10;
private: private:
std::string filename_; std::string filename_;
HybridSmoother smoother_; HybridSmoother smoother_;
HybridNonlinearFactorGraph newFactors_; HybridNonlinearFactorGraph newFactors_, allFactors_;
Values initial_; Values initial_;
/** /**
@ -134,15 +134,34 @@ class Experiment {
/// @brief Perform smoother update and optimize the graph. /// @brief Perform smoother update and optimize the graph.
auto smootherUpdate(size_t maxNrHypotheses) { auto smootherUpdate(size_t maxNrHypotheses) {
std::cout << "Smoother update: " << newFactors_.size() << std::endl;
gttic_(SmootherUpdate); gttic_(SmootherUpdate);
clock_t beforeUpdate = clock(); clock_t beforeUpdate = clock();
auto linearized = newFactors_.linearize(initial_); auto linearized = newFactors_.linearize(initial_);
smoother_.update(*linearized, maxNrHypotheses); smoother_.update(*linearized, maxNrHypotheses);
allFactors_.push_back(newFactors_);
newFactors_.resize(0); newFactors_.resize(0);
clock_t afterUpdate = clock(); clock_t afterUpdate = clock();
return afterUpdate - beforeUpdate; return afterUpdate - beforeUpdate;
} }
/// @brief Re-linearize, solve ALL, and re-initialize smoother.
auto reInitialize() {
std::cout << "================= Re-Initialize: " << allFactors_.size()
<< std::endl;
clock_t beforeUpdate = clock();
allFactors_ = allFactors_.restrict(smoother_.fixedValues());
auto linearized = allFactors_.linearize(initial_);
auto bayesNet = linearized->eliminateSequential();
HybridValues delta = bayesNet->optimize();
initial_ = initial_.retract(delta.continuous());
smoother_.reInitialize(std::move(*bayesNet));
clock_t afterUpdate = clock();
std::cout << "Took " << (afterUpdate - beforeUpdate) / CLOCKS_PER_SEC
<< " seconds." << std::endl;
return afterUpdate - beforeUpdate;
}
// Parse line from file // Parse line from file
std::pair<std::vector<Pose2>, std::pair<size_t, size_t>> parseLine( std::pair<std::vector<Pose2>, std::pair<size_t, size_t>> parseLine(
const std::string& line) const { const std::string& line) const {
@ -238,17 +257,13 @@ class Experiment {
} }
if (numberOfHybridFactors >= updateFrequency) { if (numberOfHybridFactors >= updateFrequency) {
// print the keys involved in the smoother update
std::cout << "Smoother update: " << newFactors_.size() << std::endl;
auto time = smootherUpdate(maxNrHypotheses); auto time = smootherUpdate(maxNrHypotheses);
smootherUpdateTimes.push_back({index, time}); smootherUpdateTimes.push_back({index, time});
numberOfHybridFactors = 0; numberOfHybridFactors = 0;
updateCount++; updateCount++;
if (updateCount % reLinearizationFrequency == 0) { if (updateCount % reLinearizationFrequency == 0) {
std::cout << "Re-linearizing: " << newFactors_.size() << std::endl; reInitialize();
HybridValues delta = smoother_.optimize();
result.insert_or_assign(initial_.retract(delta.continuous()));
} }
} }