Add function for projecting color to depth

include/librealsense2/rsutil.h

@@ -6,8 +6,12 @@
 
 #include "h/rs_types.h"
 #include "h/rs_sensor.h"
+#include "h/rs_frame.h"
+#include "rs.h"
 #include "assert.h"
-
+#include <stdlib.h>
+#include <stdbool.h>
+#include <stdint.h>
 #include <math.h>
 
 
@@ -80,4 +84,80 @@ static void rs2_fov(const struct rs2_intrinsics * intrin, float to_fov[2])
     to_fov[1] = (atan2f(intrin->ppy + 0.5f, intrin->fy) + atan2f(intrin->height - (intrin->ppy + 0.5f), intrin->fy)) * 57.2957795f;
 }
 
+static void next_pixel_in_line(float curr[2], const float start[2], const float end[2])
+{
+    float line_slope = (end[1] - start[1]) / (end[0] - start[0]);
+    if (abs(end[0] - curr[0]) > abs(end[1] - curr[1]))
+    {
+        curr[0] = end[0] > curr[0] ? curr[0] + 1 : curr[0] - 1;
+        curr[1] = end[1] - line_slope * (end[0] - curr[0]);
+    }
+    else
+    {
+        curr[1] = end[1] > curr[1] ? curr[1] + 1 : curr[1] - 1;
+        curr[0] = end[0] - ((end[1] + curr[1]) / line_slope);
+    }
+}
+
+static bool is_pixel_in_line(const float curr[2], const float start[2], const float end[2])
+{
+    return ((end[0] >= start[0] && end[0] >= curr[0] && curr[0] >= start[0]) || (end[0] <= start[0] && end[0] <= curr[0] && curr[0] <= start[0])) &&
+           ((end[1] >= start[1] && end[1] >= curr[1] && curr[1] >= start[1]) || (end[1] <= start[1] && end[1] <= curr[1] && curr[1] <= start[1]));
+}
+
+static void adjust_2D_point_to_boundary(float p[2], int width, int height)
+{
+    if (p[0] < 0) p[0] = 0;
+    if (p[0] > width) p[0] = width;
+    if (p[1] < 0) p[1] = 0;
+    if (p[1] > height) p[1] = height;
+}
+
+/* Find projected pixel with unknown depth search along line. */
+static void rs2_project_color_pixel_to_depth_pixel(float to_pixel[2],
+    const uint16_t* data, float depth_scale,
+    float depth_min, float depth_max, 
+    const struct rs2_intrinsics* depth_intrin,
+    const struct rs2_intrinsics* color_intrin,
+    const struct rs2_extrinsics* color_to_depth,
+    const struct rs2_extrinsics* depth_to_color,
+    const float from_pixel[2])
+{
+    //Find line start pixel
+    float start_pixel[2] = { 0 }, min_point[3] = { 0 }, min_transformed_point[3] = { 0 };
+    rs2_deproject_pixel_to_point(min_point, color_intrin, from_pixel, depth_min);
+    rs2_transform_point_to_point(min_transformed_point, color_to_depth, min_point);
+    rs2_project_point_to_pixel(start_pixel, depth_intrin, min_transformed_point);
+    adjust_2D_point_to_boundary(start_pixel, depth_intrin->width, depth_intrin->height);
+
+    //Find line end depth pixel
+    float end_pixel[2] = { 0 }, max_point[3] = { 0 }, max_transformed_point[3] = { 0 };
+    rs2_deproject_pixel_to_point(max_point, color_intrin, from_pixel, depth_max);
+    rs2_transform_point_to_point(max_transformed_point, color_to_depth, max_point);
+    rs2_project_point_to_pixel(end_pixel, depth_intrin, max_transformed_point);
+    adjust_2D_point_to_boundary(end_pixel, depth_intrin->width, depth_intrin->height);
+
+    //search along line for the depth pixel that it's projected pixel is the closest to the input pixel
+    float min_dist = -1;
+    for (float p[2] = { start_pixel[0], start_pixel[1] }; is_pixel_in_line(p, start_pixel, end_pixel); next_pixel_in_line(p, start_pixel, end_pixel))
+    {
+        float depth = depth_scale * data[(int)p[1] * depth_intrin->width + (int)p[0]];
+        if (depth == 0) 
+            continue;
+
+        float projected_pixel[2] = { 0 }, point[3] = { 0 }, transformed_point[3] = { 0 };
+        rs2_deproject_pixel_to_point(point, depth_intrin, p, depth);
+        rs2_transform_point_to_point(transformed_point, depth_to_color, point);
+        rs2_project_point_to_pixel(projected_pixel, color_intrin, transformed_point);
+
+        float new_dist = pow((projected_pixel[1] - from_pixel[1]), 2) + pow((projected_pixel[0] - from_pixel[0]), 2);
+        if (new_dist < min_dist || min_dist < 0)
+        {
+            min_dist = new_dist;
+            to_pixel[0] = p[0];
+            to_pixel[1] = p[1];
+        }
+    }
+}
+
 #endif

unit-tests/CMakeLists.txt

@@ -38,7 +38,7 @@ install(
     TARGETS
 
     live-test
-
+	
     RUNTIME DESTINATION
     ${CMAKE_INSTALL_PREFIX}/bin
 )
