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

Added Mem/LaserScanVoxelSize and OdomF2M/ScanSubtractAngle parameters. util3d::computeNormalsComplexity() now returns PCA's eigen vectors and values optionally. RegistrationIcp: detecting complexity of environment when PointToPLane is used, if too low, PointToPoint is done and movements are limited to main direction of the normals.

Browse Source
This commit is contained in:
matlabbe
2017-09-19 14:15:17 -04:00
parent cf6478b633
commit fd18c0b2e9
12 changed files with 491 additions and 188 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+31
View File
@@ -88,6 +88,7 @@ Memory::Memory(const ParametersMap & parameters) :
_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
_laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
_laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()),
_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
@@ -440,6 +441,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
Parameters::parse(parameters, Parameters::kMemCompressionParallelized(), _compressionParallelized);
Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
Parameters::parse(parameters, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
Parameters::parse(parameters, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
Parameters::parse(parameters, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
@@ -3760,6 +3762,35 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
if(stats) stats->addStatistic(Statistics::kTimingMemScan_downsampling(), t*1000.0f);
UDEBUG("time downsampling scan = %fs", t);
}
if(!laserScan.empty() && _laserScanVoxelSize > 0.0f && !isIntermediateNode)
{
float pointsBeforeFiltering = laserScan.cols;
if(laserScan.channels() == 4 || laserScan.channels() == 7)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(laserScan);
cloud = util3d::voxelize(cloud, _laserScanVoxelSize);
laserScan = util3d::laserScanFromPointCloud(*cloud);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
cloud = util3d::voxelize(cloud, _laserScanVoxelSize);
if(laserScan.channels() == 2 || laserScan.channels() == 5)
{
laserScan = util3d::laserScan2dFromPointCloud(*cloud);
}
else
{
laserScan = util3d::laserScanFromPointCloud(*cloud);
}
}
float ratio = float(laserScan.cols) / pointsBeforeFiltering;
maxLaserScanMaxPts = int(float(maxLaserScanMaxPts) * ratio);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemScan_voxel_filtering(), t*1000.0f);
UDEBUG("time voxel filtering scan = %fs", t);
}
if(!laserScan.empty() &&
(_laserScanNormalK > 0 || _laserScanNormalRadius>0.0f) &&
laserScan.channels() > 1 && laserScan.channels() < 5 &&