diff --git a/common/cpp/CMakeLists.txt b/common/cpp/CMakeLists.txt
index 2a9be0f3b3f32963d088d6af06809db57281d3b7..c8248564d8629021ce24b40f7952a20a4d7337bc 100644
--- a/common/cpp/CMakeLists.txt
+++ b/common/cpp/CMakeLists.txt
@@ -1,13 +1,15 @@
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})
diff --git a/common/cpp/include/ftl/utility/opencv_to_pcl.hpp b/common/cpp/include/ftl/utility/opencv_to_pcl.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ce6a8fa1f37731e20b0aec2d9f43dbdfece56ca6
--- /dev/null
+++ b/common/cpp/include/ftl/utility/opencv_to_pcl.hpp
@@ -0,0 +1,22 @@
+#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>
+#endif // HAVE_PCL
+
+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 // _FTL_COMMON_OPENCV_TO_PCL_HPP_
diff --git a/common/cpp/src/opencv_to_pcl.cpp b/common/cpp/src/opencv_to_pcl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e1cd4683067bbf564594268d9bc11f6ce1aa9c38
--- /dev/null
+++ b/common/cpp/src/opencv_to_pcl.cpp
@@ -0,0 +1,35 @@
+#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
diff --git a/renderer/cpp/CMakeLists.txt b/renderer/cpp/CMakeLists.txt
index 6453c92a60f2931febe2bb433ad167df74928911..6ef0af73b9e9f21de44ab64c6e9ecbe30b6ee6a8 100644
--- a/renderer/cpp/CMakeLists.txt
+++ b/renderer/cpp/CMakeLists.txt
@@ -10,6 +10,6 @@ target_include_directories(ftlrender PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
PRIVATE src)
-target_link_libraries(ftlrender Threads::Threads glog::glog ${OpenCV_LIBS} ${PCL_LIBRARIES})
+target_link_libraries(ftlrender ftlcommon Threads::Threads glog::glog ${OpenCV_LIBS} ${PCL_LIBRARIES})
#ADD_SUBDIRECTORY(test)
diff --git a/renderer/cpp/src/display.cpp b/renderer/cpp/src/display.cpp
index 45945b090d23049a30dee06de8cc59849e2b450a..c24fe35b1a5972ce34ff00fcf5ddcfa60bc0ac14 100644
--- a/renderer/cpp/src/display.cpp
+++ b/renderer/cpp/src/display.cpp
@@ -5,6 +5,7 @@
#include <glog/logging.h>
#include <ftl/display.hpp>
+#include <ftl/utility/opencv_to_pcl.hpp>
using ftl::Display;
using cv::Mat;
@@ -75,48 +76,6 @@ Display::~Display() {
#endif // HAVE_VIZ
}
-#if defined HAVE_PCL
-static pcl::PointCloud<pcl::PointXYZRGB>::Ptr matToPointXYZ(const cv::Mat &cvcloud, 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>);
-
- 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];
-
- //LOG(INFO) << point.x;
- 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
-
bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) {
Mat idepth;
@@ -124,7 +83,7 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) {
#if defined HAVE_PCL
cv::Mat_<cv::Vec3f> XYZ(depth.rows, depth.cols); // Output point cloud
reprojectImageTo3D(depth, XYZ, q_, false);
- auto pc = matToPointXYZ(XYZ, rgb);
+ auto pc = ftl::utility::matToPointXYZ(XYZ, rgb);
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pc);
if (!pclviz_->updatePointCloud<pcl::PointXYZRGB> (pc, rgb, "reconstruction")) {