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

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); UASSERT(_scanDecimation >= 1);
UTimer timer; UTimer timer;
pcl::IndicesPtr validIndices(new std::vector<int>); pcl::IndicesPtr validIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData( pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
data, data,
_scanDecimation, _scanDecimation,
_scanMaxDepth, _scanMaxDepth,
@@ -317,7 +317,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
} }
else if(!cloud->is_dense) 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); pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
cloud = denseCloud; cloud = denseCloud;
} }
@@ -328,7 +328,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
{ {
Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z()); Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint); 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); pcl::concatenateFields(*cloud, *normals, *cloudNormals);
scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse()); 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) 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(); 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[3] = pt.normal_y;
out[4] = pt.normal_z; out[4] = pt.normal_z;
} }
else // 6 and 7 channels else if(laserScan.type() == CV_32FC(6))
{ {
pcl::PointNormal pt; pcl::PointNormal pt;
pt.x=ptr[0]; pt.x=ptr[0];
@@ -98,6 +98,23 @@ cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transfor
out[4] = pt.normal_y; out[4] = pt.normal_y;
out[5] = pt.normal_z; 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; return output;