i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

DBViewer: fixed grid cell size in 3D View. Memory/Grid, using Icp/PointToPlaneGroundNormalsUp parameter when normals are computed. Grid: fixed 2D noHit ray tracing.

Browse Source
This commit is contained in:
matlabbe
2022-01-25 18:10:43 -05:00
parent 2dc7b59b05
commit d56692640e
11 changed files with 91 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
corelib/src/Memory.cpp
View File

@@ -98,6 +98,7 @@ Memory::Memory(const ParametersMap & parameters) :
_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
_laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
_laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()),
_laserScanGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
_localBundleOnLoopClosure(Parameters::defaultRGBDLocalBundleOnLoopClosure()),
_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
@@ -565,6 +566,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
Parameters::parse(params, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
Parameters::parse(params, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
Parameters::parse(params, Parameters::kIcpPointToPlaneGroundNormalsUp(), _laserScanGroundNormalsUp);
Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
Parameters::parse(params, Parameters::kRGBDLocalBundleOnLoopClosure(), _localBundleOnLoopClosure);
Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
@@ -5480,7 +5482,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
0,
_laserScanVoxelSize,
_laserScanNormalK,
_laserScanNormalRadius);
_laserScanNormalRadius,
_laserScanGroundNormalsUp);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemScan_filtering(), t*1000.0f);
UDEBUG("time normals scan = %fs", t);

2
corelib/src/OccupancyGrid.cpp
View File

@@ -56,6 +56,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
projMapFrame_(Parameters::defaultGridMapFrameProjection()),
maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
normalKSearch_(Parameters::defaultGridNormalK()),
groundNormalsUp_(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f),
clusterRadius_(Parameters::defaultGridClusterRadius()),
minClusterSize_(Parameters::defaultGridMinClusterSize()),
@@ -115,6 +116,7 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), groundNormalsUp_);
if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
{
maxGroundAngle_ *= M_PI/180.0f;

74
corelib/src/util3d_filtering.cpp
View File

@@ -1930,20 +1930,22 @@ pcl::IndicesPtr normalFiltering(
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint)
const Eigen::Vector4f & viewpoint,
float groundNormalsUp)
{
pcl::IndicesPtr indices(new std::vector<int>);
return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint)
const Eigen::Vector4f & viewpoint,
float groundNormalsUp)
{
pcl::IndicesPtr indices(new std::vector<int>);
return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
return normalFiltering(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
}
@@ -1954,7 +1956,8 @@ pcl::IndicesPtr normalFilteringImpl(
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint)
const Eigen::Vector4f & viewpoint,
float groundNormalsUp)
{
pcl::IndicesPtr output(new std::vector<int>());
@@ -1994,6 +1997,12 @@ pcl::IndicesPtr normalFilteringImpl(
for(unsigned int i=0; i<cloud_normals->size(); ++i)
{
Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->size()!=0?indices->at(i):i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
v *= -1.0f;
}
float angle = pcl::getAngle3D(normal, v);
if(angle < angleMax)
{
@@ -2011,10 +2020,10 @@ pcl::IndicesPtr normalFiltering(
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint)
const Eigen::Vector4f & viewpoint,
float groundNormalsUp)
{
return normalFilteringImpl<pcl::PointXYZ>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
return normalFilteringImpl<pcl::PointXYZ>(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -2022,9 +2031,10 @@ pcl::IndicesPtr normalFiltering(
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint)
const Eigen::Vector4f & viewpoint,
float groundNormalsUp)
{
return normalFilteringImpl<pcl::PointXYZRGB>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
return normalFilteringImpl<pcl::PointXYZRGB>(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
@@ -2032,17 +2042,20 @@ pcl::IndicesPtr normalFiltering(
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint)
const Eigen::Vector4f & viewpoint,
float groundNormalsUp)
{
return normalFilteringImpl<pcl::PointXYZI>(cloud, indices, angleMax, normal, normalKSearch, viewpoint);
return normalFilteringImpl<pcl::PointXYZI>(cloud, indices, angleMax, normal, normalKSearch, viewpoint, groundNormalsUp);
}
template<typename PointT>
template<typename PointNormalT>
pcl::IndicesPtr normalFilteringImpl(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const typename pcl::PointCloud<PointNormalT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal)
const Eigen::Vector4f & normal,
const Eigen::Vector4f & viewpoint,
float groundNormalsUp)
{
pcl::IndicesPtr output(new std::vector<int>());
@@ -2055,6 +2068,11 @@ pcl::IndicesPtr normalFilteringImpl(
for(unsigned int i=0; i<indices->size(); ++i)
{
Eigen::Vector4f v(cloud->at(indices->at(i)).normal_x, cloud->at(indices->at(i)).normal_y, cloud->at(indices->at(i)).normal_z, 0.0f);
if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(indices->at(i)).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
v *= -1.0f;
}
float angle = pcl::getAngle3D(normal, v);
if(angle < angleMax)
{
@@ -2068,6 +2086,11 @@ pcl::IndicesPtr normalFilteringImpl(
for(unsigned int i=0; i<cloud->size(); ++i)
{
Eigen::Vector4f v(cloud->at(i).normal_x, cloud->at(i).normal_y, cloud->at(i).normal_z, 0.0f);
if(groundNormalsUp>0.0f && v[2] < -groundNormalsUp && cloud->at(i).z < viewpoint[3]) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
v *= -1.0f;
}
float angle = pcl::getAngle3D(normal, v);
if(angle < angleMax)
{
@@ -2086,30 +2109,33 @@ pcl::IndicesPtr normalFiltering(
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint)
int,
const Eigen::Vector4f & viewpoint,
float groundNormalsUp)
{
return normalFilteringImpl<pcl::PointNormal>(cloud, indices, angleMax, normal);
return normalFilteringImpl<pcl::PointNormal>(cloud, indices, angleMax, normal, viewpoint, groundNormalsUp);
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint)
int,
const Eigen::Vector4f & viewpoint,
float groundNormalsUp)
{
return normalFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, angleMax, normal);
return normalFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, angleMax, normal, viewpoint, groundNormalsUp);
}
pcl::IndicesPtr normalFiltering(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float angleMax,
const Eigen::Vector4f & normal,
int normalKSearch,
const Eigen::Vector4f & viewpoint)
int,
const Eigen::Vector4f & viewpoint,
float groundNormalsUp)
{
return normalFilteringImpl<pcl::PointXYZINormal>(cloud, indices, angleMax, normal);
return normalFilteringImpl<pcl::PointXYZINormal>(cloud, indices, angleMax, normal, viewpoint, groundNormalsUp);
}
std::vector<pcl::IndicesPtr> extractClusters(

2
corelib/src/util3d_mapping.cpp
View File

@@ -122,7 +122,7 @@ void occupancy2DFromLaserScan(
}
else
{
scanNoHit = scanHit;
scanNoHit = scanNoHitIn;
}
std::map<int, Transform> poses;