Merge branch 'pcl'

.gitlab-ci.yml

@@ -5,7 +5,7 @@ windows job:
     - 'call "C:/Program Files (x86)/Microsoft Visual Studio/2017/Community/VC/Auxiliary/Build/vcvars64.bat"'
     - mkdir build
     - cd build
-    - 'cmake -DCMAKE_GENERATOR_PLATFORM=x64 -DOpenCV_DIR="D:/opencv-4.0.1/build/install" -DCUDA_TOOLKIT_ROOT_DIR="C:/Program Files/NVIDIA GPU Computing Toolkit/CUDA/v10.1" ..'
+    - 'cmake -DWITH_PCL=FALSE -DCMAKE_GENERATOR_PLATFORM=x64 -DOpenCV_DIR="D:/opencv-4.0.1/build/install" -DCUDA_TOOLKIT_ROOT_DIR="C:/Program Files/NVIDIA GPU Computing Toolkit/CUDA/v10.1" ..'
     - devenv ftl.utu.fi.sln /build Release
   
 linux job:
@@ -15,9 +15,9 @@ linux job:
   image: ubuntu:18.04
   
   before_script:
-   - export DEBIAN_FRONTEND=noninteractive
-   - apt-get update -qq && apt-get install -y -qq g++ cmake git
-   - apt-get install -y -qq libopencv-dev libgoogle-glog-dev liburiparser-dev libreadline-dev libmsgpack-dev uuid-dev
+    - export DEBIAN_FRONTEND=noninteractive
+    - apt-get update -qq && apt-get install -y -qq g++ cmake git
+    - apt-get install -y -qq libopencv-dev libgoogle-glog-dev liburiparser-dev libreadline-dev libmsgpack-dev uuid-dev libpcl-dev
   script:
     - mkdir build
     - cd build

CMakeLists.txt

@@ -11,6 +11,8 @@ include(GNUInstallDirs)
 include(CTest)
 enable_testing()
 
+option(WITH_PCL "Use PCL if available" ON)
+option(WITH_FIXSTARS "Use Fixstars libSGM if available" ON)
 option(BUILD_VISION "Enable the vision component" ON)
 option(BUILD_RECONSTRUCT "Enable the reconstruction component" ON)
 option(BUILD_RENDERER "Enable the renderer component" ON)
@@ -19,16 +21,31 @@ set(THREADS_PREFER_PTHREAD_FLAG ON)
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/")
 
 include(Findglog)
-#find_package( glog REQUIRED )
+
 find_package( OpenCV REQUIRED )
 find_package( Threads REQUIRED )
 find_package( URIParser REQUIRED )
 find_package( MsgPack REQUIRED )
-find_package( LibSGM )
-#find_package( ZLIB REQUIRED )
 
+if (WITH_FIXSTARS)
+	find_package( LibSGM )
+endif()
+
+if(${CMAKE_VERSION} VERSION_GREATER "3.12.0") 
+	cmake_policy(SET CMP0074 NEW)
+endif()
+
+if (WITH_PCL)
+	find_package( PCL QUIET COMPONENTS io common visualization registration )
+endif()
+
+set(CMAKE_CXX_STANDARD 17) # For PCL/VTK https://github.com/PointCloudLibrary/pcl/issues/2686
 set(HAVE_OPENCV TRUE)
 
+if (PCL_FOUND)
+	set(HAVE_PCL TRUE)
+endif()
+
 # Readline library is not required on Windows
 # May also entirely remove dependence on this... it should be optional at least.
 if (NOT WIN32)

common/cpp/CMakeLists.txt

 set(COMMONSRC
 	src/config.cpp
 	src/configuration.cpp
+	src/opencv_to_pcl.cpp
 )
 
 add_library(ftlcommon ${COMMONSRC})
 
 target_include_directories(ftlcommon PUBLIC
+	${PCL_INCLUDE_DIRS}
 	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
 	$<INSTALL_INTERFACE:include>
 	PRIVATE src)
-target_link_libraries(ftlcommon glog::glog)
+target_link_libraries(ftlcommon glog::glog ${OpenCV_LIBS} ${PCL_LIBRARIES})
 

common/cpp/include/ftl/config.h.in

@@ -6,6 +6,7 @@
 #cmakedefine HAVE_URIPARSESINGLE
 #cmakedefine HAVE_CUDA
 #cmakedefine HAVE_OPENCV
+#cmakedefine HAVE_PCL
 #cmakedefine HAVE_RENDER
 
 extern const char *FTL_VERSION_LONG;

common/cpp/include/ftl/utility/opencv_to_pcl.hpp

+#ifndef _FTL_COMMON_OPENCV_TO_PCL_HPP_
+#define _FTL_COMMON_OPENCV_TO_PCL_HPP_
+
+#include <ftl/config.h>
+#include <opencv2/opencv.hpp>
+
+#if defined HAVE_PCL
+#include <pcl/common/common_headers.h>
+
+namespace ftl {
+namespace utility {
+
+/**
+ * Convert an OpenCV point cloud matrix and RGB image to a PCL XYZRGB point cloud.
+ */
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr matToPointXYZ(const cv::Mat &cvcloud, const cv::Mat &rgbimage);
+
+};
+};
+#endif  // HAVE_PCL
+
+#endif  // _FTL_COMMON_OPENCV_TO_PCL_HPP_

