added conversion from rs2 to pcl and refactoring

3d-scanner.cpp

@@ -1,17 +1,24 @@
 #include <librealsense2/rs.hpp>
 #include <iostream>
+#include <pcl/io/ply_io.h>
+#include <pcl/point_types.h>
+#include "pcl-converter.hpp"
 
-void update_data_and_save(rs2::frame data_frame, rs2::points &points, rs2::pointcloud &pc, rs2::frame &color, const std::string &fileName);
+struct ColoredPoints
+{
+    rs2::points points;
+    rs2::video_frame color;
+
+    ColoredPoints(rs2::points p, rs2::video_frame c) : points(p), color(c) {}
+};
+
+void configure_filters(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, float min_dist, float max_dist, int magnitude, int smooth_alpha, int smooth_delta, int holes_fill);
+void apply_filters(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, rs2::disparity_transform &depth_to_disparity, rs2::disparity_transform &disparity_to_depth, rs2::depth_frame &depth_frame);
+ColoredPoints scan(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, rs2::disparity_transform &depth_to_disparity, rs2::disparity_transform &disparity_to_depth, rs2::pipeline &pipe);
 
 int main()
 try
 {
-
-    rs2::pointcloud original_pc;
-    rs2::pointcloud filtered_pc;
-    rs2::points original_points;
-    rs2::points filtered_points;
-
     rs2::pipeline pipe;
 
     pipe.start();
@@ -19,33 +26,41 @@ try
     // define filters
     rs2::threshold_filter thr_filter; // Threshold  - removes values outside recommended range
     rs2::spatial_filter spat_filter;  // Spatial    - edge-preserving spatial smoothing
-    // rs2::hole_filling_filter hole_filter;                  // Hole filling - fills small holes in the depth image
+    // rs2::hole_filling_filter hole_filter;                    // Hole filling - fills small holes in the depth image
     const std::string disparity_filter_name = "Disparity"; // Declare disparity transform from depth to disparity and vice versa
     rs2::disparity_transform depth_to_disparity(true);
     rs2::disparity_transform disparity_to_depth(false);
+    configure_filters(thr_filter, spat_filter, 0.0, 0.5, 5, 1, 1, 5);
 
-    // configure filters
-    thr_filter.set_option(RS2_OPTION_MIN_DISTANCE, 0.0);
-    thr_filter.set_option(RS2_OPTION_MAX_DISTANCE, 0.5);
-    spat_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, 5);
-    spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, 1);
-    spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, 1);
-    spat_filter.set_option(RS2_OPTION_HOLES_FILL, 5);
-    // hole_filter.set_option(RS2_OPTION_HOLES_FILL, 1);
+    ColoredPoints points = scan(thr_filter, spat_filter, depth_to_disparity, disparity_to_depth, pipe);
 
-    // let the auto exposure settle
-    for (auto i = 0; i < 30; ++i)
-        pipe.wait_for_frames();
+    PCLConverter::convert_and_save_ply("output.ply", points.points, points.color);
 
-    rs2::frameset data = pipe.wait_for_frames();
-    rs2::video_frame color = data.get_color_frame();
-    if (!color)
-        color = data.get_infrared_frame();
-    rs2::depth_frame depth_frame = data.get_depth_frame();
+    return EXIT_SUCCESS;
+}
+catch (const rs2::error &e)
+{
+    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
+    return EXIT_FAILURE;
+}
+catch (const std::exception &e)
+{
+    std::cerr << e.what() << std::endl;
+    return EXIT_FAILURE;
+}
 
-    rs2::depth_frame filtered = depth_frame;
+void configure_filters(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, float min_dist, float max_dist, int magnitude, int smooth_alpha, int smooth_delta, int holes_fill)
+{
+    thr_filter.set_option(RS2_OPTION_MIN_DISTANCE, min_dist);             // 0.0
+    thr_filter.set_option(RS2_OPTION_MAX_DISTANCE, max_dist);             // 0.5
+    spat_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, magnitude);       // 5
+    spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, smooth_alpha); // 1
+    spat_filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, smooth_delta); // 1
+    spat_filter.set_option(RS2_OPTION_HOLES_FILL, holes_fill);            // 5
+    // hole_filter.set_option(RS2_OPTION_HOLES_FILL, 1); //1
+}
 
