i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged master to multicamera branch

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-29 14:54:49 -04:00
parent c6d0d47b1c ec8946dc2f
commit c5046df226
9 changed files with 53 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+21 -16
View File
@@ -2562,17 +2562,16 @@ void Rtabmap::optimizeCurrentMap(
{
//Optimize the map
optimizedPoses.clear();
UDEBUG("Optimize map: around location %d", id);
UINFO("Optimize map: around location %d", id);
if(_memory && id > 0)
{
UTimer timer;
std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true);
UDEBUG("get ids=%d", (int)ids.size());
if(!_optimizeFromGraphEnd && ids.size() > 1)
{
id = ids.begin()->first;
}
UINFO("get ids time %f s", timer.ticks());
UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks());
optimizedPoses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints);
@@ -2597,7 +2596,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
std::multimap<int, Link> edgeConstraints;
UDEBUG("ids=%d", (int)ids.size());
_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase);
UDEBUG("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
UINFO("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
if(constraints)
{
@@ -2614,6 +2613,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
{
optimizedPoses = _graphOptimizer->optimize(fromId, poses, edgeConstraints);
}
UINFO("Optimization time %f s", timer.ticks());
return optimizedPoses;
}
@@ -2801,8 +2801,8 @@ void Rtabmap::getGraph(
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
bool optimized,
bool global,
std::map<int, Signature> * signatures)
bool global,
std::map<int, Signature> * signatures)
{
if(_memory && _memory->getLastWorkingSignature())
{
@@ -2824,8 +2824,8 @@ void Rtabmap::getGraph(
std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
}
if(signatures)
if(signatures)
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
@@ -2834,8 +2834,8 @@ void Rtabmap::getGraph(
int mapId = -1;
std::string label;
double stamp = 0;
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true);
std::vector<unsigned char> userData;
_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, global);
signatures->insert(std::make_pair(iter->first,
Signature(iter->first,
mapId,
@@ -2846,7 +2846,7 @@ void Rtabmap::getGraph(
std::multimap<int, pcl::PointXYZ>(),
odomPose,
userData,
SensorData())));
SensorData())));
}
}
}
@@ -2986,6 +2986,7 @@ bool Rtabmap::computePath(
// return true if path is updated
bool Rtabmap::computePath(int targetNode, bool global)
{
UINFO("Planning a path to node %d (global=%d)", targetNode, global?1:0);
this->clearPath();
if(!_rgbdSlamMode)
@@ -2994,23 +2995,27 @@ bool Rtabmap::computePath(int targetNode, bool global)
return false;
}
UTimer totalTimer;
UTimer timer;
std::map<int, Transform> nodes;
std::multimap<int, Link> constraints;
this->getGraph(nodes, constraints, true, global);
std::multimap<int, Link> constraints;
this->getGraph(nodes, constraints, true, global);
UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
if(computePath(targetNode, nodes, constraints))
{
updateGoalIndex();
}
UINFO("Time computing path = %fs", timer.ticks());
UINFO("Time computing path (A*) = %fs", timer.ticks());
UINFO("Total planning time = %fs (%d nodes, %f m long)", totalTimer.ticks(), (int)_path.size(), graph::computePathLength(_path));
return _path.size()>0;
}
bool Rtabmap::computePath(const Transform & targetPose, bool global)
{
UINFO("Planning a path to pose %s (global=%d)", targetPose.prettyPrint().c_str(), global?1:0);
this->clearPath();
std::list<std::pair<int, Transform> > pathPoses;
@@ -3027,8 +3032,8 @@ bool Rtabmap::computePath(const Transform & targetPose, bool global)
std::map<int, int> mapIds;
std::map<int, double> stamps;
std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas;
this->getGraph(nodes, constraints, true, global);
std::map<int, std::vector<unsigned char> > userDatas;
this->getGraph(nodes, constraints, true, global);
UINFO("Time creating graph (global=%s) = %fs", global?"true":"false", timer.ticks());
int nearestId = rtabmap::graph::findNearestNode(nodes, targetPose);