common/cpp/src/opencv_to_pcl.cpp

+#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>);
+
+	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));
+		point.rgb = *reinterpret_cast<float*>(&rgb);
+
+		point_cloud_ptr -> points.push_back(point);
+	}
+	}
+	point_cloud_ptr->width = cvcloud.cols;
+	point_cloud_ptr->height = cvcloud.rows;
+
+	return point_cloud_ptr;
+}
+#endif  // HAVE_PCL
\ No newline at end of file

renderer/cpp/CMakeLists.txt

-# Need to include staged files and libs
-#include_directories(${PROJECT_SOURCE_DIR}/net/cpp/include)
-#include_directories(${PROJECT_BINARY_DIR})
-
-
 add_library(ftlrender
 	src/display.cpp
 )
 
+# These cause errors in CI build and are being removed from PCL in newer versions
+# target_compile_options(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 ftlcommon Threads::Threads glog::glog ${OpenCV_LIBS} ${PCL_LIBRARIES})
 
 #ADD_SUBDIRECTORY(test)

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 {
 
 /**
@@ -30,6 +35,9 @@ 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 render(const cv::Mat &img, style_t s=STYLE_NORMAL);
 
 	bool active() const;
@@ -40,9 +48,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

@@ -5,16 +5,68 @@
 #include <glog/logging.h>
 
 #include <ftl/display.hpp>
+#include <ftl/utility/opencv_to_pcl.hpp>
 
 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_->setShowFPS(true);
+	pclviz_->initCameraParameters ();
+
+	pclviz_->registerKeyboardCallback (
+		[](const pcl::visualization::KeyboardEvent &event, void* viewer_void) {
+			auto viewer = *static_cast<pcl::visualization::PCLVisualizer::Ptr*>(viewer_void);
+			pcl::visualization::Camera cam;
+			viewer->getCameraParameters(cam);
+
+			Eigen::Vector3f pos(cam.pos[0], cam.pos[1], cam.pos[2]);
+			Eigen::Vector3f focal(cam.focal[0], cam.focal[1], cam.focal[2]);
+			Eigen::Vector3f dir = focal - pos; //.normalize();
+			dir.normalize();
+
+			const float speed = 40.0f;
+
+			if (event.getKeySym() == "Up") {
+				pos += speed*dir;
+				focal += speed*dir;
+			} else if (event.getKeySym() == "Down") {
+				pos -= speed*dir;
+				focal -= speed*dir;
+			} else if (event.getKeySym() == "Left") {
+				Eigen::Matrix3f m = Eigen::AngleAxisf(-0.5f*M_PI, Eigen::Vector3f::UnitY()).toRotationMatrix();
+				dir = m*dir;
+				pos += speed*dir;
+				focal += speed*dir;
+			} else if (event.getKeySym() == "Right") {
+				Eigen::Matrix3f m = Eigen::AngleAxisf(0.5f*M_PI, Eigen::Vector3f::UnitY()).toRotationMatrix();
+				dir = m*dir;
+				pos += speed*dir;
+				focal += speed*dir;
+			}
+
+
+			cam.pos[0] = pos[0];
+			cam.pos[1] = pos[1];
+			cam.pos[2] = pos[2];
+			cam.focal[0] = focal[0];
+			cam.focal[1] = focal[1];
+			cam.focal[2] = focal[2];
+			viewer->setCameraParameters(cam);
+
+		}, (void*)&pclviz_);
+#endif  // HAVE_PCL
+
 	active_ = true;
 }
 
@@ -28,7 +80,18 @@ 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
+		cv::Mat_<cv::Vec3f> XYZ(depth.rows, depth.cols);   // Output point cloud
+		reprojectImageTo3D(depth, XYZ, q_, false);
+		auto pc = ftl::utility::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_->setCameraPosition(-878.0, -71.0, -2315.0, -0.1, -0.99, 0.068, 0.0, -1.0, 0.0);
+			pclviz_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "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 +108,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,6 +147,17 @@ 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);
+	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");
+	return true;
+}
+#endif  // HAVE_PCL
 bool Display::render(const cv::Mat &img, style_t s) {
 	if (s == STYLE_NORMAL) {
 		cv::imshow("Image", img);
@@ -107,7 +181,9 @@ bool Display::render(const cv::Mat &img, style_t s) {
 
 void Display::wait(int ms) {
 	if (config_["points"] && q_.rows != 0) {
-		#if defined HAVE_VIZ
+		#if defined HAVE_PCL
+		pclviz_->spinOnce(20);
+		#elif defined HAVE_VIZ
 		window_->spinOnce(1, true);
 		#endif  // HAVE_VIZ
 	}
@@ -121,7 +197,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_;