-    /* Apply filters.
+/* Apply filters.
     The implemented flow of the filters pipeline is in the following order:
     1. apply decimation filter
     2. apply threshold filter
@@ -55,32 +70,34 @@ try
     6. revert the results back (if step Disparity filter was applied
     to depth domain (each post processing block is optional and can be applied independantly).
     */
-
-    filtered = thr_filter.process(filtered);
-    filtered = depth_to_disparity.process(filtered);
-    filtered = spat_filter.process(filtered);
-    filtered = disparity_to_depth.process(filtered);
-    // filtered = hole_filter.process(filtered);
-
-    update_data_and_save(depth_frame, original_points, original_pc, color, "original");
-    update_data_and_save(filtered, filtered_points, filtered_pc, color, "filtered");
-
-    return EXIT_SUCCESS;
-}
-catch (const rs2::error &e)
+void apply_filters(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, rs2::disparity_transform &depth_to_disparity, rs2::disparity_transform &disparity_to_depth, rs2::depth_frame &depth_frame)
 {
-    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
-    return EXIT_FAILURE;
-}
-catch (const std::exception &e)
-{
-    std::cerr << e.what() << std::endl;
-    return EXIT_FAILURE;
+    depth_frame = thr_filter.process(depth_frame);
+    depth_frame = depth_to_disparity.process(depth_frame);
+    depth_frame = spat_filter.process(depth_frame);
+    depth_frame = disparity_to_depth.process(depth_frame);
+    // filtered = hole_filter.process(depth_frame);
 }
 
-void update_data_and_save(rs2::frame data_frame, rs2::points &points, rs2::pointcloud &pc, rs2::frame &color, const std::string &fileName)
+ColoredPoints scan(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, rs2::disparity_transform &depth_to_disparity, rs2::disparity_transform &disparity_to_depth, rs2::pipeline &pipe)
 {
-    pc.map_to(color);                               // Map the colored image to the point cloud
-    points = pc.calculate(data_frame);              // Generate pointcloud from the depth data
-    points.export_to_ply(fileName + ".ply", color); // export pointcloud to .ply file
+    rs2::pointcloud pointcloud;
+    rs2::points points;
+
+    // let the auto exposure settle
+    for (auto i = 0; i < 30; ++i)
+        pipe.wait_for_frames();
+
+    rs2::frameset data = pipe.wait_for_frames();
+    rs2::video_frame color = data.get_color_frame();
+    if (!color)
+        color = data.get_infrared_frame();
+    rs2::depth_frame depth_frame = data.get_depth_frame();
+
+    apply_filters(thr_filter, spat_filter, depth_to_disparity, disparity_to_depth, depth_frame);
+
+    pointcloud.map_to(color);
+    points = pointcloud.calculate(depth_frame);
+
+    return ColoredPoints(points, color);
 }

CMakeLists.txt

@@ -39,14 +39,19 @@ endif()
 #     message(WARN "Failed to find_library(realsense2)")
 # endif()
 find_package(realsense2 REQUIRED)
+find_package(PCL 1.3 REQUIRED)
 # find_package(GLFW3 REQUIRED)
 
 # set(SOURCES 3d-scanner.cpp ${CMAKE_CURRENT_SOURCE_DIR}/thirdParty/openglHelper.hpp)
-add_executable(3d-scanner 3d-scanner.cpp)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+add_executable(3d-scanner 3d-scanner.cpp pcl-converter.cpp) 
 include_directories(3d-scanner ${realsense2_INCLUDE_DIR})
 # include_directories(${CMAKE_CURRENT_SOURCE_DIR}/thirdParty)
 # include_directories(/opt/homebrew/include/)
 # target_link_libraries(3d-scanner ${DEPENDENCIES})
