#include <ftl/utility/opencv_to_pcl.hpp> #if defined HAVE_PCL pcl::PointCloud<pcl::PointXYZRGB>::Ptr ftl::utility::matToPointXYZ(const cv::Mat &cvcloud, const cv::Mat &rgbimage) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>); point_cloud_ptr->width = cvcloud.cols * cvcloud.rows; point_cloud_ptr->height = 1; for(int i=0;i<cvcloud.rows;i++) { for(int j=0;j<cvcloud.cols;j++) { //std::cout<<i<<endl; pcl::PointXYZRGB point; cv::Vec3f cvpoint = cvcloud.at<cv::Vec3f>(i,j); point.x = cvpoint[0]; point.y = cvpoint[1]; point.z = cvpoint[2]; if (point.x == INFINITY || point.y == INFINITY || point.z == INFINITY || point.z < 600.0f) { point.x = 0; point.y = 0; point.z = 0; } cv::Point3_<uchar> prgb = rgbimage.at<cv::Point3_<uchar>>(i, j); // when color needs to be added: uint32_t rgb = (static_cast<uint32_t>(prgb.z) << 16 | static_cast<uint32_t>(prgb.y) << 8 | static_cast<uint32_t>(prgb.x)); // cppcheck-suppress invalidPointerCast point.rgb = *reinterpret_cast<float*>(&rgb); point_cloud_ptr -> points.push_back(point); } } return point_cloud_ptr; } #endif // HAVE_PCL