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

g2o: fixed vertigo index when using landmarks. graphViewer: fixed outlier wrong set to some landmark links

Browse Source
This commit is contained in:
matlabbe
2019-04-12 21:31:20 -04:00
parent 428d5be129
commit 7acaac7a6d
2 changed files with 2 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -432,7 +432,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
UDEBUG("fill edges to g2o..."); UDEBUG("fill edges to g2o...");
#if defined(RTABMAP_VERTIGO) #if defined(RTABMAP_VERTIGO)
int vertigoVertexId = landmarkVertexOffset - (poses.begin()->first<0?poses.begin()->first:0); int vertigoVertexId = landmarkVertexOffset - (poses.begin()->first<0?poses.begin()->first-1:0);
#endif #endif
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{ {

5
guilib/src/GraphViewer.cpp
View File

@@ -521,10 +521,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
if(iter->second.type() != Link::kNeighbor && if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged) iter->second.type() != Link::kNeighborMerged)
{ {
float linearError = uMax3( float linearError = fabs(iter->second.transform().getNorm() - t.getNorm());
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z()));
if(linearError > _loopClosureOutlierThr) if(linearError > _loopClosureOutlierThr)
{ {
linkItem->setColor(_loopClosureRejectedColor); linkItem->setColor(_loopClosureRejectedColor);