Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

IOS: extra optimisations doesn’t stack #1402

Open
APOS80 opened this issue Dec 8, 2024 · 3 comments
Open

IOS: extra optimisations doesn’t stack #1402

APOS80 opened this issue Dec 8, 2024 · 3 comments

Comments

@APOS80
Copy link

APOS80 commented Dec 8, 2024

Besides “standard optimisation” you can add “advanced” optimisations.

But the advanced optimisations doesn’t seems to be added, only one seems to be in affect at a time.

Choosing “Detect more loop closures” gives an improved result but when “Bundle adjustment” is chosen I get another result without the previous improvement. Choosing the previous setting takes me back to the first improvement.

Maybe I’m missing something. Is the advanced settings already applied in Standard optimisation?

@matlabbe
Copy link
Member

The post processing options are defined here:

int RTABMapApp::postProcessing(int approach)
{
postProcessing_ = true;
LOGI("postProcessing begin(%d)", approach);
int returnedValue = 0;
if(rtabmap_)
{
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
// detect more loop closures
if(approach == -1 || approach == 2)
{
if(approach == -1)
{
progressionStatus_.reset(6);
}
returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1, true, true, approach==-1?&progressionStatus_:0);
if(approach == -1 && progressionStatus_.isCanceled())
{
postProcessing_ = false;
return -1;
}
}
// graph optimization
if(returnedValue >=0)
{
if (approach == 1)
{
if(rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O))
{
std::map<int, rtabmap::Signature> signatures;
rtabmap_->getGraph(poses, links, true, true, &signatures);
rtabmap::ParametersMap param;
param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "30"));
param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0"));
rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param);
poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures);
delete sba;
}
else
{
UERROR("g2o not available!");
}
}
else if(approach!=4 && approach!=5 && approach != 7)
{
// simple graph optmimization
rtabmap_->getGraph(poses, links, true, true);
}
}
if(poses.size())
{
boost::mutex::scoped_lock lock(rtabmapMutex_);
rtabmap::Statistics stats = rtabmap_->getStatistics();
stats.setPoses(poses);
stats.setConstraints(links);
LOGI("PostProcessing, sending rtabmap event to update graph...");
rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats));
rtabmap_->setOptimizedPoses(poses, links);
}
else if(approach!=4 && approach!=5 && approach != 7)
{
returnedValue = -1;
}
if(returnedValue >=0)
{
boost::mutex::scoped_lock lock(renderingMutex_);
// filter polygons
if(approach == -1 || approach == 4)
{
filterPolygonsOnNextRender_ = true;
}
// gain compensation
if(approach == -1 || approach == 5 || approach == 6)
{
gainCompensationOnNextRender_ = approach == 6 ? 2 : 1; // 2 = full, 1 = fast
}
// bilateral filtering
if(approach == 7)
{
bilateralFilteringOnNextRender_ = true;
}
}
}
postProcessing_ = false;
LOGI("postProcessing end(%d) -> %d", approach, returnedValue);
return returnedValue;
}

with approach==-1 the "Standard Optimization" approach: it does "Detect more loop closures" -> "Graph optimization" -> "Polygon cleanup" -> "Fast gain compensation". Except for "Detect more loop closures", the post processing options are ephemeral, meaning if you reload the database, they won't be there. For example, "bundle adjustment" would be reverted to standard graph optimization after reloading the database or selecting another post-processing option affecting graph optimization. Same thing for gain compensation, it is not saved in database.

The only way to save those optimizations is to "Assemble" the map and save it, then the optimized/exported map will contain all optimizations, even on reload.

@APOS80
Copy link
Author

APOS80 commented Dec 10, 2024

So bundle adjustment disregards standard graph optimisation?

@matlabbe
Copy link
Member

It would use the current optimized poses to start the optimization from. Bundle adjustment is actually an improved version of the standard graph optimization (odometry links + loop closure links) by adding 2D-3D feature constraints in the optimization. Pure bundle adjustment would only care about the camera poses and the 2D-3D features, while in our version we also keep the odometry contraints and loop closure contraints between the camera poses.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants