From a72448ed6390bf9f01dbd5e1e6f696c93ef28ce0 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Thu, 23 May 2019 09:47:42 +0300
Subject: [PATCH] Allow point source colouring
---
applications/reconstruct/src/main.cpp | 41 ++++++++++++++++++++++++++-
1 file changed, 40 insertions(+), 1 deletion(-)
diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index f688f41c8..42e14f458 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -67,6 +67,7 @@ using cv::Mat;
namespace ftl {
namespace rgbd {
PointCloud<PointXYZRGB>::Ptr createPointCloud(RGBDSource *src);
+PointCloud<PointXYZRGB>::Ptr createPointCloud(RGBDSource *src, const cv::Point3_<uchar> &col);
}
}
@@ -109,6 +110,44 @@ PointCloud<PointXYZRGB>::Ptr ftl::rgbd::createPointCloud(RGBDSource *src) {
return point_cloud_ptr;
}
+PointCloud<PointXYZRGB>::Ptr ftl::rgbd::createPointCloud(RGBDSource *src, const cv::Point3_<uchar> &prgb) {
+ const double CX = src->getParameters().cx;
+ const double CY = src->getParameters().cy;
+ const double FX = src->getParameters().fx;
+ const double FY = src->getParameters().fy;
+
+ cv::Mat rgb;
+ cv::Mat depth;
+ src->getRGBD(rgb, depth);
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
+ point_cloud_ptr->width = rgb.cols * rgb.rows;
+ point_cloud_ptr->height = 1;
+
+ for(int i=0;i<rgb.rows;i++) {
+ const float *sptr = depth.ptr<float>(i);
+ for(int j=0;j<rgb.cols;j++) {
+ float d = sptr[j] * 1000.0f;
+
+ pcl::PointXYZRGB point;
+ point.x = (((double)j + CX) / FX) * d;
+ point.y = (((double)i + CY) / FY) * d;
+ point.z = d;
+
+ if (point.x == INFINITY || point.y == INFINITY || point.z > 20000.0f || point.z < 0.04f) {
+ point.x = 0.0f; point.y = 0.0f; point.z = 0.0f;
+ }
+
+ 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);
+ }
+ }
+
+ return point_cloud_ptr;
+}
+
#ifdef HAVE_PCL
#include <pcl/filters/uniform_sampling.h>
@@ -332,7 +371,7 @@ static void run() {
//if (!display.active()) continue;
active += 1;
- clouds[i] = ftl::rgbd::createPointCloud(input);
+ clouds[i] = ftl::rgbd::createPointCloud(input); //, (i==0) ? cv::Point3_<uchar>(255,0,0) : cv::Point3_<uchar>(0,0,255));
//display.render(rgb, depth,input->getParameters());
//display.render(clouds[i]);
--
GitLab