+target_link_libraries(3d-scanner ${PCL_LIBRARIES})
 target_link_libraries(3d-scanner ${realsense2_LIBRARY})
 # target_link_libraries(3d-scanner glfw)
 # set_target_properties (3d-scanner PROPERTIES FOLDER Examples)s

pcl-converter.cpp

@@ -0,0 +1,56 @@
+#include "pcl-converter.hpp"
+
+// Get RGB values based on normals - texcoords, normals value [u v]
+std::tuple<uint8_t, uint8_t, uint8_t> PCLConverter::get_texcolor(rs2::video_frame texture, rs2::texture_coordinate texcoords)
+{
+    const int w = texture.get_width(), h = texture.get_height();
+
+    // convert normals [u v] to basic coords [x y]
+    int x = std::min(std::max(int(texcoords.u * w + .5f), 0), w - 1);
+    int y = std::min(std::max(int(texcoords.v * h + .5f), 0), h - 1);
+
+    int idx = x * texture.get_bytes_per_pixel() + y * texture.get_stride_in_bytes();
+    const auto texture_data = reinterpret_cast<const uint8_t *>(texture.get_data());
+    return std::tuple<uint8_t, uint8_t, uint8_t>(texture_data[idx], texture_data[idx + 1], texture_data[idx + 2]);
+}
+
+PtrCloud PCLConverter::points_to_pcl(const rs2::points &points, const rs2::video_frame &color)
+{
+
+    auto sp = points.get_profile().as<rs2::video_stream_profile>();
+    PtrCloud cloud(new PointCloudRGB);
+
+    // Config of PCL Cloud object
+    cloud->width = static_cast<uint32_t>(sp.width());
+    cloud->height = static_cast<uint32_t>(sp.height());
+    cloud->is_dense = false;
+    cloud->points.resize(points.size());
+
+    auto tex_coords = points.get_texture_coordinates();
+    auto vertices = points.get_vertices();
+
+    // Iterating through all points and setting XYZ coordinates
+    // and RGB values
+    for (int i = 0; i < points.size(); ++i)
+    {
+        cloud->points[i].x = vertices[i].x;
+        cloud->points[i].y = vertices[i].y;
+        cloud->points[i].z = vertices[i].z;
+
+        std::tuple<uint8_t, uint8_t, uint8_t> current_color;
+        current_color = get_texcolor(color, tex_coords[i]);
+
+        // Reversed order- 2-1-0 because of BGR (Blue, Green, Red) model used in camera
+        cloud->points[i].r = std::get<2>(current_color);
+        cloud->points[i].g = std::get<1>(current_color);
+        cloud->points[i].b = std::get<0>(current_color);
+    }
+
+    return cloud;
+}
+
+void PCLConverter::convert_and_save_ply(const std::string &fileName, const rs2::points &points, const rs2::video_frame &color)
+{
+    auto cloud = points_to_pcl(points, color);
+    pcl::io::savePLYFile(fileName, *cloud);
+}

pcl-converter.hpp

@@ -0,0 +1,19 @@
+#pragma once
+#include <librealsense2/rs.hpp>
+#include <tuple>
+#include <pcl/io/ply_io.h>
+#include <pcl/point_types.h>
+
+typedef pcl::PointXYZRGB PointRGB;
+typedef pcl::PointCloud<PointRGB> PointCloudRGB;
+typedef PointCloudRGB::Ptr PtrCloud;
+
+class PCLConverter
+{
+public:
+    static PtrCloud points_to_pcl(const rs2::points &points, const rs2::video_frame &color);
+    static void convert_and_save_ply(const std::string &fileName, const rs2::points &points, const rs2::video_frame &color);
+
+private:
+    static std::tuple<uint8_t, uint8_t, uint8_t> get_texcolor(rs2::video_frame texture, rs2::texture_coordinate texcoords);
+};