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

Updated how local scan matching is done (doing it on segmented local paths)

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-27 02:43:06 -04:00
parent 2e706ed01f
commit db90da7303
16 changed files with 532 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/include/rtabmap/core/DBReader.h
View File

@@ -49,7 +49,7 @@ public:
DBReader(const std::string & databasePath,
float frameRate = 0.0f,
bool odometryIgnored = false,
float delayToStartSec = 0.0f);
bool ignoreGoalDelay = false);
virtual ~DBReader();
bool init(int startIndex=0);
@@ -64,7 +64,7 @@ private:
std::string _path;
float _frameRate;
bool _odometryIgnored;
float _delayToStartSec;
bool _ignoreGoalDelay;
DBDriver * _dbDriver;
UTimer _timer;

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

@@ -145,6 +145,10 @@ std::multimap<int, Link>::iterator RTABMAP_EXP findLink(
std::multimap<int, Link> & links,
int from,
int to);
std::multimap<int, int>::iterator RTABMAP_EXP findLink(
std::multimap<int, int> & links,
int from,
int to);
/**
* Get only the the most recent or older poses in the defined radius.

3
corelib/include/rtabmap/core/Memory.h
View File

@@ -220,6 +220,7 @@ private:
bool _generateIds;
bool _badSignaturesIgnored;
int _imageDecimation;
float _laserScanVoxelSize;
bool _localSpaceLinksKeptInWM;
float _rehearsalMaxDistance;
float _rehearsalMaxAngle;
@@ -255,6 +256,8 @@ private:
bool _bowForce2D;
bool _bowEpipolarGeometry;
float _bowEpipolarGeometryVar;
float _icpMaxTranslation;
float _icpMaxRotation;
int _icpDecimation;
float _icpMaxDepth;
float _icpVoxelSize;

13
corelib/include/rtabmap/core/Parameters.h
View File

@@ -194,7 +194,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, GenerateIds, bool, true, "True=Generate location IDs, False=use input image IDs.");
RTABMAP_PARAM(Mem, BadSignaturesIgnored, bool, false, "Bad signatures are ignored.");
RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
RTABMAP_PARAM(Mem, ImageDecimation, int, 1, "Image decimation (>=1).");
RTABMAP_PARAM(Mem, ImageDecimation, int, 1, "Image decimation (>=1) when creating a signature.");
RTABMAP_PARAM(Mem, LaserScanVoxelSize, float, 0.0, "If > 0.0, voxelize laser scans when creating a signature.");
RTABMAP_PARAM(Mem, LocalSpaceLinksKeptInWM, bool, true, "If local space links are kept in WM.");
@@ -287,7 +288,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, GoalReachedRadius, float, 0.5, "Goal reached radius (m).");
RTABMAP_PARAM(RGBD, PlanWithNearNodesLinked, bool, true, "Before planning in the graph, near nodes are linked together (even if they don't belong to same map). Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
RTABMAP_PARAM(RGBD, PlanVirtualLinks, bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
RTABMAP_PARAM(RGBD, PlanVirtualLinksMaxDiffID, int, 50, "Max difference ID to add virtual links before planning.");
RTABMAP_PARAM(RGBD, GoalsSavedInUserData, bool, true, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
@@ -295,8 +297,8 @@ class RTABMAP_EXP Parameters
// Local loop closure detection
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, false, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors, int, 20, "Maximum nearest neighbor.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 50, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 50, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius, float, 1.0, "Path filtering radius.");
// Graph optimization
RTABMAP_PARAM(RGBD, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO and 1=g2o.");
@@ -345,7 +347,8 @@ class RTABMAP_EXP Parameters
// Loop closure constraint
RTABMAP_PARAM(LccIcp, Type, int, 0, "0=No ICP, 1=ICP 3D, 2=ICP 2D");
RTABMAP_PARAM(LccIcp, MaxDistance, float, 0.2, "Maximum ICP correction distance accepted (m).");
RTABMAP_PARAM(LccIcp, MaxTranslation, float, 0.2, "Maximum ICP translation correction accepted (m).");
RTABMAP_PARAM(LccIcp, MaxRotation, float, 0.78, "Maximum ICP rotation correction accepted (rad).");
RTABMAP_PARAM(LccBow, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
RTABMAP_PARAM(LccBow, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences.");

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

@@ -138,7 +138,8 @@ public:
int getPathCurrentGoalId() const;
const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
std::map<int, Transform> getWMPosesInRadius(int fromId, int maxNearestNeighbors, float radius, int maxDiffID, int & nearestId) const;
std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
std::list<std::map<int, Transform> > getPaths(std::map<int, Transform> poses) const;
void adjustLikelihood(std::map<int, float> & likelihood) const;
std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
const std::map<int, float> & likelihood) const;
@@ -174,13 +175,12 @@ private:
float _rgbdAngularUpdate;
float _newMapOdomChangeDistance;
int _globalLoopClosureIcpType;
float _globalLoopClosureIcpMaxDistance;
bool _poseScanMatching;
bool _localLoopClosureDetectionTime;
bool _localLoopClosureDetectionSpace;
float _localRadius;
float _localDetectMaxNeighbors;
int _localDetectMaxDiffID;
float _localPathFilteringRadius;
std::string _databasePath;
bool _optimizeFromGraphEnd;
bool _reextractLoopClosureFeatures;
@@ -190,7 +190,8 @@ private:
int _reextractMaxWords;
bool _startNewMapOnLoopClosure;
float _goalReachedRadius; // meters
bool _planWithNearNodesLinked;
bool _planVirtualLinks;
int _planVirtualLinksMaxDiffID;
bool _goalsSavedInUserData;
std::pair<int, float> _loopClosureHypothesis;

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

@@ -53,6 +53,7 @@ namespace rtabmap {
class RTABMAP_EXP Statistics
{
RTABMAP_STATS(Loop, RejectedHypothesis,);
RTABMAP_STATS(Loop, Accepted_hypothesis_id,);
RTABMAP_STATS(Loop, Highest_hypothesis_id,);
RTABMAP_STATS(Loop, Highest_hypothesis_value,);
RTABMAP_STATS(Loop, Vp_hypothesis,);
@@ -64,9 +65,10 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(LocalLoop, Odom_corrected,);
RTABMAP_STATS(LocalLoop, Time_closures,);
RTABMAP_STATS(LocalLoop, Space_closure_id,);
RTABMAP_STATS(LocalLoop, Space_nearest_id,);
RTABMAP_STATS(LocalLoop, Space_neighbors,);
RTABMAP_STATS(LocalLoop, Space_last_closure_id,);
RTABMAP_STATS(LocalLoop, Space_paths,);
RTABMAP_STATS(LocalLoop, Space_closures_added,);
RTABMAP_STATS(LocalLoop, Space_closures_added_icp_only,);
RTABMAP_STATS(Memory, Working_memory_size,);
RTABMAP_STATS(Memory, Short_time_memory_size,);

5
corelib/src/DBDriverSqlite3.cpp
View File

@@ -865,9 +865,8 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
{
query << "SELECT id "
<< "FROM Node "
<< "LEFT OUTER JOIN Link "
<< "ON id = from_id "
<< "WHERE type==0 " // select only nodes with neighor links, ignore merged nodes
<< "INNER JOIN Link "
<< "ON id = to_id " // use to_id tp ignore all children (which don't have link pointing on them)
<< "ORDER BY id";
}

18
corelib/src/DBReader.cpp
View File

@@ -45,11 +45,11 @@ namespace rtabmap {
DBReader::DBReader(const std::string & databasePath,
float frameRate,
bool odometryIgnored,
float delayToStartSec) :
bool ignoreGoalDelay) :
_path(databasePath),
_frameRate(frameRate),
_odometryIgnored(odometryIgnored),
_delayToStartSec(delayToStartSec),
_ignoreGoalDelay(ignoreGoalDelay),
_dbDriver(0),
_currentId(_ids.end())
{
@@ -126,10 +126,6 @@ void DBReader::setFrameRate(float frameRate)
void DBReader::mainLoopBegin()
{
if(_delayToStartSec > 0.0f)
{
uSleep(_delayToStartSec*1000.0f);
}
_timer.start();
}
@@ -174,7 +170,7 @@ void DBReader::mainLoop()
{
this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdGoal, "", goalId));
if(_currentId != _ids.end())
if(!_ignoreGoalDelay && _currentId != _ids.end())
{
// get stamp for the next signature to compute the delay
// that was used originally for planning
@@ -192,10 +188,10 @@ void DBReader::mainLoop()
goalId, delay);
uSleep(delay*1000);
}
else
{
UWARN("stamps = %d=%f %d=%f ", data.id(), data.stamp(), *_currentId, stamp);
}
}
else
{
UWARN("Goal %d detected, posting it!", goalId);
}
}

28
corelib/src/Graph.cpp
View File

@@ -907,6 +907,34 @@ std::multimap<int, Link>::iterator findLink(
return links.end();
}
std::multimap<int, int>::iterator findLink(
std::multimap<int, int> & links,
int from,
int to)
{
std::multimap<int, int>::iterator iter = links.find(from);
while(iter != links.end() && iter->first == from)
{
if(iter->second == to)
{
return iter;
}
++iter;
}
// let's try to -> from
iter = links.find(to);
while(iter != links.end() && iter->first == to)
{
if(iter->second == from)
{
return iter;
}
++iter;
}
return links.end();
}
std::map<int, Transform> radiusPosesFiltering(
const std::map<int, Transform> & poses,
float radius,

119
corelib/src/Memory.cpp
View File

@@ -69,6 +69,7 @@ Memory::Memory(const ParametersMap & parameters) :
_generateIds(Parameters::defaultMemGenerateIds()),
_badSignaturesIgnored(Parameters::defaultMemBadSignaturesIgnored()),
_imageDecimation(Parameters::defaultMemImageDecimation()),
_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
_localSpaceLinksKeptInWM(Parameters::defaultMemLocalSpaceLinksKeptInWM()),
_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
@@ -96,6 +97,9 @@ Memory::Memory(const ParametersMap & parameters) :
_bowEpipolarGeometry(Parameters::defaultLccBowEpipolarGeometry()),
_bowEpipolarGeometryVar(Parameters::defaultLccBowEpipolarGeometryVar()),
_icpMaxTranslation(Parameters::defaultLccIcpMaxTranslation()),
_icpMaxRotation(Parameters::defaultLccIcpMaxRotation()),
_icpDecimation(Parameters::defaultLccIcp3Decimation()),
_icpMaxDepth(Parameters::defaultLccIcp3MaxDepth()),
_icpVoxelSize(Parameters::defaultLccIcp3VoxelSize()),
@@ -344,6 +348,7 @@ Memory::~Memory()
if(!_memoryChanged && _linksChanged && _dbDriver)
{
// don't update the time stamps!
UDEBUG("");
_dbDriver->setTimestampUpdateEnabled(false);
}
this->clear();
@@ -390,6 +395,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kMemTransferSortingByWeightId(), _transferSortingByWeightId);
Parameters::parse(parameters, Parameters::kMemSTMSize(), _maxStMemSize);
Parameters::parse(parameters, Parameters::kMemImageDecimation(), _imageDecimation);
Parameters::parse(parameters, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
Parameters::parse(parameters, Parameters::kMemLocalSpaceLinksKeptInWM(), _localSpaceLinksKeptInWM);
Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
@@ -424,6 +430,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kLccBowForce2D(), _bowForce2D);
Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometry(), _bowEpipolarGeometry);
Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometryVar(), _bowEpipolarGeometryVar);
Parameters::parse(parameters, Parameters::kLccIcpMaxTranslation(), _icpMaxTranslation);
Parameters::parse(parameters, Parameters::kLccIcpMaxRotation(), _icpMaxRotation);
Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), _icpDecimation);
Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), _icpMaxDepth);
Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), _icpVoxelSize);
@@ -1033,6 +1041,7 @@ void Memory::clear()
UDEBUG("Adding statistics after run...");
if(_memoryChanged)
{
UDEBUG("");
_dbDriver->addStatisticsAfterRun(memSize,
_lastSignature?_lastSignature->id():0,
UProcessInfo::getMemoryUsage(),
@@ -2185,8 +2194,26 @@ Transform Memory::computeIcpTransform(
if(!icpT.isNull() && hasConverged &&
correspondencesRatio >= _icpCorrespondenceRatio)
{
transform = icpT * guess;
transform = transform.inverse();
float x,y,z, roll,pitch,yaw;
icpT.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
if((_icpMaxTranslation>0.0f &&
(x > _icpMaxTranslation ||
y > _icpMaxTranslation ||
z > _icpMaxTranslation))
||
(_icpMaxRotation>0.0f &&
(roll > _icpMaxRotation ||
pitch > _icpMaxRotation ||
yaw > _icpMaxRotation)))
{
msg = uFormat("Cannot compute transform (ICP correction too large)");
UINFO(msg.c_str());
}
else
{
transform = icpT * guess;
transform = transform.inverse();
}
}
else
{
@@ -2228,7 +2255,7 @@ Transform Memory::computeIcpTransform(
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newS.getLaserScanRaw(), guess);
//voxelize
if(_icp2VoxelSize > 0.0f)
if(_icp2VoxelSize > _laserScanVoxelSize)
{
oldCloud = util3d::voxelize<pcl::PointXYZ>(oldCloud, _icp2VoxelSize);
newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize);
@@ -2258,16 +2285,43 @@ Transform Memory::computeIcpTransform(
(int)oldCloud->size(),
correspondencesRatio*100.0f);
//pcl::io::savePCDFile("oldCloud.pcd", *oldCloud);
//pcl::io::savePCDFile("newCloud.pcd", *newCloud);
//UWARN("saved oldCloud.pcd and newCloud.pcd");
//if(!icpT.isNull())
//{
// newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
// pcl::io::savePCDFile("newCloudFinal.pcd", *newCloud);
// UWARN("saved newCloudFinal.pcd");
//}
if(inliers)
{
*inliers = correspondences;
}
if(!icpT.isNull() && hasConverged &&
correspondencesRatio >= _icp2CorrespondenceRatio)
if(!icpT.isNull() && hasConverged && correspondencesRatio >= _icp2CorrespondenceRatio)
{
transform = icpT * guess;
transform = transform.inverse();
float x,y,z, roll,pitch,yaw;
icpT.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
if((_icpMaxTranslation>0.0f &&
(x > _icpMaxTranslation ||
y > _icpMaxTranslation ||
z > _icpMaxTranslation))
||
(_icpMaxRotation>0.0f &&
(roll > _icpMaxRotation ||
pitch > _icpMaxRotation ||
yaw > _icpMaxRotation)))
{
msg = uFormat("Cannot compute transform (ICP correction too large)");
UINFO(msg.c_str());
}
else
{
transform = icpT * guess;
transform = transform.inverse();
}
}
else
{
@@ -2329,10 +2383,12 @@ Transform Memory::computeScanMatchingTransform(
{
if(iter->first != newId)
{
const Signature * s = this->getSignature(iter->first);
Signature * s = this->_getSignature(iter->first);
if(!s->getLaserScanCompressed().empty())
{
*assembledOldClouds += *util3d::cvMat2Cloud(rtabmap::uncompressData(s->getLaserScanCompressed()), iter->second);
cv::Mat scan;
s->uncompressData(0, 0, &scan);
*assembledOldClouds += *util3d::cvMat2Cloud(scan, iter->second);
}
else
{
@@ -2348,21 +2404,19 @@ Transform Memory::computeScanMatchingTransform(
}
// get the new cloud
const Signature * newS = getSignature(newId);
Signature * newS = _getSignature(newId);
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud;
UASSERT(uContains(poses, newId));
newCloud = util3d::cvMat2Cloud(rtabmap::uncompressData(newS->getLaserScanCompressed()), poses.at(newId));
cv::Mat newScan;
newS->uncompressData(0, 0, &newScan);
newCloud = util3d::cvMat2Cloud(newScan, poses.at(newId));
//voxelize
if(newCloud->size() && _icp2VoxelSize > 0.0f)
if(newCloud->size() && _icp2VoxelSize > _laserScanVoxelSize)
{
newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize);
}
//UWARN("local scan matching pcd saved!");
//pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
//pcl::io::savePCDFile("new.pcd", *newCloud);
Transform transform;
if(assembledOldClouds->size() && newCloud->size())
{
@@ -2397,8 +2451,11 @@ Transform Memory::computeScanMatchingTransform(
{
transform = poses.at(newId).inverse()*icpT.inverse() * poses.at(oldId);
//newCloud = util3d::cvMat2Cloud(util3d::uncompressData(newS->getDepth2DCompressed()), poses.at(oldId)*transform.inverse());
//pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
//pcl::io::savePCDFile("new.pcd", *newCloud);
//newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
//pcl::io::savePCDFile("newFinal.pcd", *newCloud);
//UWARN("local scan matching old.pcd, new.pcd and newFinal.pcd saved!");
}
else
{
@@ -2444,6 +2501,17 @@ bool Memory::addLink(int oldId, int newId, const Transform & transform, Link::Ty
UDEBUG("Add link between %d and %d", oldS->id(), newS->id());
if(rotVariance == 0)
{
rotVariance = 0.000001; // set small variance (0.001 m x 0.001 m)
UWARN("Null rotation variance detected, set to something very small (0.001m^2)!");
}
if(transVariance == 0)
{
transVariance = 0.000001; // set small variance (0.001 m x 0.001 m)
UWARN("Null transitional variance detected, set to something very small (0.001m^2)!");
}
oldS->addLink(Link(oldS->id(), newS->id(), type, transform.inverse(), rotVariance, transVariance));
newS->addLink(Link(newS->id(), oldS->id(), type, transform, rotVariance, transVariance));
@@ -3330,7 +3398,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
{
if(id <= 0)
{
UWARN("Received image ID is null. "
UERROR("Received image ID is null. "
"Please set parameter Mem/GenerateIds to \"true\" or "
"make sure the input source provides image ids (seq).");
return 0;
@@ -3341,7 +3409,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
}
else
{
UWARN("Id of acquired image (%d) is smaller than the last in memory (%d). "
UERROR("Id of acquired image (%d) is smaller than the last in memory (%d). "
"Please set parameter Mem/GenerateIds to \"true\" or "
"make sure the input source provides image ids (seq) over the last in "
"memory, which is %d.",
@@ -3758,6 +3826,13 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
}
}
// apply icp2 voxel?
cv::Mat laserScan = data.laserScan();
if(!laserScan.empty() && _laserScanVoxelSize > 0.0f)
{
laserScan = util3d::laserScanFromPointCloud(*util3d::voxelize<pcl::PointXYZ>(util3d::laserScanToPointCloud(laserScan), _laserScanVoxelSize));
}
Signature * s;
if(this->isBinDataKept())
{
@@ -3772,7 +3847,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
rtabmap::CompressionThread ctImage(image, std::string(".jpg"));
rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
rtabmap::CompressionThread ctDepth2d(data.laserScan());
rtabmap::CompressionThread ctDepth2d(laserScan);
ctImage.start();
ctDepth.start();
ctDepth2d.start();
@@ -3809,13 +3884,13 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
words3D,
data.pose(),
data.userData(),
rtabmap::compressData2(data.laserScan()));
rtabmap::compressData2(laserScan));
}
if(this->isRawDataKept())
{
s->setImageRaw(image);
s->setDepthRaw(depthOrRightImage);
s->setLaserScanRaw(data.laserScan());
s->setLaserScanRaw(laserScan);
}

285
corelib/src/Rtabmap.cpp
View File

@@ -92,13 +92,12 @@ Rtabmap::Rtabmap() :
_rgbdAngularUpdate(Parameters::defaultRGBDAngularUpdate()),
_newMapOdomChangeDistance(Parameters::defaultRGBDNewMapOdomChangeDistance()),
_globalLoopClosureIcpType(Parameters::defaultLccIcpType()),
_globalLoopClosureIcpMaxDistance(Parameters::defaultLccIcpMaxDistance()),
_poseScanMatching(Parameters::defaultRGBDPoseScanMatching()),
_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
_localRadius(Parameters::defaultRGBDLocalRadius()),
_localDetectMaxNeighbors(Parameters::defaultRGBDLocalLoopDetectionNeighbors()),
_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
_localPathFilteringRadius(Parameters::defaultRGBDLocalLoopDetectionPathFilteringRadius()),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_reextractLoopClosureFeatures(Parameters::defaultLccReextractActivated()),
@@ -108,7 +107,8 @@ Rtabmap::Rtabmap() :
_reextractMaxWords(Parameters::defaultLccReextractMaxWords()),
_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
_planWithNearNodesLinked(Parameters::defaultRGBDPlanWithNearNodesLinked()),
_planVirtualLinks(Parameters::defaultRGBDPlanVirtualLinks()),
_planVirtualLinksMaxDiffID(Parameters::defaultRGBDPlanVirtualLinksMaxDiffID()),
_goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
_loopClosureHypothesis(0,0.0f),
_highestHypothesis(0,0.0f),
@@ -381,12 +381,11 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rgbdAngularUpdate);
Parameters::parse(parameters, Parameters::kRGBDNewMapOdomChangeDistance(), _newMapOdomChangeDistance);
Parameters::parse(parameters, Parameters::kRGBDPoseScanMatching(), _poseScanMatching);
Parameters::parse(parameters, Parameters::kLccIcpMaxDistance(), _globalLoopClosureIcpMaxDistance);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionNeighbors(), _localDetectMaxNeighbors);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kLccReextractActivated(), _reextractLoopClosureFeatures);
Parameters::parse(parameters, Parameters::kLccReextractNNType(), _reextractNNType);
@@ -395,7 +394,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kLccReextractMaxWords(), _reextractMaxWords);
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
Parameters::parse(parameters, Parameters::kRGBDPlanWithNearNodesLinked(), _planWithNearNodesLinked);
Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinks(), _planVirtualLinks);
Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinksMaxDiffID(), _planVirtualLinksMaxDiffID);
Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
// RGB-D SLAM stuff
@@ -996,20 +996,8 @@ bool Rtabmap::process(const SensorData & data)
Transform transform = _memory->computeVisualTransform(*iter, signature->id(), &rejectedMsg, &inliers, &variance);
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
Transform icpTransform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance);
float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
if(!icpTransform.isNull() &&
_globalLoopClosureIcpMaxDistance>0.0f &&
squaredNorm > _globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance)
{
UWARN("Local loop closure rejected (%d->%d) (ICP correction too large %f > %f [squared norm])",
signature->id(),
*iter,
squaredNorm,
_globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance);
icpTransform.setNull();
}
transform = icpTransform;
transform = _memory->computeIcpTransform(*iter, signature->id(), transform, _globalLoopClosureIcpType==1, &rejectedMsg, 0, &variance);
variance = 1.0f; // ICP, set variance to 1
}
if(!transform.isNull())
{
@@ -1361,14 +1349,17 @@ bool Rtabmap::process(const SensorData & data)
{
nearNodesByDist.insert(std::make_pair(iter->second, iter->first));
}
for(std::multimap<float, int>::iterator iter=nearNodesByDist.begin();
for(std::map<float, int>::iterator iter=nearNodesByDist.begin();
iter!=nearNodesByDist.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
++iter)
{
const Signature * s = _memory->getSignature(iter->second);
UASSERT(s != 0);
for(std::map<int, Link>::const_iterator jter=s->getLinks().begin();
jter!=s->getLinks().end() && retrievalLocalIds.size() < _maxLocalRetrieved;
UASSERT(s!=0);
// If there is a change of direction, better to be retrieving
// ALL nearest signatures than only newest neighbors
const std::map<int, Link> & links = s->getLinks();
for(std::map<int, Link>::const_iterator jter=links.begin();
jter!=links.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
++jter)
{
if(_memory->getSignature(jter->first) == 0)
@@ -1488,20 +1479,8 @@ bool Rtabmap::process(const SensorData & data)
}
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
Transform icpTransform = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
float squaredNorm = (transform.inverse()*icpTransform).getNormSquared();
if(!icpTransform.isNull() &&
_globalLoopClosureIcpMaxDistance>0.0f &&
squaredNorm > _globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance)
{
UWARN("Global loop closure rejected (%d->%d) (ICP correction too large %f > %f [squared norm])",
signature->id(),
_loopClosureHypothesis.first,
squaredNorm,
_globalLoopClosureIcpMaxDistance*_globalLoopClosureIcpMaxDistance);
icpTransform.setNull();
}
transform = icpTransform;
transform = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, &rejectedMsg, 0, &variance);
variance = 1.0f; // ICP, set variance to 1
}
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
@@ -1532,11 +1511,11 @@ bool Rtabmap::process(const SensorData & data)
timeAddLoopClosureLink = timer.ticks();
ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
int localSpaceDetectionPosesCount = 0;
int localSpaceClosureId = 0;
int localSpaceNearestId = 0;
if(_loopClosureHypothesis.first == 0 &&
_localLoopClosureDetectionSpace &&
int localSpaceClosuresAdded = 0;
int localSpaceClosuresAddedByICPOnly = 0;
int lastLocalSpaceClosureId = 0;
int localSpacePaths = 0;
if(_localLoopClosureDetectionSpace &&
!signature->getLaserScanCompressed().empty())
{
if(_graphOptimizer->iterations() == 0)
@@ -1548,43 +1527,93 @@ bool Rtabmap::process(const SensorData & data)
//============================================================
// Scan matching LOCAL LOOP CLOSURE SPACE
//============================================================
std::map<int, Transform> localSpacePoses;
localSpaceNearestId = 0;
localSpacePoses = this->getWMPosesInRadius(
std::map<int, Transform> forwardPoses;
forwardPoses = this->getForwardWMPoses(
signature->id(),
_localDetectMaxNeighbors,
0,
_localRadius,
_localDetectMaxDiffID,
localSpaceNearestId);
_localDetectMaxDiffID);
// add current node to poses
localSpacePoses.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
localSpaceDetectionPosesCount = (int)localSpacePoses.size()-1;
//The nearest will be the reference for a loop closure transform
if(localSpacePoses.size() &&
localSpaceNearestId &&
signature->getLinks().find(localSpaceNearestId) == signature->getLinks().end())
std::list<std::map<int, Transform> > forwardPaths = getPaths(forwardPoses);
localSpacePaths = forwardPaths.size();
for(std::list<std::map<int, Transform> >::iterator iter=forwardPaths.begin(); iter!=forwardPaths.end(); ++iter)
{
double variance = 1.0;
std::string rejectedMsg;
Transform t = _memory->computeScanMatchingTransform(signature->id(), localSpaceNearestId, localSpacePoses, &rejectedMsg, 0, &variance);
if(!t.isNull())
{
localSpaceClosureId = localSpaceNearestId;
UINFO("Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
localSpaceNearestId,
t.prettyPrint().c_str());
_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, 1, 1); // set Identify covariance
std::map<int, Transform> & path = *iter;
UASSERT(path.size());
// Old map -> new map, used for localization correction on loop closure
const Signature * oldS = _memory->getSignature(localSpaceNearestId);
UASSERT(oldS != 0);
_mapTransform = oldS->getPose() * t.inverse() * signature->getPose().inverse();
}
else
// only do local loop closure detection if there is no
// global loop closure already detected on this path
if(_loopClosureHypothesis.first == 0 || path.find(_loopClosureHypothesis.first) == path.end())
{
UINFO("Local loop closure (space) rejected: %s", rejectedMsg.c_str());
//find the nearest pose on the path
int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
UASSERT(nearestId > 0);
// nearest pose must be close
if(_localPathFilteringRadius <= 0.0f ||
_optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)) < _localPathFilteringRadius)
{
// path filtering
if(_localPathFilteringRadius > 0.0f)
{
path = graph::radiusPosesFiltering(path, _localPathFilteringRadius, CV_PI, true);
path.insert(*_optimizedPoses.find(nearestId)); // make sure the nearest pose is still here
}
// 1) look for loop closures based on visual correspondences
double variance = 1.0;
Transform transform = _memory->computeVisualTransform(nearestId, signature->id(), 0, 0, &variance);
bool foundByVisual = false;
if(!transform.isNull() && _globalLoopClosureIcpType > 0)
{
transform = _memory->computeIcpTransform(_loopClosureHypothesis.first, signature->id(), transform, _globalLoopClosureIcpType == 1, 0, 0, &variance);
variance = 1.0f; // ICP, set variance to 1
}
if(transform.isNull())
{
if(path.size() > 2) // more than current+nearest
{
// 2) Assemble scans in the path and do ICP only
// add current node to poses
path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
//The nearest will be the reference for a loop closure transform
if(signature->getLinks().find(nearestId) == signature->getLinks().end())
{
transform = _memory->computeScanMatchingTransform(signature->id(), nearestId, path, 0, 0, &variance);
}
}
}
else
{
foundByVisual = true;
}
if(!transform.isNull())
{
UINFO("Add local loop closure in SPACE (%d->%d) %s",
signature->id(),
nearestId,
transform.prettyPrint().c_str());
// set Identify covariance if laser scan matching only
_memory->addLink(nearestId, signature->id(), transform, Link::kLocalSpaceClosure, foundByVisual?variance:1, foundByVisual?variance:1);
// Old map -> new map, used for localization correction on loop closure
const Signature * oldS = _memory->getSignature(nearestId);
UASSERT(oldS != 0);
_mapTransform = oldS->getPose() * transform.inverse() * signature->getPose().inverse();
++localSpaceClosuresAdded;
if(!foundByVisual)
{
++localSpaceClosuresAddedByICPOnly;
}
lastLocalSpaceClosureId = nearestId;
}
else
{
UINFO("Local loop closure %d (space) rejected", nearestId);
}
}
}
}
}
@@ -1599,7 +1628,7 @@ bool Rtabmap::process(const SensorData & data)
(_loopClosureHypothesis.first>0 || // can be different map of the current one
localLoopClosuresInTimeFound>0 || // only same map of the current one
scanMatchingSuccess || // only same map of the current one
localSpaceClosureId>0 || // can be different map of the current one
lastLocalSpaceClosureId>0 || // can be different map of the current one
signaturesRetrieved.size())) // can be different map of the current one
{
if(_memory->isIncremental())
@@ -1616,10 +1645,10 @@ bool Rtabmap::process(const SensorData & data)
UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
}
}
else if(_loopClosureHypothesis.first > 0 || localSpaceClosureId > 0 || signaturesRetrieved.size())
else if(_loopClosureHypothesis.first > 0 || lastLocalSpaceClosureId > 0 || signaturesRetrieved.size())
{
UINFO("Update map correction: Localization mode");
int oldId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:localSpaceClosureId?localSpaceClosureId:_highestHypothesis.first;
int oldId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastLocalSpaceClosureId?lastLocalSpaceClosureId:_highestHypothesis.first;
UASSERT(oldId != 0);
if(signaturesRetrieved.size() || _optimizedPoses.find(oldId) == _optimizedPoses.end())
{
@@ -1672,7 +1701,7 @@ bool Rtabmap::process(const SensorData & data)
int lcHypothesisReactivated = 0;
float rehearsalValue = uValue(statistics_.data(), Statistics::kMemoryRehearsal_sim(), 0.0f);
int rehearsalMaxId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
sLoop = _memory->getSignature(_loopClosureHypothesis.first?_loopClosureHypothesis.first:localSpaceClosureId?localSpaceClosureId:_highestHypothesis.first);
sLoop = _memory->getSignature(_loopClosureHypothesis.first?_loopClosureHypothesis.first:lastLocalSpaceClosureId?lastLocalSpaceClosureId:_highestHypothesis.first);
if(sLoop)
{
lcHypothesisReactivated = sLoop->isSaved()?1.0f:0.0f;
@@ -1699,6 +1728,7 @@ bool Rtabmap::process(const SensorData & data)
ULOGGER_INFO("send all stats...");
statistics_.setExtended(1);
statistics_.addStatistic(Statistics::kLoopAccepted_hypothesis_id(), _loopClosureHypothesis.first);
statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_id(), _highestHypothesis.first);
statistics_.addStatistic(Statistics::kLoopHighest_hypothesis_value(), _highestHypothesis.second);
statistics_.addStatistic(Statistics::kLoopHypothesis_reactivated(), lcHypothesisReactivated);
@@ -1710,11 +1740,12 @@ bool Rtabmap::process(const SensorData & data)
statistics_.addStatistic(Statistics::kLocalLoopOdom_corrected(), scanMatchingSuccess?1:0);
statistics_.addStatistic(Statistics::kLocalLoopTime_closures(), localLoopClosuresInTimeFound);
statistics_.addStatistic(Statistics::kLocalLoopSpace_neighbors(), localSpaceDetectionPosesCount);
statistics_.addStatistic(Statistics::kLocalLoopSpace_closure_id(), localSpaceClosureId);
statistics_.addStatistic(Statistics::kLocalLoopSpace_nearest_id(), localSpaceNearestId);
statistics_.setLocalLoopClosureId(localSpaceClosureId);
if(_loopClosureHypothesis.first || localSpaceClosureId)
statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added(), localSpaceClosuresAdded);
statistics_.addStatistic(Statistics::kLocalLoopSpace_closures_added_icp_only(), localSpaceClosuresAddedByICPOnly);
statistics_.addStatistic(Statistics::kLocalLoopSpace_paths(), localSpacePaths);
statistics_.addStatistic(Statistics::kLocalLoopSpace_last_closure_id(), lastLocalSpaceClosureId);
statistics_.setLocalLoopClosureId(lastLocalSpaceClosureId);
if(_loopClosureHypothesis.first || lastLocalSpaceClosureId)
{
UASSERT(uContains(sLoop->getLinks(), signature->id()));
UINFO("Set loop closure transform = %s", sLoop->getLinks().at(signature->id()).transform().prettyPrint().c_str());
@@ -2099,13 +2130,13 @@ void Rtabmap::dumpData() const
}
// fromId must be in _memory and in _optimizedPoses
// Get poses in front of the robot
std::map<int, Transform> Rtabmap::getWMPosesInRadius(
// Get poses in front of the robot, return optimized poses
std::map<int, Transform> Rtabmap::getForwardWMPoses(
int fromId,
int maxNearestNeighbors,
float radius,
int maxDiffID, // 0 means ignore
int & nearestId) const
int maxDiffID // 0 means ignore
) const
{
std::map<int, Transform> poses;
if(_memory && fromId > 0)
@@ -2127,12 +2158,15 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
}
for(std::map<int, Transform>::const_iterator iter = _optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
// Only locations in Working Memory not too far from the current node (so inside the margin)
bool diffIdOk = maxDiffID == 0 || uContains(margins, iter->first);
if(stm.find(iter->first) == stm.end() && diffIdOk)
if(iter->first != fromId)
{
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first;
// Only locations in Working Memory not too far from the current node (so inside the margin)
bool diffIdOk = maxDiffID == 0 || uContains(margins, iter->first);
if(stm.find(iter->first) == stm.end() && diffIdOk)
{
(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
ids[oi++] = iter->first;
}
}
}
@@ -2142,8 +2176,6 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
UASSERT(_optimizedPoses.find(fromId) != _optimizedPoses.end());
Transform fromT = _optimizedPoses.at(fromId);
nearestId = 0;
float minDistance = -1;
if(cloud->size())
{
//if(cloud->size())
@@ -2153,17 +2185,16 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
//}
//filter poses in front of the fromId
Transform t=Transform::getIdentity();
t.x() = radius*0.95f;
float x,y,z, roll,pitch,yaw;
(fromT*t).getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
fromT.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
pcl::CropBox<pcl::PointXYZ> cropbox;
cropbox.setInputCloud(cloud);
cropbox.setMin(Eigen::Vector4f(-radius, -radius, -radius, 0));
cropbox.setMax(Eigen::Vector4f(radius, radius, radius, 0));
cropbox.setMin(Eigen::Vector4f(-1, -radius, -999999, 0));
cropbox.setMax(Eigen::Vector4f(radius, radius, 999999, 0));
cropbox.setRotation(Eigen::Vector3f(roll, pitch, yaw));
cropbox.setTranslation(Eigen::Vector3f(x, y, z));
cropbox.setRotation(Eigen::Vector3f(roll,pitch,yaw));
pcl::IndicesPtr indices(new std::vector<int>());
cropbox.filter(*indices);
@@ -2190,11 +2221,6 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
//inliers.push_back(pcl::PointXYZ(tmp.x(), tmp.y(), tmp.z()));
UDEBUG("Inlier %d: %s", ids[ind[i]], tmp.prettyPrint().c_str());
poses.insert(std::make_pair(ids[ind[i]], tmp));
if(minDistance == -1 || minDistance > dist[i])
{
nearestId = ids[ind[i]];
minDistance = dist[i];
}
}
}
@@ -2209,19 +2235,42 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
// c.push_back(pcl::PointXYZ(ct.x(), ct.y(), ct.z()));
// pcl::io::savePCDFile("radiusNearestPt.pcd", c);
//}
if(nearestId == 0 && poses.size())
{
UWARN("Flushing poses (%d) because nearest id of %d can't be found!", (int)poses.size(), fromId);
poses.clear();
}
}
}
UDEBUG("nearestId = %d, minDistance=%f poses=%d", nearestId, minDistance, (int)poses.size());
}
return poses;
}
// Get paths in front of the robot, returned optimized poses
std::list<std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transform> poses) const
{
std::list<std::map<int, Transform> > paths;
if(_memory && poses.size())
{
// Segment poses connected only by neighbor links
while(poses.size())
{
std::map<int, Transform> path;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
{
if(path.size() == 0 || uContains(_memory->getNeighborLinks(path.rbegin()->first), iter->first))
{
path.insert(*iter);
poses.erase(iter++);
}
else
{
break;
}
}
UASSERT(path.size());
paths.push_back(path);
}
}
return paths;
}
void Rtabmap::optimizeCurrentMap(
int id,
bool lookInDatabase,
@@ -2556,10 +2605,20 @@ bool Rtabmap::computePath(
links.insert(std::make_pair(iter->second.to(), iter->first)); // <->
}
// Add links between neighbor nodes in the goal radius.
if(_planWithNearNodesLinked)
if(_planVirtualLinks)
{
std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI);
links.insert(clusters.begin(), clusters.end());
for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
{
if(graph::findLink(links, iter->first, iter->second) != links.end())
{
if(_planVirtualLinksMaxDiffID <= 0 ||
abs(iter->first - iter->second) < _planVirtualLinksMaxDiffID)
{
links.insert(*iter);
}
}
}
}
UINFO("Computing path from location %d to %d", currentNode, targetNode);