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

Added error log message when graph optimization error is huge (error ratio over > 100) and RGBD/OptimizeMaxError is disabled.

Browse Source
This commit is contained in:
matlabbe
2023-07-03 18:04:54 -07:00
parent b8c298efd9
commit 64f79813cd
1 changed files with 257 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

361
corelib/src/Rtabmap.cpp
View File

@@ -3169,7 +3169,7 @@ bool Rtabmap::process(
UWARN("Optimization failed, rejecting localization!"); UWARN("Optimization failed, rejecting localization!");
rejectLocalization = true; rejectLocalization = true;
} }
else if(_optimizationMaxError > 0.0f) else
{ {
UINFO("Compute max graph errors..."); UINFO("Compute max graph errors...");
const Link * maxLinearLink = 0; const Link * maxLinearLink = 0;
@@ -3199,7 +3199,7 @@ bool Rtabmap::process(
maxLinearLink->transVariance(), maxLinearLink->transVariance(),
maxLinearError/sqrt(maxLinearLink->transVariance()), maxLinearError/sqrt(maxLinearLink->transVariance()),
_optimizationMaxError); _optimizationMaxError);
if(maxLinearErrorRatio > _optimizationMaxError) if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
{ {
UWARN("Rejecting localization (%d <-> %d) in this " UWARN("Rejecting localization (%d <-> %d) in this "
"iteration because a wrong loop closure has been " "iteration because a wrong loop closure has been "
@@ -3218,6 +3218,19 @@ bool Rtabmap::process(
_optimizationMaxError); _optimizationMaxError);
rejectLocalization = true; rejectLocalization = true;
} }
else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100)
{
UERROR("Huge optimization error detected!"
"Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
maxLinearErrorRatio,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearLink->type(),
maxLinearError,
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str());
}
} }
if(maxAngularLink) if(maxAngularLink)
{ {
@@ -3228,7 +3241,7 @@ bool Rtabmap::process(
maxAngularLink->rotVariance(), maxAngularLink->rotVariance(),
maxAngularError/sqrt(maxAngularLink->rotVariance()), maxAngularError/sqrt(maxAngularLink->rotVariance()),
_optimizationMaxError); _optimizationMaxError);
if(maxAngularErrorRatio > _optimizationMaxError) if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
{ {
UWARN("Rejecting localization (%d <-> %d) in this " UWARN("Rejecting localization (%d <-> %d) in this "
"iteration because a wrong loop closure has been " "iteration because a wrong loop closure has been "
@@ -3247,6 +3260,19 @@ bool Rtabmap::process(
_optimizationMaxError); _optimizationMaxError);
rejectLocalization = true; rejectLocalization = true;
} }
else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100)
{
UERROR("Huge optimization error detected!"
"Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
maxAngularErrorRatio,
maxAngularLink->from(),
maxAngularLink->to(),
maxAngularLink->type(),
maxAngularError*180.0f/CV_PI,
sqrt(maxAngularLink->rotVariance()),
Parameters::kRGBDOptimizeMaxError().c_str());
}
} }
} }
@@ -3283,7 +3309,7 @@ bool Rtabmap::process(
UWARN("Optimization failed, rejecting localization!"); UWARN("Optimization failed, rejecting localization!");
rejectLocalization = true; rejectLocalization = true;
} }
else if(_optimizationMaxError > 0.0f) else
{ {
UINFO("Compute max graph errors..."); UINFO("Compute max graph errors...");
const Link * maxLinearLink = 0; const Link * maxLinearLink = 0;
@@ -3313,7 +3339,7 @@ bool Rtabmap::process(
maxLinearLink->transVariance(), maxLinearLink->transVariance(),
maxLinearError/sqrt(maxLinearLink->transVariance()), maxLinearError/sqrt(maxLinearLink->transVariance()),
_optimizationMaxError); _optimizationMaxError);
if(maxLinearErrorRatio > _optimizationMaxError) if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
{ {
UWARN("Rejecting localization (%d <-> %d) in this " UWARN("Rejecting localization (%d <-> %d) in this "
"iteration because a wrong loop closure has been " "iteration because a wrong loop closure has been "
@@ -3332,6 +3358,19 @@ bool Rtabmap::process(
_optimizationMaxError); _optimizationMaxError);
rejectLocalization = true; rejectLocalization = true;
} }
else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100)
{
UERROR("Huge optimization error detected!"
"Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
maxLinearErrorRatio,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearLink->type(),
maxLinearError,
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str());
}
} }
if(maxAngularLink) if(maxAngularLink)
{ {
@@ -3342,7 +3381,7 @@ bool Rtabmap::process(
maxAngularLink->rotVariance(), maxAngularLink->rotVariance(),
maxAngularError/sqrt(maxAngularLink->rotVariance()), maxAngularError/sqrt(maxAngularLink->rotVariance()),
_optimizationMaxError); _optimizationMaxError);
if(maxAngularErrorRatio > _optimizationMaxError) if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
{ {
UWARN("Rejecting localization (%d <-> %d) in this " UWARN("Rejecting localization (%d <-> %d) in this "
"iteration because a wrong loop closure has been " "iteration because a wrong loop closure has been "
@@ -3361,6 +3400,19 @@ bool Rtabmap::process(
_optimizationMaxError); _optimizationMaxError);
rejectLocalization = true; rejectLocalization = true;
} }
else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100)
{
UERROR("Huge optimization error detected!"
"Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
maxAngularErrorRatio,
maxAngularLink->from(),
maxAngularLink->to(),
maxAngularLink->type(),
maxAngularError*180.0f/CV_PI,
sqrt(maxAngularLink->rotVariance()),
Parameters::kRGBDOptimizeMaxError().c_str());
}
} }
} }
} }
@@ -3597,7 +3649,6 @@ bool Rtabmap::process(
rejectedLandmark = true; rejectedLandmark = true;
} }
else if(_memory->isIncremental() && else if(_memory->isIncremental() &&
_optimizationMaxError > 0.0f &&
loopClosureLinksAdded.size() && loopClosureLinksAdded.size() &&
optimizationIterations > 0 && optimizationIterations > 0 &&
constraints.size()) constraints.size())
@@ -3623,7 +3674,7 @@ bool Rtabmap::process(
if(maxLinearLink) if(maxLinearLink)
{ {
UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance())); UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
if(maxLinearErrorRatio > _optimizationMaxError) if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
{ {
UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this " UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
"iteration because a wrong loop closure has been " "iteration because a wrong loop closure has been "
@@ -3643,11 +3694,24 @@ bool Rtabmap::process(
_optimizationMaxError); _optimizationMaxError);
reject = true; reject = true;
} }
else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100)
{
UERROR("Huge optimization error detected!"
"Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
maxLinearErrorRatio,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearLink->type(),
maxLinearError,
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str());
}
} }
if(maxAngularLink) if(maxAngularLink)
{ {
UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance())); UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
if(maxAngularErrorRatio > _optimizationMaxError) if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
{ {
UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this " UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
"iteration because a wrong loop closure has been " "iteration because a wrong loop closure has been "
@@ -3667,6 +3731,19 @@ bool Rtabmap::process(
_optimizationMaxError); _optimizationMaxError);
reject = true; reject = true;
} }
else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100)
{
UERROR("Huge optimization error detected!"
"Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
maxAngularErrorRatio,
maxAngularLink->from(),
maxAngularLink->to(),
maxAngularLink->type(),
maxAngularError*180.0f/CV_PI,
sqrt(maxAngularLink->rotVariance()),
Parameters::kRGBDOptimizeMaxError().c_str());
}
} }
if(reject) if(reject)
@@ -5522,106 +5599,130 @@ int Rtabmap::detectMoreLoopClosures(
if(!t.isNull()) if(!t.isNull())
{ {
bool updateConstraints = true; bool updateConstraints = true;
if(_optimizationMaxError > 0.0f)
{
//optimize the graph to see if the new constraint is globally valid
int fromId = from; //optimize the graph to see if the new constraint is globally valid
int mapId = signatures.at(from).mapId();
// use first node of the map containing from int fromId = from;
for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster) int mapId = signatures.at(from).mapId();
// use first node of the map containing from
for(std::map<int, Signature>::iterator ster=signatures.begin(); ster!=signatures.end(); ++ster)
{
if(ster->second.mapId() == mapId)
{ {
if(ster->second.mapId() == mapId) fromId = ster->first;
break;
}
}
std::multimap<int, Link> linksIn = links;
linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance))));
const Link * maxLinearLink = 0;
const Link * maxAngularLink = 0;
float maxLinearError = 0.0f;
float maxAngularError = 0.0f;
float maxLinearErrorRatio = 0.0f;
float maxAngularErrorRatio = 0.0f;
std::map<int, Transform> optimizedPoses;
std::multimap<int, Link> links;
UASSERT(poses.find(fromId) != poses.end());
UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
UASSERT(graph::findLink(links, from, to) != links.end());
optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
std::string msg;
if(optimizedPoses.size())
{
graph::computeMaxGraphErrors(
optimizedPoses,
links,
maxLinearErrorRatio,
maxAngularErrorRatio,
maxLinearError,
maxAngularError,
&maxLinearLink,
&maxAngularLink);
if(maxLinearLink)
{
UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
{ {
fromId = ster->first; msg = uFormat("Rejecting edge %d->%d because "
break; "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
"\"%s\" is %f.",
from,
to,
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearErrorRatio,
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError);
}
else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100)
{
UERROR("Huge optimization error detected!"
"Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
maxLinearErrorRatio,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearLink->type(),
maxLinearError,
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str());
} }
} }
std::multimap<int, Link> linksIn = links; else if(maxAngularLink)
linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, getInformation(info.covariance))));
const Link * maxLinearLink = 0;
const Link * maxAngularLink = 0;
float maxLinearError = 0.0f;
float maxAngularError = 0.0f;
float maxLinearErrorRatio = 0.0f;
float maxAngularErrorRatio = 0.0f;
std::map<int, Transform> optimizedPoses;
std::multimap<int, Link> links;
UASSERT(poses.find(fromId) != poses.end());
UASSERT_MSG(poses.find(from) != poses.end(), uFormat("id=%d poses=%d links=%d", from, (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(poses.find(to) != poses.end(), uFormat("id=%d poses=%d links=%d", to, (int)poses.size(), (int)links.size()).c_str());
_graphOptimizer->getConnectedGraph(fromId, poses, linksIn, optimizedPoses, links);
UASSERT(optimizedPoses.find(fromId) != optimizedPoses.end());
UASSERT_MSG(optimizedPoses.find(from) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", from, (int)optimizedPoses.size(), (int)links.size()).c_str());
UASSERT_MSG(optimizedPoses.find(to) != optimizedPoses.end(), uFormat("id=%d poses=%d links=%d", to, (int)optimizedPoses.size(), (int)links.size()).c_str());
UASSERT(graph::findLink(links, from, to) != links.end());
optimizedPoses = _graphOptimizer->optimize(fromId, optimizedPoses, links);
std::string msg;
if(optimizedPoses.size())
{ {
graph::computeMaxGraphErrors( UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
optimizedPoses, if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
links,
maxLinearErrorRatio,
maxAngularErrorRatio,
maxLinearError,
maxAngularError,
&maxLinearLink,
&maxAngularLink);
if(maxLinearLink)
{ {
UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to()); msg = uFormat("Rejecting edge %d->%d because "
if(maxLinearErrorRatio > _optimizationMaxError) "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
{ "\"%s\" is %f m.",
msg = uFormat("Rejecting edge %d->%d because " from,
"graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). " to,
"\"%s\" is %f.", maxAngularError*180.0f/M_PI,
from, maxAngularLink->from(),
to, maxAngularLink->to(),
maxLinearError, maxAngularErrorRatio,
maxLinearLink->from(), sqrt(maxAngularLink->rotVariance()),
maxLinearLink->to(), Parameters::kRGBDOptimizeMaxError().c_str(),
maxLinearErrorRatio, _optimizationMaxError);
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError);
}
} }
else if(maxAngularLink) else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100)
{ {
UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to()); UERROR("Huge optimization error detected!"
if(maxAngularErrorRatio > _optimizationMaxError) "Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
{ "enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
msg = uFormat("Rejecting edge %d->%d because " maxAngularErrorRatio,
"graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). " maxAngularLink->from(),
"\"%s\" is %f m.", maxAngularLink->to(),
from, maxAngularLink->type(),
to, maxAngularError*180.0f/CV_PI,
maxAngularError*180.0f/M_PI, sqrt(maxAngularLink->rotVariance()),
maxAngularLink->from(), Parameters::kRGBDOptimizeMaxError().c_str());
maxAngularLink->to(),
maxAngularErrorRatio,
sqrt(maxAngularLink->rotVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError);
}
} }
} }
else }
{ else
msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", {
from, msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
to); from,
} to);
if(!msg.empty()) }
{ if(!msg.empty())
UWARN("%s", msg.c_str()); {
updateConstraints = false; UWARN("%s", msg.c_str());
} updateConstraints = false;
else }
{ else
poses = optimizedPoses; {
} poses = optimizedPoses;
} }
if(updateConstraints) if(updateConstraints)
@@ -5904,7 +6005,7 @@ bool Rtabmap::addLink(const Link & link)
{ {
msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", link.from(), link.to()); msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!", link.from(), link.to());
} }
else if(_optimizationMaxError > 0.0f) else
{ {
float maxLinearError = 0.0f; float maxLinearError = 0.0f;
float maxLinearErrorRatio = 0.0f; float maxLinearErrorRatio = 0.0f;
@@ -5925,7 +6026,7 @@ bool Rtabmap::addLink(const Link & link)
if(maxLinearLink) if(maxLinearLink)
{ {
UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to()); UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
if(maxLinearErrorRatio > _optimizationMaxError) if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
{ {
msg = uFormat("Rejecting edge %d->%d because " msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). " "graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
@@ -5940,11 +6041,24 @@ bool Rtabmap::addLink(const Link & link)
Parameters::kRGBDOptimizeMaxError().c_str(), Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError); _optimizationMaxError);
} }
else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100)
{
UERROR("Huge optimization error detected!"
"Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
maxLinearErrorRatio,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearLink->type(),
maxLinearError,
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str());
}
} }
else if(maxAngularLink) else if(maxAngularLink)
{ {
UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to()); UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
if(maxAngularErrorRatio > _optimizationMaxError) if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
{ {
msg = uFormat("Rejecting edge %d->%d because " msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). " "graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
@@ -5959,6 +6073,19 @@ bool Rtabmap::addLink(const Link & link)
Parameters::kRGBDOptimizeMaxError().c_str(), Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError); _optimizationMaxError);
} }
else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100)
{
UERROR("Huge optimization error detected!"
"Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
maxAngularErrorRatio,
maxAngularLink->from(),
maxAngularLink->to(),
maxAngularLink->type(),
maxAngularError*180.0f/CV_PI,
sqrt(maxAngularLink->rotVariance()),
Parameters::kRGBDOptimizeMaxError().c_str());
}
} }
} }
if(!msg.empty()) if(!msg.empty())
@@ -6063,7 +6190,7 @@ bool Rtabmap::addLink(const Link & link)
UWARN("Optimization failed, rejecting localization!"); UWARN("Optimization failed, rejecting localization!");
rejectLocalization = true; rejectLocalization = true;
} }
else if(_optimizationMaxError > 0.0f) else
{ {
UINFO("Compute max graph errors..."); UINFO("Compute max graph errors...");
float maxLinearError = 0.0f; float maxLinearError = 0.0f;
@@ -6090,7 +6217,7 @@ bool Rtabmap::addLink(const Link & link)
if(maxLinearLink) if(maxLinearLink)
{ {
UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance())); UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
if(maxLinearErrorRatio > _optimizationMaxError) if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
{ {
UWARN("Rejecting localization (%d <-> %d) in this " UWARN("Rejecting localization (%d <-> %d) in this "
"iteration because a wrong loop closure has been " "iteration because a wrong loop closure has been "
@@ -6109,11 +6236,24 @@ bool Rtabmap::addLink(const Link & link)
_optimizationMaxError); _optimizationMaxError);
rejectLocalization = true; rejectLocalization = true;
} }
else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100)
{
UERROR("Huge optimization error detected!"
"Linear error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
maxLinearErrorRatio,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearLink->type(),
maxLinearError,
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str());
}
} }
if(maxAngularLink) if(maxAngularLink)
{ {
UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance())); UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
if(maxAngularErrorRatio > _optimizationMaxError) if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
{ {
UWARN("Rejecting localization (%d <-> %d) in this " UWARN("Rejecting localization (%d <-> %d) in this "
"iteration because a wrong loop closure has been " "iteration because a wrong loop closure has been "
@@ -6132,6 +6272,19 @@ bool Rtabmap::addLink(const Link & link)
_optimizationMaxError); _optimizationMaxError);
rejectLocalization = true; rejectLocalization = true;
} }
else if(_optimizationMaxError == 0.0f && maxAngularErrorRatio>100)
{
UERROR("Huge optimization error detected!"
"Angular error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). You may consider "
"enabling \"%s\" to reject those bad optimizations by setting it to a non null value!",
maxAngularErrorRatio,
maxAngularLink->from(),
maxAngularLink->to(),
maxAngularLink->type(),
maxAngularError*180.0f/CV_PI,
sqrt(maxAngularLink->rotVariance()),
Parameters::kRGBDOptimizeMaxError().c_str());
}
} }
} }