@@ -52,6 +52,12 @@ else() # Data shall be preserved between reboots. For Linux distributions/ ANDRO
     set(Deployment_Location /tmp/)
 endif()
 
+add_custom_command(TARGET live-test POST_BUILD
+    COMMAND ${CMAKE_COMMAND} -E copy_directory
+    ${CMAKE_CURRENT_SOURCE_DIR}/resources
+    ${Deployment_Location}
+)
+
 #Post-Processing data set for unit-tests
 #message(STATUS "Post processing deployment directory=${Deployment_Location}")
 list(APPEND PP_Tests_List  1525186403504 # D415_DS(2)

unit-tests/unit-tests-live.cpp

@@ -13,6 +13,7 @@
 #include <chrono>
 #include <ctime>
 #include <algorithm>
+#include <librealsense2/rsutil.h>
 
 using namespace rs2;
 
@@ -4811,3 +4812,94 @@ TEST_CASE("Syncer try wait for frames", "[live][software-device]") {
         }
     }
 }
+
+TEST_CASE("Projection from recording", "[software-device][using_pipeline][projection]") {
+    rs2::context ctx;
+    if (!make_context(SECTION_FROM_TEST_NAME, &ctx, "2.13.0"))
+        return;
+    std::string folder_name = get_folder_path(special_folder::temp_folder);
+    const std::string filename = folder_name + "single_depth_color_640x480.bag";
+    REQUIRE(file_exists(filename));
+    auto dev = ctx.load_device(filename);
+
+    syncer sync;
+    std::vector<sensor> sensors = dev.query_sensors();
+    REQUIRE(sensors.size() == 2);
+    for (auto s : sensors)
+    {
+        REQUIRE_NOTHROW(s.open(s.get_stream_profiles().front()));
+        REQUIRE_NOTHROW(s.start(sync));
+    }
+
+    rs2::frame depth;
+    rs2::stream_profile depth_profile;
+    rs2::stream_profile color_profile;
+
+    while (!depth_profile || !color_profile)
+    {
+        frameset frames = sync.wait_for_frames(200);
+        REQUIRE(frames.size() > 0);
+        if (frames.size() == 1)
+        {
+            if (frames.get_profile().stream_type() == RS2_STREAM_DEPTH)
+            {
+                depth = frames.get_depth_frame();
+                depth_profile = depth.get_profile();
+            }
+            else
+            {
+                color_profile = frames.get_color_frame().get_profile();
+            }
+        }
+        else
+        {
+            depth = frames.get_depth_frame();
+            depth_profile = depth.get_profile();
+            color_profile = frames.get_color_frame().get_profile();
+        }
+    }
+
+    auto depth_intrin = depth_profile.as<rs2::video_stream_profile>().get_intrinsics();
+    auto color_intrin = color_profile.as<rs2::video_stream_profile>().get_intrinsics();
+    auto depth_extrin_to_color = depth_profile.as<rs2::video_stream_profile>().get_extrinsics_to(color_profile);
+    auto color_extrin_to_depth = color_profile.as<rs2::video_stream_profile>().get_extrinsics_to(depth_profile);
+
+    float depth_scale = 0;
+    for (auto s : sensors)
+    {
+        auto depth_sensor = s.is<rs2::depth_sensor>();
+        if (s.is<rs2::depth_sensor>())
+        {
+            REQUIRE_NOTHROW(depth_scale = s.as<rs2::depth_sensor>().get_depth_scale());
+        }
+    }
+
+    int count = 0;
+    for (float i = 0; i < depth_intrin.width; i++)
+    {
+        for (float j = 0; j < depth_intrin.height; j++)
+        {
+            float depth_pixel[2] = { i, j };
+            auto udist = depth.as<rs2::depth_frame>().get_distance(depth_pixel[0], depth_pixel[1]);
+            if (udist == 0) continue;
+
+            float from_pixel[2] = { 0 }, to_pixel[2] = { 0 }, point[3] = { 0 }, other_point[3] = { 0 };
+            rs2_deproject_pixel_to_point(point, &depth_intrin, depth_pixel, udist);
+            rs2_transform_point_to_point(other_point, &depth_extrin_to_color, point);
+            rs2_project_point_to_pixel(from_pixel, &color_intrin, other_point);
+
+            rs2_project_color_pixel_to_depth_pixel(to_pixel, reinterpret_cast<const uint16_t*>(depth.get_data()), depth_scale, 0.1, 10,
+                &depth_intrin, &color_intrin,
+                &color_extrin_to_depth, &depth_extrin_to_color, from_pixel);
+
+            float dist = sqrt(pow((depth_pixel[1] - to_pixel[1]), 2) + pow((depth_pixel[0] - to_pixel[0]), 2));
+            if (dist > 1)
+                count++;
+            if (dist > 2)
+                printf("%f\n", dist);
+        }
+    }
+    const double MAX_ERROR_PERCENTAGE = 0.1;
+    REQUIRE(count * 100 / (depth_intrin.width * depth_intrin.height) < MAX_ERROR_PERCENTAGE);
+    CAPTURE(count);
+}