added small user interface and alignment of scans

3d-scanner.cpp

@@ -1,8 +1,9 @@
 #include <librealsense2/rs.hpp>
 #include <iostream>
+#include <filesystem>
 #include <pcl/io/ply_io.h>
 #include <pcl/point_types.h>
-#include "pcl-converter.hpp"
+#include "pcl-helper.hpp"
 
 struct ColoredPoints
 {
@@ -12,9 +13,14 @@ struct ColoredPoints
     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);
+void configure_filters(rs2::decimation_filter &dec_filter, rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, int dec_filter_magnitude, float min_dist, float max_dist, int magnitude, int smooth_alpha, int smooth_delta, int holes_fill);
+void apply_filters(rs2::decimation_filter &dec_filter, 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::decimation_filter &dec_filter, 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);
+void scan_and_convert(rs2::decimation_filter &dec_filter, 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, std::vector<PCLHelper::PtrCloud> &pointclouds);
+bool align_and_save_pointclouds(std::vector<PCLHelper::PtrCloud> &pointclouds);
+void save_original_pointclouds(std::vector<PCLHelper::PtrCloud> &pointclouds);
+void wipe_old_scans();
+int prompt_user();
 
 int main()
 try
@@ -24,17 +30,60 @@ try
     pipe.start();
 
     // define filters
-    rs2::threshold_filter thr_filter; // Threshold  - removes values outside recommended range
-    rs2::spatial_filter spat_filter;  // Spatial    - edge-preserving spatial smoothing
+    rs2::decimation_filter dec_filter; // Decimation - reduces depth frame density
+    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
     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);
-
-    ColoredPoints points = scan(thr_filter, spat_filter, depth_to_disparity, disparity_to_depth, pipe);
-
-    PCLConverter::convert_and_save_ply("output.ply", points.points, points.color);
+    configure_filters(dec_filter, thr_filter, spat_filter, 3, 0.0, 0.4, 5, 1, 1, 5);
+
+    std::vector<PCLHelper::PtrCloud> pointclouds;
+    PCLHelper::PtrCloud converted_points;
+    PCLHelper::PtrCloud cropped_points;
+    bool running = true;
+    while (running)
+    {
+
+        int number = prompt_user();
+
+        switch (number)
+        {
+        case 1:
+            scan_and_convert(dec_filter, thr_filter, spat_filter, depth_to_disparity, disparity_to_depth, pipe, pointclouds);
+            break;
+        case 2:
+            if (pointclouds.size() < 2)
+            {
+                std::cout << "Not enough pointclouds to merge, just exiting...";
+            }
+            else
+            {
+                save_original_pointclouds(pointclouds);
+                if (!align_and_save_pointclouds(pointclouds))
+                {
+                    std::cout << "Failed to align pointclouds, exiting...";
+                    break;
+                }
+                std::cout << "\nYou can find the scanned object parts in the output[Nr].ply files.";
+            }
+
+            break;
+        case 3:
+            pointclouds.clear();
+            std::cout << "Scan buffer cleared.";
+            wipe_old_scans();
+            break;
+        case 4:
+            std::cout << "Exiting...";
+            running = false;
+            break;
+        default:
+            std::cout << "Invalid option, please try again.";
+            break;
+        }
+    }
 
     return EXIT_SUCCESS;
 }
@@ -49,8 +98,9 @@ catch (const std::exception &e)
     return EXIT_FAILURE;
 }
 
-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 configure_filters(rs2::decimation_filter &dec_filter, rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_filter, int dec_filter_magnitude, float min_dist, float max_dist, int magnitude, int smooth_alpha, int smooth_delta, int holes_fill)
 {
+    dec_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, dec_filter_magnitude);
     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
@@ -70,8 +120,9 @@ void configure_filters(rs2::threshold_filter &thr_filter, rs2::spatial_filter &s
     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).
     */
-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)
+void apply_filters(rs2::decimation_filter &dec_filter, 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)
 {
+    depth_frame = dec_filter.process(depth_frame);
     depth_frame = thr_filter.process(depth_frame);
     depth_frame = depth_to_disparity.process(depth_frame);
     depth_frame = spat_filter.process(depth_frame);
@@ -79,7 +130,7 @@ void apply_filters(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_
     // filtered = hole_filter.process(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)
+ColoredPoints scan(rs2::decimation_filter &dec_filter, 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)
 {
     rs2::pointcloud pointcloud;
     rs2::points points;
@@ -94,10 +145,82 @@ ColoredPoints scan(rs2::threshold_filter &thr_filter, rs2::spatial_filter &spat_
         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);
+    apply_filters(dec_filter, 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);
+}
+
+void scan_and_convert(rs2::decimation_filter &dec_filter, 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, std::vector<PCLHelper::PtrCloud> &pointclouds)
+{
+    // does a scan, convertes it to pcl format and puts it into an array
+    std::cout << "Scanning... ";
+    ColoredPoints scanned_points = scan(dec_filter, thr_filter, spat_filter, depth_to_disparity, disparity_to_depth, pipe);
+    std::cout << "\nConverting... ";
+    PCLHelper::PtrCloud converted_points = PCLHelper::points_to_pcl(scanned_points.points, scanned_points.color);
+    // PCLHelper::PtrCloud cropped_points = PCLHelper::crop_cloud(converted_points, -0.15, -0.2, 0.0, 0.15, 0.05, 0.3);
+    // pointclouds.push_back(cropped_points);
+    pointclouds.push_back(converted_points);
+}
+
+bool align_and_save_pointclouds(std::vector<PCLHelper::PtrCloud> &pointclouds)
+{
+    for (int i = 0; i < pointclouds.size(); i++)
+    {
+        if (i == 0)
+        {
+            PCLHelper::save_to_ply("scans_fitted/", "output" + std::to_string(i) + ".ply", pointclouds[i]);
+            continue;
+        }
+
+        if (PCLHelper::icp(pointclouds[0], pointclouds[i], 80) == 0)
+        {
+            PCLHelper::save_to_ply("scans_fitted/", "output" + std::to_string(i) + ".ply", pointclouds[i]);
+        }
+        else
+        {
+            std::cout << "ICP failed";
+            return false;
+        }
+    }
+    return true;
+}
+
+void save_original_pointclouds(std::vector<PCLHelper::PtrCloud> &pointclouds)
+{
+    for (int i = 0; i < pointclouds.size(); i++)
+    {
+        PCLHelper::save_to_ply("scans_original/", "output" + std::to_string(i) + ".ply", pointclouds[i]);
+    }
+}
+void wipe_old_scans()
+{
+    std::filesystem::path dirPathFitted = "scans_fitted";
+    std::filesystem::path dirPathOriginal = "scans_original";
+
+    std::uintmax_t numRemovedFitted = std::filesystem::remove_all(dirPathFitted);
+    std::uintmax_t numRemovedOriginal = std::filesystem::remove_all(dirPathOriginal);
+
+    if (numRemovedFitted == 0 && numRemovedOriginal == 0)
+    {
+        std::cout << "No Scans to delete. " << std::filesystem::current_path() << std::endl;
+    }
+    else
+    {
+        std::cout << "All scans delted." << std::endl;
+    }
+}
+
+int prompt_user()
+{
+    int number;
+    std::cout << "\n Welcome to the 3d-scanner, choose your option: ";
+    std::cout << "\n (1) Scan ";
+    std::cout << "\n (2) Align and save pointclouds ";
+    std::cout << "\n (3) Wipe old scans ";
+    std::cout << "\n (4) exit \n";
+    std::cin >> number;
+    return number;
 }

CMakeLists.txt

@@ -46,7 +46,7 @@ find_package(PCL 1.3 REQUIRED)
 include_directories(${PCL_INCLUDE_DIRS})
 link_directories(${PCL_LIBRARY_DIRS})
 add_definitions(${PCL_DEFINITIONS})
-add_executable(3d-scanner 3d-scanner.cpp pcl-converter.cpp) 
+add_executable(3d-scanner 3d-scanner.cpp pcl-helper.cpp) 
 include_directories(3d-scanner ${realsense2_INCLUDE_DIR})
 # include_directories(${CMAKE_CURRENT_SOURCE_DIR}/thirdParty)
 # include_directories(/opt/homebrew/include/)

pcl-converter.cpp

@@ -1,56 +0,0 @@
-#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-helper.cpp

@@ -0,0 +1,103 @@
+#include "pcl-helper.hpp"
+
+// Get RGB values based on normals - texcoords, normals value [u v]
+std::tuple<uint8_t, uint8_t, uint8_t> PCLHelper::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 PCLHelper::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 PCLHelper::convert_and_save_ply(const std::string &fileName, const rs2::points &points, const rs2::video_frame &color)
+{
+    PtrCloud cloud = points_to_pcl(points, color);
+    pcl::io::savePLYFile(fileName, *cloud);
+}
+
+void PCLHelper::save_to_ply(const std::string folderPath, const std::string &fileName, const PtrCloud &cloud)
+{
+    std::filesystem::create_directories(folderPath);
+    pcl::io::savePLYFile(folderPath + fileName, *cloud);
+}
+
+int PCLHelper::icp(PtrCloud &cloud_target, PtrCloud &cloud_source, int iterations)
+{
+    std::cout << "\ntarget cloud size is: " << cloud_target->size() << std::endl;
+    std::cout << "\nsource cloud size is: " << cloud_source->size() << std::endl;
+    // TODO: make faster
+    // icp algo will align source pointcloud to fit target pointcloud
+    pcl::IterativeClosestPoint<PointRGB, PointRGB> icp;
+    icp.setMaximumIterations(iterations);
+    icp.setInputSource(cloud_source); // will be transformed to fit target
+    icp.setInputTarget(cloud_target); // will be the base cloud
+    icp.align(*cloud_source);
+
+    if (icp.hasConverged())
+    {
+        std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
+        return 0;
+    }
+    else
+    {
+        PCL_ERROR("\nICP has not converged.\n");
+        return (-1);
+    }
+}
+
+PtrCloud PCLHelper::crop_cloud(PtrCloud &cloud, float min_x, float min_y, float min_z, float max_x, float max_y, float max_z)
+{
+    Eigen::Vector4f minPoint(min_x, min_y, min_z, 1);
+    Eigen::Vector4f maxPoint(max_x, max_y, max_z, 1);
+
+    pcl::CropBox<PointRGB> cropFilter;
+    cropFilter.setInputCloud(cloud);
+    cropFilter.setMin(minPoint);
+    cropFilter.setMax(maxPoint);
+    // cropFilter.setKeepOrganized(true);
+
+    PtrCloud cropped_cloud(new PointCloudRGB);
+    cropFilter.filter(*cropped_cloud);
+
+    return cropped_cloud;
+}

pcl-converter.hpp

@@ -1,19 +1,30 @@
 #pragma once
 #include <librealsense2/rs.hpp>
 #include <tuple>
+#include <filesystem>
 #include <pcl/io/ply_io.h>
 #include <pcl/point_types.h>
+#include <pcl/registration/icp.h>
+#include <pcl/filters/crop_box.h>
 
 typedef pcl::PointXYZRGB PointRGB;
 typedef pcl::PointCloud<PointRGB> PointCloudRGB;
 typedef PointCloudRGB::Ptr PtrCloud;
 
-class PCLConverter
+class PCLHelper
 {
 public:
+    using PointRGB = pcl::PointXYZRGB;
+    using PointCloudRGB = pcl::PointCloud<PointRGB>;
+    using PtrCloud = PointCloudRGB::Ptr;
     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);
+    static void save_to_ply(const std::string folderPath, const std::string &fileName, const PtrCloud &cloud);
+    static int icp(PtrCloud &cloud_target, PtrCloud &cloud_source, int iterations = 1);
+    static PtrCloud crop_cloud(PtrCloud &cloud, float min_x, float min_y, float min_z, float max_x, float max_y, float max_z);
 
 private:
     static std::tuple<uint8_t, uint8_t, uint8_t> get_texcolor(rs2::video_frame texture, rs2::texture_coordinate texcoords);
-};
+};
+
+// test