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

Added rtabmap::computePath() using a pose goal instead of node Id. Graph methods moved in rtabmap::graph namespace

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-06 13:52:02 -05:00
parent 3d1eccbce1
commit e3074d304c
6 changed files with 226 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
corelib/include/rtabmap/core/Graph.h
View File

@@ -36,6 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
namespace graph {
std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,
@@ -107,5 +109,17 @@ std::vector<int> RTABMAP_EXP computePath(
int from,
int to);
int RTABMAP_EXP findNearestNode(
const std::map<int, rtabmap::Transform> & nodes,
const rtabmap::Transform & targetPose);
std::map<int, float> RTABMAP_EXP getNodesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
int maxNearestNeighbors,
float radius);
} /* namespace graph */
} /* namespace rtabmap */
#endif /* GRAPH_H_ */

8
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -118,11 +118,13 @@ public:
bool optimized,
bool global);
void clearPath();
std::list<std::pair<int, Transform> > computePath(int targetNode);
std::list<std::pair<int, Transform> > computePath(int targetNode, bool global);
std::list<std::pair<int, Transform> > computePath(const Transform & targetPose, bool global);
const std::vector<int> & getPath() const {return _path;}
std::list<std::pair<int, Transform> > getPathNextPoses() const;
std::vector<int> getPathNextNodes() const;
int getPathGoalId() const;
int getPathCurrentGoalId() const;
const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
std::map<int, float> getNodesInRadius(int fromId, int maxNearestNeighbors, float radius) const;
std::map<int, Transform> getWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
@@ -136,6 +138,7 @@ private:
std::map<int, Transform> & optimizedPoses,
std::multimap<int, Link> * constraints = 0) const;
void updateGoalIndex();
bool computePath(int targetNode, const std::map<int, Transform> & nodes, const std::multimap<int, rtabmap::Link> & constraints);
void setupLogFiles(bool overwrite = false);
void flushStatisticLogs();
@@ -209,6 +212,7 @@ private:
std::vector<int> _path;
unsigned int _pathCurrentIndex;
unsigned int _pathGoalIndex;
Transform _pathTransformToGoal;
};

82
corelib/src/Graph.cpp
View File

@@ -39,6 +39,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
namespace graph {
std::multimap<int, Link>::iterator findLink(
std::multimap<int, Link> & links,
int from,
@@ -753,5 +755,85 @@ std::vector<int> computePath(
return uListToVector(path);
}
int findNearestNode(
const std::map<int, rtabmap::Transform> & nodes,
const rtabmap::Transform & targetPose)
{
int id = 0;
if(nodes.size() && !targetPose.isNull())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(nodes.size());
std::vector<int> ids(nodes.size());
int oi = 0;
for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
{
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first;
}
std::map<int, float> foundNodes;
if(cloud->size())
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(cloud);
std::vector<int> ind;
std::vector<float> dist;
pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
kdTree->nearestKSearch(pt, 1, ind, dist);
if(ind.size() && dist.size() && ind[0] >= 0)
{
UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
id = ids[ind[0]];
}
}
}
return id;
}
// return <id, distance>, including query
std::map<int, float> getNodesInRadius(
int nodeId,
const std::map<int, Transform> & nodes,
int maxNearestNeighbors,
float radius)
{
UASSERT(uContains(nodes, nodeId));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(nodes.size());
std::vector<int> ids(nodes.size());
int oi = 0;
for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
{
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first;
}
Transform fromT = nodes.at(nodeId);
std::map<int, float> foundNodes;
if(cloud->size())
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(cloud);
std::vector<int> ind;
std::vector<float> dist;
pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
for(unsigned int i=0; i<ind.size(); ++i)
{
if(ind[i] >=0)
{
UDEBUG("Inlier %d: %f", ids[ind[i]], dist[i]);
foundNodes.insert(std::make_pair(ids[ind[i]], dist[i]));
}
}
}
UDEBUG("found nodes=%d", (int)foundNodes.size());
return foundNodes;
}
} /* namespace graph */
} /* namespace rtabmap */

184
corelib/src/Rtabmap.cpp
View File

