diff --git a/renderer/cpp/src/display.cpp b/renderer/cpp/src/display.cpp index 52e3fe82d25df5543e46020a95f727e3eb37f29f..172095ff4132d6e98f709b61335567eb02b6bb3b 100644 --- a/renderer/cpp/src/display.cpp +++ b/renderer/cpp/src/display.cpp @@ -36,6 +36,7 @@ Display::~Display() { #endif // HAVE_VIZ } +#if defined HAVE_PCL static pcl::PointCloud<pcl::PointXYZRGB>::Ptr matToPointXYZ(const cv::Mat &OpencVPointCloud, const cv::Mat &rgbimage) { /* * Function: Get from a Mat to pcl pointcloud datatype @@ -46,8 +47,6 @@ static pcl::PointCloud<pcl::PointXYZRGB>::Ptr matToPointXYZ(const cv::Mat &Openc //char pr=100, pg=100, pb=100; pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);//(new pcl::pointcloud<pcl::pointXYZ>); - LOG(INFO) << "OCVPC SIZE = " << OpencVPointCloud.cols << "," << OpencVPointCloud.rows; - for(int i=0;i<OpencVPointCloud.rows;i++) { for(int j=0;j<OpencVPointCloud.cols;j++) { //std::cout<<i<<endl; @@ -58,10 +57,10 @@ static pcl::PointCloud<pcl::PointXYZRGB>::Ptr matToPointXYZ(const cv::Mat &Openc point.y = cvpoint[1]; //OpencVPointCloud.at<float>(1,i); point.z = cvpoint[2]; //OpencVPointCloud.at<float>(2,i); - cv::Point3_<uchar> prgb; // = rgbimage.at<cv::Point3_<uchar>>((int)point.x, (int)point.y); + 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.x) << 16 | static_cast<uint32_t>(prgb.y) << 8 | static_cast<uint32_t>(prgb.z)); + uint32_t rgb = (static_cast<uint32_t>(prgb.z) << 16 | static_cast<uint32_t>(prgb.y) << 8 | static_cast<uint32_t>(prgb.x)); point.rgb = *reinterpret_cast<float*>(&rgb); point_cloud_ptr -> points.push_back(point); @@ -72,6 +71,7 @@ static pcl::PointCloud<pcl::PointXYZRGB>::Ptr matToPointXYZ(const cv::Mat &Openc return point_cloud_ptr; } +#endif // HAVE_PCL bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) { Mat idepth; @@ -99,7 +99,7 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) { );*/ cv::Mat_<cv::Vec3f> XYZ(depth.rows, depth.cols); // Output point cloud - reprojectImageTo3D(depth+20.0f, XYZ, q_, true); + reprojectImageTo3D(depth, XYZ, q_, true); auto pc = matToPointXYZ(XYZ, rgb); pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pc);