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

util3d::transformLaserScan(): supporting 7 channels

Browse Source
This commit is contained in:
matlabbe
2017-09-18 21:49:57 -04:00
parent a70996f079
commit cf6478b633
2 changed files with 22 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
corelib/src/CameraThread.cpp
View File

@@ -298,7 +298,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
UASSERT(_scanDecimation >= 1);
UTimer timer;
pcl::IndicesPtr validIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
data,
_scanDecimation,
_scanMaxDepth,
@@ -317,7 +317,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
}
else if(!cloud->is_dense)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
cloud = denseCloud;
}
@@ -328,7 +328,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
{
Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint);
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
}

21
corelib/src/util3d_transforms.cpp
View File

@@ -38,7 +38,7 @@ namespace util3d
cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6));
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
cv::Mat output = laserScan.clone();
@@ -81,7 +81,7 @@ cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transfor
out[3] = pt.normal_y;
out[4] = pt.normal_z;
}
else // 6 and 7 channels
else if(laserScan.type() == CV_32FC(6))
{
pcl::PointNormal pt;
pt.x=ptr[0];
@@ -98,6 +98,23 @@ cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transfor
out[4] = pt.normal_y;
out[5] = pt.normal_z;
}
else // 7 channels
{
pcl::PointNormal pt;
pt.x=ptr[0];
pt.y=ptr[1];
pt.z=ptr[2];
pt.normal_x=ptr[4];
pt.normal_y=ptr[5];
pt.normal_z=ptr[6];
pt = util3d::transformPoint(pt, transform);
out[0] = pt.x;
out[1] = pt.y;
out[2] = pt.z;
out[4] = pt.normal_x;
out[5] = pt.normal_y;
out[6] = pt.normal_z;
}
}
}
return output;