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

Removed parameter "LccIcp/HighTransitionalVariance" (Identity covariance is set directly on pose correction and local loop closure detection in space)

Browse Source 
Covariance of virtual links added on the path is set to Identity. For those added to keep the path linked to current map, their covariance is set to 100.
DatabaseViewer: set fixed colors on constraints view
This commit is contained in:
Mathieu Labbe
2015-02-25 17:06:52 -05:00
parent eb96fe1249
commit 1d39db2bcc
11 changed files with 174 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -332,7 +332,6 @@ 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, HighTransitionalVariance, bool, true, "Set high transitional variance on odometry pose correction and on local loop closure in space. Particularly useful when the robot is moving in hallways with short-range laser rangefinder.");
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.");
@@ -358,7 +357,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(LccIcp2, MaxCorrespondenceDistance, float, 0.1, "Max distance for point correspondences.");
RTABMAP_PARAM(LccIcp2, Iterations, int, 30, "Max iterations.");
RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(LccIcp2, VoxelSize, float, 0.005, "Voxel size to be used for ICP computation.");
RTABMAP_PARAM(LccIcp2, VoxelSize, float, 0.05, "Voxel size to be used for ICP computation.");
// Stereo disparity
RTABMAP_PARAM(Stereo, WinSize, int, 16, "See cv::calcOpticalFlowPyrLK().");

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

@@ -183,7 +183,6 @@ private:
bool _startNewMapOnLoopClosure;
float _goalReachedRadius; // meters
bool _planWithNearNodesLinked;
bool _icpHighTransVariance;
std::pair<int, float> _loopClosureHypothesis;
std::pair<int, float> _highestHypothesis;

12
corelib/src/Rtabmap.cpp
View File

@@ -58,7 +58,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define LOG_I "LogI.txt"
#define GRAPH_FILE_NAME "Graph.dot"
#define HIGH_VARIANCE 10000
//
//
@@ -111,7 +111,6 @@ Rtabmap::Rtabmap() :
_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
_planWithNearNodesLinked(Parameters::defaultRGBDPlanWithNearNodesLinked()),
_icpHighTransVariance(Parameters::defaultLccIcpHighTransitionalVariance()),
_loopClosureHypothesis(0,0.0f),
_highestHypothesis(0,0.0f),
_lastProcessTime(0.0),
@@ -378,7 +377,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
Parameters::parse(parameters, Parameters::kRGBDPlanWithNearNodesLinked(), _planWithNearNodesLinked);
Parameters::parse(parameters, Parameters::kLccIcpHighTransitionalVariance(), _icpHighTransVariance);
// RGB-D SLAM stuff
if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
@@ -873,7 +871,7 @@ bool Rtabmap::process(const SensorData & data)
oldId,
signature->getLinks().at(oldId).transform().prettyPrint().c_str(),
t.prettyPrint().c_str());
_memory->updateLink(signature->id(), oldId, t, variance, _icpHighTransVariance?HIGH_VARIANCE:variance);
_memory->updateLink(signature->id(), oldId, t, 1, 1); // set Identify covariance
}
else
{
@@ -1486,7 +1484,7 @@ bool Rtabmap::process(const SensorData & data)
signature->id(),
localSpaceNearestId,
t.prettyPrint().c_str());
_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, variance, _icpHighTransVariance?HIGH_VARIANCE:variance);
_memory->addLink(localSpaceNearestId, signature->id(), t, Link::kLocalSpaceClosure, 1, 1); // set Identify covariance
// Old map -> new map, used for localization correction on loop closure
const Signature * oldS = _memory->getSignature(localSpaceNearestId);
@@ -1568,7 +1566,7 @@ bool Rtabmap::process(const SensorData & data)
Transform virtualLoop = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(_path[_pathCurrentIndex].first);
if(_localRadius > 0.0f && virtualLoop.getNorm() < _localRadius)
{
_memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, HIGH_VARIANCE, HIGH_VARIANCE);
_memory->addLink(_path[_pathCurrentIndex].first, signature->id(), virtualLoop, Link::kVirtualClosure, 100, 100); // set high variance
}
}
}
@@ -2608,7 +2606,7 @@ void Rtabmap::updateGoalIndex()
if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
{
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
_memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, HIGH_VARIANCE, HIGH_VARIANCE);
_memory->addLink(_path[i-1].first, _path[i].first, virtualLoop, Link::kVirtualClosure, 1, 1); // on the optimized path, set Identity variance
UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
}
}

11
corelib/src/util3d.cpp
View File

@@ -589,6 +589,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
int decimation)
{
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
UASSERT(imageDepth.rows % decimation == 0);
UASSERT(imageDepth.cols % decimation == 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(decimation < 1)
{
@@ -630,6 +633,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
{
UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
UASSERT(imageDepth.rows % decimation == 0);
UASSERT(imageDepth.cols % decimation == 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(decimation < 1)
{
@@ -691,6 +697,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
int decimation)
{
UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
UASSERT(imageDisparity.rows % decimation == 0);
UASSERT(imageDisparity.cols % decimation == 0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if(decimation < 1)
{
@@ -738,6 +747,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
UASSERT(imageRgb.rows == imageDisparity.rows &&
imageRgb.cols == imageDisparity.cols &&
(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1));
UASSERT(imageDisparity.rows % decimation == 0);
UASSERT(imageDisparity.cols % decimation == 0);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(decimation < 1)
{