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

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)
{