@@ -121,7 +121,8 @@ Rtabmap::Rtabmap() :
_mapCorrection(Transform::getIdentity()),
_mapTransform(Transform::getIdentity()),
_pathCurrentIndex(0),
_pathGoalIndex(0)
_pathGoalIndex(0),
_pathTransformToGoal(Transform::getIdentity())
{
}
@@ -643,7 +644,7 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
}
rtabmap::saveTOROGraph(path, poses, constraints);
rtabmap::graph::saveTOROGraph(path, poses, constraints);
}
}
@@ -1895,55 +1896,6 @@ void Rtabmap::dumpData() const
}
}
// fromId must be in _memory and in _optimizedPoses
// return <id, distance>
std::map<int, float> Rtabmap::getNodesInRadius(
int fromId,
int maxNearestNeighbors,
float radius) const
{
UDEBUG("");
const Signature * fromS = _memory->getSignature(fromId);
UASSERT(fromS != 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(_optimizedPoses.size());
std::vector<int> ids(_optimizedPoses.size());
int oi = 0;
for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first;
}
cloud->resize(oi);
ids.resize(oi);
UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
Transform fromT = _optimizedPoses.at(fromId);
std::map<int, float> nodes;
if(cloud->size())
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(cloud);
std::vector<int> ind;
std::vector<float> dist;
pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
kdTree->radiusSearch(pt, radius, ind, dist, maxNearestNeighbors);
for(unsigned int i=0; i<ind.size(); ++i)
{
if(ind[i] >=0)
{
UDEBUG("Inlier %d: %f", ids[ind[i]], dist[i]);
nodes.insert(std::make_pair(ids[ind[i]], dist[i]));
}
}
}
UDEBUG("nodes=%d", (int)nodes.size());
return nodes;
}
// fromId must be in _memory and in _optimizedPoses
// Get poses in front of the robot
std::map<int, Transform> Rtabmap::getWMPosesInRadius(
@@ -2107,7 +2059,7 @@ void Rtabmap::optimizeCurrentMap(
}
else
{
rtabmap::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true, _toroIgnoreVariance);
rtabmap::graph::optimizeTOROGraph(ids, poses, edgeConstraints, optimizedPoses, _toroIterations, true, _toroIgnoreVariance);
}
}
}
@@ -2331,50 +2283,39 @@ void Rtabmap::clearPath()
_path.clear();
_pathCurrentIndex=0;
_pathGoalIndex = 0;
_pathTransformToGoal.setIdentity();
if(_memory)
{
_memory->removeAllVirtualLinks();
}
}
// return true if path is updated
std::list<std::pair<int, Transform> > Rtabmap::computePath(int targetNode)
bool Rtabmap::computePath(
int targetNode,
const std::map<int, Transform> & nodes,
const std::multimap<int, rtabmap::Link> & constraints)
{
this->clearPath();
std::list<std::pair<int, Transform> > pathPoses;
if(!_rgbdSlamMode)
{
UWARN("A path can only be computed in RGBD-SLAM mode");
return pathPoses;
}
if(_memory->getWorkingMem().size() <= 1 || !_memory->getLastWorkingSignature()) // ignore virtual place
{
UWARN("Working memory is empty... cannot compute a path");
return pathPoses;
return false;
}
int currentNode = _memory->getLastWorkingSignature()->id();
UTimer timer;
std::map<int, Transform> globalGraph;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
this->getGraph(globalGraph, constraints, mapIds, true, true);
if(!uContains(globalGraph, currentNode))
if(!uContains(nodes, currentNode))
{
UWARN("Last signature %d not found in the global graph! Cannot compute a path", currentNode);
return pathPoses;
UWARN("Last signature %d not found in the graph! Cannot compute a path", currentNode);
return false;
}
if(!uContains(globalGraph, targetNode))
if(!uContains(nodes, targetNode))
{
UWARN("Goal %d not found in the global graph! Cannot compute a path", targetNode);
return pathPoses;
UWARN("Goal %d not found in the graph! Cannot compute a path", targetNode);
return false;
}
std::multimap<int, int> links;
for(std::multimap<int, rtabmap::Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
for(std::multimap<int, rtabmap::Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{
links.insert(std::make_pair(iter->first, iter->second.to()));
links.insert(std::make_pair(iter->second.to(), iter->first)); // <->
@@ -2383,11 +2324,8 @@ std::list<std::pair<int, Transform> > Rtabmap::computePath(int targetNode)
//std::multimap<int, int> clusters = rtabmap::radiusPosesClustering(globalGraph, _goalMetricError, CV_PI);
//links.insert(clusters.begin(), clusters.end());
UINFO("Time creating global graph = %fs", timer.ticks());
UINFO("Computing path from location %d to %d", currentNode, targetNode);
_path = rtabmap::computePath(globalGraph, links, currentNode, targetNode);
UINFO("Time computing path = %fs", timer.ticks());
_path = rtabmap::graph::computePath(nodes, links, currentNode, targetNode);
if(_path.size()<2)
{
@@ -2412,11 +2350,83 @@ std::list<std::pair<int, Transform> > Rtabmap::computePath(int targetNode)
}
}
updateGoalIndex();
return _path.size()>0;
}
for(unsigned int i = 0; i<_path.size(); ++i)
// return true if path is updated
std::list<std::pair<int, Transform> > Rtabmap::computePath(int targetNode, bool global)
{
this->clearPath();
std::list<std::pair<int, Transform> > pathPoses;
if(!_rgbdSlamMode)
{
pathPoses.push_back(std::make_pair(_path[i], globalGraph.at(_path[i])));
UWARN("A path can only be computed in RGBD-SLAM mode");
return pathPoses;
}
UTimer timer;
std::map<int, Transform> nodes;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
this->getGraph(nodes, constraints, mapIds, true, global);
UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
if(computePath(targetNode, nodes, constraints))
{
updateGoalIndex();
for(unsigned int i = 0; i<_path.size(); ++i)
{
pathPoses.push_back(std::make_pair(_path[i], nodes.at(_path[i])));
}
}
UINFO("Time computing path = %fs", timer.ticks());
return pathPoses;
}
std::list<std::pair<int, Transform> > Rtabmap::computePath(const Transform & targetPose, bool global)
{
this->clearPath();
std::list<std::pair<int, Transform> > pathPoses;
if(!_rgbdSlamMode)
{
UWARN("This method can only be used in RGBD-SLAM mode");
return pathPoses;
}
//Find the nearest node
UTimer timer;
std::map<int, Transform> nodes;
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
this->getGraph(nodes, constraints, mapIds, true, global);
UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);
UINFO("Nearest node found=%d ,%fs", nearestId, timer.ticks());
if(nearestId > 0)
{
if(computePath(nearestId, nodes, constraints))
{
UASSERT(_path.size() > 0);
UASSERT(uContains(nodes, _path.back()));
_pathTransformToGoal = nodes.at(_path.back()).inverse() * targetPose;
updateGoalIndex();
for(unsigned int i = 0; i<_path.size(); ++i)
{
pathPoses.push_back(std::make_pair(_path[i], nodes.at(_path[i])));
}
pathPoses.back().second *= _pathTransformToGoal;
}
UINFO("Time computing path = %fs", timer.ticks());
}
else
{
UWARN("Nearest node not found in graph (size=%d) for pose %s", (int)nodes.size(), targetPose.prettyPrint().c_str());
}
return pathPoses;
@@ -2469,7 +2479,7 @@ std::vector<int> Rtabmap::getPathNextNodes() const
return ids;
}
int Rtabmap::getPathGoalId() const
int Rtabmap::getPathCurrentGoalId() const
{
if(_path.size())
{
@@ -2501,7 +2511,7 @@ void Rtabmap::updateGoalIndex()
if(uContains(_optimizedPoses, goalId))
{
//use local position to know if the goal is reached
float d = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(goalId));
float d = _optimizedPoses.at(_memory->getLastWorkingSignature()->id()).getDistance(_optimizedPoses.at(goalId)*_pathTransformToGoal);
if(d < _goalReachedRadius)
{
UINFO("Goal %d reached!", goalId);
@@ -2521,7 +2531,7 @@ void Rtabmap::updateGoalIndex()
break;
}
}
UASSERT(_pathGoalIndex <= _path.size() && goalIndex >= 0 && goalIndex <= (int)_path.size());
UASSERT(_pathGoalIndex < _path.size() && goalIndex >= 0 && goalIndex < (int)_path.size());
if((int)_pathGoalIndex != goalIndex)
{
UINFO("Updated current goal from %d to %d (%d/%d)",

40
guilib/src/DatabaseViewer.cpp
View File

@@ -241,7 +241,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
// Added links
for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
{
std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::findLink(linksRefined_, iter->second.from(), iter->second.to());
std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
if(refinedIter != linksRefined_.end())
{
memory_->addLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), refinedIter->second.type(), refinedIter->second.variance());
@@ -574,7 +574,7 @@ void DatabaseViewer::generateTOROGraph()
QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".graph", tr("TORO file (*.graph)"));
if(!path.isEmpty())
{
rtabmap::saveTOROGraph(path.toStdString(), uValueAt(graphes_, id), links);
rtabmap::graph::saveTOROGraph(path.toStdString(), uValueAt(graphes_, id), links);
}
}
}
@@ -806,7 +806,7 @@ void DatabaseViewer::detectMoreLoopClosures()
for(int n=0; n<iterations; ++n)
{
UINFO("iteration %d/%d", n+1, iterations);
std::multimap<int, int> clusters = rtabmap::radiusPosesClustering(
std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
optimizedPoses,
ui_->doubleSpinBox_detectMore_radius->value(),
ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
@@ -1418,7 +1418,7 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & linkIn,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo,
bool updateImageSliders)
{
std::multimap<int, Link>::iterator iter = rtabmap::findLink(linksRefined_, linkIn.from(), linkIn.to());
std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
rtabmap::Link link = linkIn;
if(iter != linksRefined_.end())
{
@@ -1701,7 +1701,7 @@ void DatabaseViewer::updateConstraintButtons()
//check for modified link
bool modified = false;
std::multimap<int, Link>::iterator iter = rtabmap::findLink(linksRefined_, currentLink.from(), currentLink.to());
std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to());
if(iter != linksRefined_.end())
{
currentLink = iter->second;
@@ -1791,8 +1791,8 @@ void DatabaseViewer::updateGraphView()
graphes_.push_back(poses_);
ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
std::map<int, int> depthGraph = rtabmap::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
rtabmap::optimizeTOROGraph(
std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(links, ui_->spinBox_optimizationsFrom->value(), 0);
rtabmap::graph::optimizeTOROGraph(
depthGraph,
poses_,
links, finalPoses,
@@ -1818,21 +1818,21 @@ void DatabaseViewer::updateGraphView()
Link DatabaseViewer::findActiveLink(int from, int to)
{
Link link;
std::multimap<int, Link>::iterator findIter = rtabmap::findLink(linksRefined_, from ,to);
std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, from ,to);
if(findIter != linksRefined_.end())
{
link = findIter->second;
}
else
{
findIter = rtabmap::findLink(linksAdded_, from ,to);
findIter = rtabmap::graph::findLink(linksAdded_, from ,to);
if(findIter != linksAdded_.end())
{
link = findIter->second;
}
else if(!containsLink(linksRemoved_, from ,to))
{
findIter = rtabmap::findLink(links_, from ,to);
findIter = rtabmap::graph::findLink(links_, from ,to);
if(findIter != links_.end())
{
link = findIter->second;
@@ -1844,7 +1844,7 @@ Link DatabaseViewer::findActiveLink(int from, int to)
bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
{
return rtabmap::findLink(links, from, to) != links.end();
return rtabmap::graph::findLink(links, from, to) != links.end();
}
void DatabaseViewer::refineConstraint()
@@ -2282,7 +2282,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
else if(containsLink(linksRemoved_, from, to))
{
//simply remove from linksRemoved
linksRemoved_.erase(rtabmap::findLink(linksRemoved_, from, to));
linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to));
updateSlider = true;
}
@@ -2315,19 +2315,19 @@ void DatabaseViewer::resetConstraint()
}
std::multimap<int, Link>::iterator iter = rtabmap::findLink(linksRefined_, from, to);
std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, from, to);
if(iter != linksRefined_.end())
{
linksRefined_.erase(iter);
this->updateGraphView();
}
iter = rtabmap::findLink(links_, from, to);
iter = rtabmap::graph::findLink(links_, from, to);
if(iter != links_.end())
{
this->updateConstraintView(iter->second);
}
iter = rtabmap::findLink(linksAdded_, from, to);
iter = rtabmap::graph::findLink(linksAdded_, from, to);
if(iter != linksAdded_.end())
{
this->updateConstraintView(iter->second);
@@ -2355,7 +2355,7 @@ void DatabaseViewer::rejectConstraint()
// find the original one
std::multimap<int, Link>::iterator iter;
iter = rtabmap::findLink(links_, from, to);
iter = rtabmap::graph::findLink(links_, from, to);
if(iter != links_.end())
{
if(iter->second.type() == Link::kNeighbor)
@@ -2368,13 +2368,13 @@ void DatabaseViewer::rejectConstraint()
}
// remove from refined and added
iter = rtabmap::findLink(linksRefined_, from, to);
iter = rtabmap::graph::findLink(linksRefined_, from, to);
if(iter != linksRefined_.end())
{
linksRefined_.erase(iter);
removed = true;
}
iter = rtabmap::findLink(linksAdded_, from, to);
iter = rtabmap::graph::findLink(linksAdded_, from, to);
if(iter != linksAdded_.end())
{
linksAdded_.erase(iter);
@@ -2397,7 +2397,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
{
std::multimap<int, rtabmap::Link>::iterator findIter;
findIter = rtabmap::findLink(linksRemoved_, iter->second.from(), iter->second.to());
findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
if(findIter != linksRemoved_.end())
{
if(!(iter->second.from() == findIter->second.from() &&
@@ -2415,7 +2415,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
}
}
findIter = rtabmap::findLink(linksRefined_, iter->second.from(), iter->second.to());
findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
if(findIter!=linksRefined_.end())
{
if(iter->second.from() == findIter->second.from() &&

14
guilib/src/MainWindow.cpp
View File

@@ -1207,7 +1207,7 @@ void MainWindow::updateMapCloud(
{
float radius = _preferencesDialog->getCloudFilteringRadius();
float angle = _preferencesDialog->getCloudFilteringAngle()*CV_PI/180.0; // convert to rad
poses = rtabmap::radiusPosesFiltering(posesIn, radius, angle);
poses = rtabmap::graph::radiusPosesFiltering(posesIn, radius, angle);
// make sure the last is here
poses.insert(*posesIn.rbegin());
for(std::map<int, Transform>::iterator iter= poses.begin(); iter!=poses.end(); ++iter)
@@ -2996,7 +2996,7 @@ void MainWindow::postProcessing()
_initProgressDialog->appendText(tr("Looking for more loop closures, clustering poses... (iteration=%1/%2, radius=%3 m angle=%4 degrees)")
.arg(n+1).arg(detectLoopClosureIterations).arg(clusterRadius).arg(clusterAngle));
std::multimap<int, int> clusters = rtabmap::radiusPosesClustering(
std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
_currentPosesMap,
clusterRadius,
clusterAngle*CV_PI/180.0);
@@ -3018,7 +3018,7 @@ void MainWindow::postProcessing()
// only add new links and one per cluster per iteration
if(addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end() &&
rtabmap::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
rtabmap::graph::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
{
if(!_cachedSignatures.contains(from))
{
@@ -3101,8 +3101,8 @@ void MainWindow::postProcessing()
_initProgressDialog->appendText(tr("Optimizing graph with new links (%1 nodes, %2 constraints)...")
.arg(odomPoses.size()).arg(_currentLinksMap.size()));
std::map<int, rtabmap::Transform> optimizedPoses;
std::map<int, int> depthGraph = rtabmap::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
rtabmap::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
rtabmap::graph::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
_currentPosesMap = optimizedPoses;
_initProgressDialog->appendText(tr("Optimizing graph with new links... done!"));
}
@@ -3256,8 +3256,8 @@ void MainWindow::postProcessing()
_initProgressDialog->appendText(tr("Optimizing graph with updated links (%1 nodes, %2 constraints)...")
.arg(odomPoses.size()).arg(_currentLinksMap.size()));
std::map<int, rtabmap::Transform> optimizedPoses;
std::map<int, int> depthGraph = rtabmap::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
rtabmap::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
std::map<int, int> depthGraph = rtabmap::graph::generateDepthGraph(_currentLinksMap, toroOptimizeFromGraphEnd?odomPoses.rbegin()->first:odomPoses.begin()->first);
rtabmap::graph::optimizeTOROGraph(depthGraph, odomPoses, _currentLinksMap, optimizedPoses, toroIterations, true, ignoreVariance);
_initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
_initProgressDialog->incrementStep();