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

fixed #276 and #277

Browse Source
This commit is contained in:
matlabbe
2018-04-14 13:41:04 -04:00
parent 79c4bd7850
commit 344dc165bc
3 changed files with 21 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
corelib/src/Optimizer.cpp
View File

@@ -279,11 +279,14 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
UDEBUG("Incremental optimization... poses=%d comstraints=%d", (int)poses.size(), (int)constraints.size()); UDEBUG("Incremental optimization... poses=%d comstraints=%d", (int)poses.size(), (int)constraints.size());
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{ {
incGraph.insert(*iter);
bool hasLoopClosure = false; bool hasLoopClosure = false;
for(std::multimap<int, Link>::iterator jter=constraintsCpy.lower_bound(iter->first); jter!=constraintsCpy.end() && jter->first==iter->first; ++jter) for(std::multimap<int, Link>::iterator jter=constraintsCpy.lower_bound(iter->first); jter!=constraintsCpy.end() && jter->first==iter->first; ++jter)
{ {
UDEBUG("%d: %d -> %d type=%d", iter->first, jter->second.from(), jter->second.to(), jter->second.type());
if(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged) if(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)
{ {
UASSERT(uContains(incGraph, iter->first));
incGraph.insert(std::make_pair(jter->second.to(), incGraph.at(iter->first) * jter->second.transform())); incGraph.insert(std::make_pair(jter->second.to(), incGraph.at(iter->first) * jter->second.transform()));
incGraphLinks.insert(*jter); incGraphLinks.insert(*jter);
} }
@@ -316,6 +319,8 @@ std::map<int, Transform> Optimizer::optimizeIncremental(
if(!incGraph.empty() && incGraph.size() == poses.size()) if(!incGraph.empty() && incGraph.size() == poses.size())
{ {
UASSERT(incGraphLinks.size() == constraints.size()); UASSERT(incGraphLinks.size() == constraints.size());
UASSERT(uContains(poses, rootId) && uContains(incGraph, rootId));
incGraph.at(rootId) = poses.at(rootId);
return this->optimize(rootId, incGraph, incGraphLinks, intermediateGraphes, finalError, iterationsDone); return this->optimize(rootId, incGraph, incGraphLinks, intermediateGraphes, finalError, iterationsDone);
} }

22
corelib/src/Rtabmap.cpp
View File

@@ -2268,10 +2268,18 @@ bool Rtabmap::process(
// if _optimizeFromGraphEnd parameter just changed state, don't use optimized poses as guess // if _optimizeFromGraphEnd parameter just changed state, don't use optimized poses as guess
float normMapCorrection = _mapCorrection.getNormSquared(); // use distance for identity detection float normMapCorrection = _mapCorrection.getNormSquared(); // use distance for identity detection
if((normMapCorrection > 0.001f && _optimizeFromGraphEnd) || if((normMapCorrection > 0.000001f && _optimizeFromGraphEnd) ||
(normMapCorrection < 0.001f && !_optimizeFromGraphEnd)) (normMapCorrection < 0.000001f && !_optimizeFromGraphEnd))
{ {
poses.clear(); for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
{
if(iter->second.type() != Link::kNeighbor && iter->second.type() != Link::kVirtualClosure)
{
UWARN("Optimization: clearing guess poses as %s may have changed state, now %s (normMapCorrection=%f)", Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false", normMapCorrection);
poses.clear();
break;
}
}
} }
std::multimap<int, Link> constraints; std::multimap<int, Link> constraints;
@@ -3311,24 +3319,24 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
if(!poses.empty() && optimizedPoses.empty() && guessPoses.empty()) if(!poses.empty() && optimizedPoses.empty() && guessPoses.empty())
{ {
UERROR("Optimization has failed, trying incremental optimization instead, this may take a while (poses=%d, links=%d)...", (int)poses.size(), (int)edgeConstraints.size()); UWARN("Optimization has failed, trying incremental optimization instead, this may take a while (poses=%d, links=%d)...", (int)poses.size(), (int)edgeConstraints.size());
optimizedPoses = _graphOptimizer->optimizeIncremental(fromId, poses, edgeConstraints, 0, error, iterationsDone); optimizedPoses = _graphOptimizer->optimizeIncremental(fromId, poses, edgeConstraints, 0, error, iterationsDone);
if(optimizedPoses.empty()) if(optimizedPoses.empty())
{ {
if(!_graphOptimizer->isCovarianceIgnored() || _graphOptimizer->type() != Optimizer::kTypeTORO) if(!_graphOptimizer->isCovarianceIgnored() || _graphOptimizer->type() != Optimizer::kTypeTORO)
{ {
UERROR("Incremental optimization also failed. You may try changing parameters to %s=0 and %s=true.", UWARN("Incremental optimization also failed. You may try changing parameters to %s=0 and %s=true.",
Parameters::kOptimizerStrategy().c_str(), Parameters::kOptimizerVarianceIgnored().c_str()); Parameters::kOptimizerStrategy().c_str(), Parameters::kOptimizerVarianceIgnored().c_str());
} }
else else
{ {
UERROR("Incremental optimization also failed."); UWARN("Incremental optimization also failed.");
} }
} }
else else
{ {
UERROR("Incremental optimization succeeded!"); UWARN("Incremental optimization succeeded!");
} }
} }
} }

1
tools/EurocDataset/main.cpp
View File

@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <yaml-cpp/yaml.h> #include <yaml-cpp/yaml.h>
#include <stdio.h> #include <stdio.h>
#include <signal.h> #include <signal.h>
#include <fstream>
using namespace rtabmap; using namespace rtabmap;