Add initial PCL vis code

renderer/cpp/CMakeLists.txt

@@ -7,11 +7,15 @@ add_library(ftlrender
 	src/display.cpp
 )
 
+# target_include_directories(ftl-vision PUBLIC ${PCL_INCLUDE_DIRS})
+target_compile_definitions(ftlrender PUBLIC ${PCL_DEFINITIONS})
+
 target_include_directories(ftlrender PUBLIC
+${PCL_INCLUDE_DIRS}
 	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
 	$<INSTALL_INTERFACE:include>
 	PRIVATE src)
-target_link_libraries(ftlrender Threads::Threads glog::glog ${OpenCV_LIBS})
+target_link_libraries(ftlrender Threads::Threads glog::glog ${OpenCV_LIBS} ${PCL_LIBRARIES})
 
 
 

renderer/cpp/include/ftl/display.hpp

@@ -11,6 +11,11 @@
 #include <opencv2/opencv.hpp>
 #include "opencv2/highgui.hpp"
 
+#if defined HAVE_PCL
+#include <pcl/common/common_headers.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#endif  // HAVE_PCL
+
 namespace ftl {
 
 /**
@@ -25,6 +30,10 @@ class Display {
 
 	bool render(const cv::Mat &rgb, const cv::Mat &depth);
 
+#if defined HAVE_PCL
+	bool render(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr);
+#endif  // HAVE_PCL
+
 	bool active() const;
 	
 	void wait(int ms);
@@ -33,9 +42,13 @@ class Display {
 	cv::Mat q_;
 	nlohmann::json config_;
 
-	#if defined HAVE_VIZ
+#if defined HAVE_VIZ
 	cv::viz::Viz3d *window_;
-	#endif  // HAVE_VIZ
+#endif  // HAVE_VIZ
+
+#if defined HAVE_PCL
+	pcl::visualization::PCLVisualizer::Ptr pclviz_;
+#endif  // HAVE_PCL
 
 	bool active_;
 };

renderer/cpp/src/display.cpp

@@ -6,15 +6,27 @@
 
 #include <ftl/display.hpp>
 
+#if defined HAVE_PCL
+#include <pcl/compression/organized_pointcloud_conversion.h>
+#endif  // HAVE_PCL
+
 using ftl::Display;
 using cv::Mat;
 using cv::Vec3f;
 
 Display::Display(nlohmann::json &config) : config_(config) {
-	#if defined HAVE_VIZ
+#if defined HAVE_VIZ
 	window_ = new cv::viz::Viz3d("FTL");
 	window_->setBackgroundColor(cv::viz::Color::white());
-	#endif  // HAVE_VIZ
+#endif  // HAVE_VIZ
+
+#if defined HAVE_PCL
+	pclviz_ = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer ("FTL Cloud"));
+	pclviz_->setBackgroundColor (255, 255, 255);
+	pclviz_->addCoordinateSystem (1.0);
+	pclviz_->initCameraParameters ();
+#endif  // HAVE_PCL
+
 	active_ = true;
 }
 
@@ -24,11 +36,79 @@ Display::~Display() {
 	#endif  // HAVE_VIZ
 }
 
+static pcl::PointCloud<pcl::PointXYZRGB>::Ptr matToPointXYZ(const cv::Mat &OpencVPointCloud, const cv::Mat &rgbimage) {
+	/*
+	*  Function: Get from a Mat to pcl pointcloud datatype
+	*  In: cv::Mat
+	*  Out: pcl::PointCloud
+	*/
+
+	//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;
+
+		pcl::PointXYZRGB point;
+		cv::Vec3f cvpoint = OpencVPointCloud.at<cv::Vec3f>(i,j);
+		point.x = cvpoint[0]; //OpencVPointCloud.at<float>(0,i);
+		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);
+
+		// 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));
+		point.rgb = *reinterpret_cast<float*>(&rgb);
+
+		point_cloud_ptr -> points.push_back(point);
+	}
+	}
+	point_cloud_ptr->width = (int)point_cloud_ptr->points.size();
+	point_cloud_ptr->height = 1;
+
+	return point_cloud_ptr;
+}
+
 bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) {
 	Mat idepth;
 
 	if (config_["points"] && q_.rows != 0) {
-		#if defined HAVE_VIZ
+#if defined HAVE_PCL
+		/*Mat ddepth;
+		depth.convertTo(ddepth, CV_16SC1, 4096);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc(new pcl::PointCloud<pcl::PointXYZRGB>);;
+		float focal = q_.at<float>(2,3);
+		float dshift = -1.0f / q_.at<float>(3,2);
+		float dscale = 256.0f;
+		std::vector<uint16_t> dispData;
+		std::vector<uint8_t> rgbData;
+
+		LOG(INFO) << "Focal = " << focal << ", baseline = " << dshift;
+
+		dispData.resize(ddepth.cols*ddepth.rows*sizeof(uint16_t));
+		rgbData.resize(rgb.cols*rgb.rows*sizeof(uint8_t)*3);
+		dispData.assign(ddepth.data, ddepth.data + dispData.size());
+		rgbData.assign(rgb.data, rgb.data + rgbData.size());
+
+		pcl::io::OrganizedConversion<pcl::PointXYZRGB>::convert(
+			*pc, focal, dshift, dscale, true, dispData, rgbData
+		);*/
+
+		cv::Mat_<cv::Vec3f> XYZ(depth.rows, depth.cols);   // Output point cloud
+		reprojectImageTo3D(depth+20.0f, XYZ, q_, true);
+		auto pc = matToPointXYZ(XYZ, rgb);
+
+		pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pc);
+		if (!pclviz_->updatePointCloud<pcl::PointXYZRGB> (pc, rgb, "reconstruction")) {
+			pclviz_->addPointCloud<pcl::PointXYZRGB> (pc, rgb, "reconstruction");
+		}
+
+		pclviz_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "reconstruction");
+#elif defined HAVE_VIZ
 		//cv::Mat Q_32F;
 		//calibrate_.getQ().convertTo(Q_32F, CV_32F);
 		cv::Mat_<cv::Vec3f> XYZ(depth.rows, depth.cols);   // Output point cloud
