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

Fixed util3d::laserScanToPointCloudRGB for 2 channels scan (added also default color param)

Browse Source
This commit is contained in:
matlabbe
2016-10-05 18:11:58 -04:00
parent 9e7b905c0b
commit 37192422be
2 changed files with 14 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/include/rtabmap/core/util3d.h
View File

@@ -195,15 +195,15 @@ cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXY
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform()); pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null. // For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform = Transform()); pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform = Transform());
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to null. // For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform = Transform()); pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform = Transform(), unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
// For laserScan of type CV_32FC2, z is set to null. // For laserScan of type CV_32FC2, z is set to null.
pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const cv::Mat & laserScan, int index); pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const cv::Mat & laserScan, int index);
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null. // For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const cv::Mat & laserScan, int index); pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const cv::Mat & laserScan, int index);
// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to null. // For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to default r,g,b parameters.
pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const cv::Mat & laserScan, int index); pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r = 255, unsigned char g = 255, unsigned char b = 255);
cv::Point3f RTABMAP_EXP projectDisparityTo3D( cv::Point3f RTABMAP_EXP projectDisparityTo3D(

14
corelib/src/util3d.cpp
View File

@@ -1240,7 +1240,7 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat
return output; return output;
} }
pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform) pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
{ {
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || 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(6));
@@ -1250,7 +1250,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat &
Eigen::Affine3f transform3f = transform.toEigen3f(); Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.cols; ++i) for(int i=0; i<laserScan.cols; ++i)
{ {
output->at(i) = util3d::laserScanToPointRGB(laserScan, i); output->at(i) = util3d::laserScanToPointRGB(laserScan, i, r, g, b);
if(!nullTransform) if(!nullTransform)
{ {
output->at(i) = pcl::transformPoint(output->at(i), transform3f); output->at(i) = pcl::transformPoint(output->at(i), transform3f);
@@ -1295,7 +1295,7 @@ pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
return output; return output;
} }
pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index) pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
{ {
UASSERT(!laserScan.empty() && index < laserScan.cols); UASSERT(!laserScan.empty() && index < laserScan.cols);
UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6)); UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
@@ -1303,7 +1303,7 @@ pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index)
const float * ptr = laserScan.ptr<float>(0, index); const float * ptr = laserScan.ptr<float>(0, index);
output.x = ptr[0]; output.x = ptr[0];
output.y = ptr[1]; output.y = ptr[1];
if(laserScan.type() >= 3) if(laserScan.channels() >= 3)
{ {
output.z = ptr[2]; output.z = ptr[2];
} }
@@ -1314,6 +1314,12 @@ pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index)
output.g = (unsigned char)((ptrInt[3] >> 8) & 0xFF); output.g = (unsigned char)((ptrInt[3] >> 8) & 0xFF);
output.r = (unsigned char)((ptrInt[3] >> 16) & 0xFF); output.r = (unsigned char)((ptrInt[3] >> 16) & 0xFF);
} }
else
{
output.r = r;
output.g = g;
output.b = b;
}
return output; return output;
} }