i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add/Refine/SBA: create Registration only once to avoid internal initialization for every link refined, added or rematched (e.g., SuperPoint or SuperGlue).

Browse Source
This commit is contained in:
matlabbe
2025-05-17 16:34:31 -07:00
parent f03139d4ff
commit 802b8f9870
6 changed files with 54 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
guilib/src/DatabaseViewer.cpp
View File

@@ -4263,6 +4263,8 @@ void DatabaseViewer::detectMoreLoopClosures()
return;
}
std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
for(int n=0; n<iterations; ++n)
{
UINFO("iteration %d/%d", n+1, iterations);
@@ -4310,7 +4312,7 @@ void DatabaseViewer::detectMoreLoopClosures()
delta.getNorm() >= ui_->doubleSpinBox_detectMore_radiusMin->value())
{
checkedLoopClosures.insert(std::make_pair(from, to));
if(addConstraint(from, to, true, useOptimizedGraphAsGuess))
if(addConstraint(from, to, reg.get(), true, useOptimizedGraphAsGuess))
{
UINFO("Added new loop closure between %d and %d.", from, to);
++added;
@@ -4568,13 +4570,16 @@ void DatabaseViewer::refineLinks(const QList<Link> & links)
progressDialog->setMinimumWidth(800);
progressDialog->show();
RegistrationIcp regProximity(ui_->parameters_toolbox->getParameters());
std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
for(int i=0; i<links.size(); ++i)
{
int from = links[i].from();
int to = links[i].to();
if(from > 0 && to > 0)
{
this->refineConstraint(links[i].from(), links[i].to(), true);
this->refineConstraint(links[i].from(), links[i].to(), reg.get(), &regProximity, true);
progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
}
else
@@ -7140,7 +7145,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
}
if(!allNodesAreInWM)
{
ui_->graphViewer->updatePosterior(colors, 1, 1);
ui_->graphViewer->updateNodeColorByValue("In WM", colors, 1, false, 1);
}
}
QGraphicsRectItem * rectScaleItem = 0;
@@ -8085,10 +8090,12 @@ void DatabaseViewer::refineConstraint()
{
int from = ids_.at(ui_->horizontalSlider_A->value());
int to = ids_.at(ui_->horizontalSlider_B->value());
refineConstraint(from, to, false);
RegistrationIcp regProximity(ui_->parameters_toolbox->getParameters());
std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
refineConstraint(from, to, reg.get(), &regProximity, false);
}
void DatabaseViewer::refineConstraint(int from, int to, bool silent)
void DatabaseViewer::refineConstraint(int from, int to, Registration * reg, RegistrationIcp * regProximity, bool silent)
{
UDEBUG("%d -> %d", from, to);
bool switchedIds = false;
@@ -8361,8 +8368,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
toS = new Signature(assembledData);
RegistrationIcp registrationIcp(parameters);
transform = registrationIcp.computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
transform = regProximity->computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
if(!transform.isNull())
{
// local scan matching proximity detection should have higher variance (see Rtabmap::process())
@@ -8380,7 +8386,6 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
}
bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
Registration * reg = Registration::create(parameters);
if( reg->isScanRequired() ||
reg->isUserDataRequired() ||
reextractVisualFeatures ||
@@ -8477,8 +8482,6 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
transform = reg->computeTransformationMod(*toS, *fromS, t.isNull()?t:t.inverse(), &info);
switchedIds = true;
}
delete reg;
}
UINFO("(%d ->%d) Registration time: %f s", currentLink.from(), currentLink.to(), timer.ticks());
@@ -8610,11 +8613,14 @@ void DatabaseViewer::addConstraint()
{
int from = ids_.at(ui_->horizontalSlider_A->value());
int to = ids_.at(ui_->horizontalSlider_B->value());
addConstraint(from, to, false);
std::shared_ptr<Registration> reg(Registration::create(ui_->parameters_toolbox->getParameters()));
addConstraint(from, to, reg.get(), false);
}
bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess)
bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool silent, bool silentlyUseOptimizedGraphAsGuess)
{
UASSERT(reg);
bool switchedIds = false;
if(from == to)
{
@@ -8641,7 +8647,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyU
UASSERT(!containsLink(linksRefined_, from, to));
ParametersMap parameters = ui_->parameters_toolbox->getParameters();
Registration * reg = Registration::create(parameters);
bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
@@ -8823,7 +8828,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyU
{
t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
}
delete reg;
UDEBUG("");
if(!t.isNull())

12
guilib/src/MainWindow.cpp
View File

@@ -6623,6 +6623,8 @@ void MainWindow::postProcessing(
odomMaxInf = graph::getMaxOdomInf(_currentLinksMap);
}
std::shared_ptr<Registration> registration(Registration::create(parameters));
UASSERT(iterations>0);
for(int n=0; n<iterations && !_progressCanceled; ++n)
{
@@ -6703,7 +6705,6 @@ void MainWindow::postProcessing(
{
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
}
Registration * registration = Registration::create(parameters);
if(reextractFeatures)
{
@@ -6735,7 +6736,6 @@ void MainWindow::postProcessing(
Parameters::kRGBDLoopClosureReextractFeatures().c_str());
}
transform = registration->computeTransformation(signatureFrom, signatureTo, Transform(), &info);
delete registration;
if(!transform.isNull())
{
//optimize the graph to see if the new constraint is globally valid
@@ -7023,7 +7023,13 @@ void MainWindow::postProcessing(
uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(sbaVariance)));
Optimizer * sbaOptimizer = Optimizer::create(sbaType, parametersSBA);
std::map<int, Transform> newPoses = sbaOptimizer->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap(), sbaRematchFeatures);
std::map<int, Transform> newPoses = sbaOptimizer->optimizeBA(
optimizedPoses.begin()->first,
optimizedPoses,
linksOut,
_cachedSignatures.toStdMap(),
sbaRematchFeatures,
parametersSBA);
delete sbaOptimizer;
if(newPoses.size())
{