@@ -45,11 +125,11 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) {
 
 		//window_->spinOnce(40, true);
 
-		#else  // HAVE_VIZ
+#else  // HAVE_VIZ
 
 		LOG(ERROR) << "Need OpenCV Viz module to display points";
 
-		#endif  // HAVE_VIZ
+#endif  // HAVE_VIZ
 	}
 
 	if (config_["left"]) {
@@ -84,9 +164,19 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) {
 	return true;
 }
 
+#if defined HAVE_PCL
+bool Display::render(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr pc) {
+	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pc);
+	pclviz_->addPointCloud<pcl::PointXYZRGB> (pc, rgb, "reconstruction");
+	return true;
+}
+#endif  // HAVE_PCL
+
 void Display::wait(int ms) {
 	if (config_["points"] && q_.rows != 0) {
-		#if defined HAVE_VIZ
+		#if defined HAVE_PCL
+		pclviz_->spinOnce (100);
+		#elif defined HAVE_VIZ
 		window_->spinOnce(1, true);
 		#endif  // HAVE_VIZ
 	}
@@ -100,7 +190,9 @@ void Display::wait(int ms) {
 }
 
 bool Display::active() const {
-	#if defined HAVE_VIZ
+	#if defined HAVE_PCL
+	return active_ && !pclviz_->wasStopped();
+	#elif defined HAVE_VIZ
 	return active_ && !window_->wasStopped();
 	#else
 	return active_;

vision/CMakeLists.txt

@@ -46,11 +46,7 @@ if (CUDA_FOUND)
 set_property(TARGET ftl-vision PROPERTY CUDA_SEPARABLE_COMPILATION ON)
 endif()
 
-# TODO: move PCL stuff elsewhere
-target_include_directories(ftl-vision PUBLIC ${PCL_INCLUDE_DIRS})
-target_compile_definitions(ftl-vision PUBLIC ${PCL_DEFINITIONS})
-
 #target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
-target_link_libraries(ftl-vision ftlcommon ftlrender Threads::Threads libelas ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} glog::glog ftlnet ${PCL_LIBRARIES})
+target_link_libraries(ftl-vision ftlcommon ftlrender Threads::Threads libelas ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} glog::glog ftlnet)