i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-11 12:09:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

Support input RGB laser scans

Browse Source
This commit is contained in:
matlabbe
2017-09-18 14:36:58 -04:00
parent aee8e0c645
commit c452990fa5
1 changed files with 50 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+50 -17
View File
@@ -1458,12 +1458,16 @@ bool convertScan3dMsg(
float scanCloudNormalRadius) float scanCloudNormalRadius)
{ {
bool containNormals = false; bool containNormals = false;
bool containColors = false;
for(unsigned int i=0; i<scan3dMsg->fields.size(); ++i) for(unsigned int i=0; i<scan3dMsg->fields.size(); ++i)
{ {
if(scan3dMsg->fields[i].name.compare("normal_x") == 0) if(scan3dMsg->fields[i].name.compare("normal_x") == 0)
{ {
containNormals = true; containNormals = true;
break; }
if(scan3dMsg->fields[i].name.compare("rgb") == 0 || scan3dMsg->fields[i].name.compare("rgba") == 0)
{
containColors = true;
} }
} }
@@ -1497,29 +1501,58 @@ bool convertScan3dMsg(
if(containNormals) if(containNormals)
{ {
pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>); if(containColors)
pcl::fromROSMsg(*scan3dMsg, *pclScan);
scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*scan3dMsg, *pclScan);
if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
{ {
//compute normals pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclScan, scanCloudNormalK, scanCloudNormalRadius); pcl::fromROSMsg(*scan3dMsg, *pclScan);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>); scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = rtabmap::util3d::laserScanFromPointCloud(*rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScanNormal));
} }
else else
{ {
pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*scan3dMsg, *pclScan);
scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan); scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
} }
} }
else
{
if(containColors)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromROSMsg(*scan3dMsg, *pclScan);
if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclScan, scanCloudNormalK, scanCloudNormalRadius);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = rtabmap::util3d::laserScanFromPointCloud(*rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScanNormal));
}
else
{
scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
}
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*scan3dMsg, *pclScan);
if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclScan, scanCloudNormalK, scanCloudNormalRadius);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = rtabmap::util3d::laserScanFromPointCloud(*rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScanNormal));
}
else
{
scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
}
}
}
return true; return true;
} }