test-distortion.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2021 Intel Corporation. All Rights Reserved.
3 
4 //#cmake: static!
5 //#cmake:add-file ../../../src/proc/sse/sse-pointcloud.cpp
6 
7 #include "../algo-common.h"
8 #include <librealsense2/rsutil.h>
9 #include "../src/proc/sse/sse-pointcloud.h"
10 #include "../src/cuda/cuda-pointcloud.cuh"
11 #include "../src/types.h"
12 
14 = { 1280,
15  720,
16  643.720581f,
17  357.821259f,
18  904.170471f,
19  905.155090f,
21  { 0.180086836f, -0.534179211f, -0.00139013783f, 0.000118769123f, 0.470662683f } };
22 
24 {
25  for (auto i = 0; i < 2; i++)
26  {
27  CAPTURE(i);
28  REQUIRE(std::abs(pixel1[i] - pixel2[i]) <= 0.001);
29  }
30 }
31 
32 TEST_CASE( "inverse_brown_conrady_deproject" )
33 {
34  float point[3] = { 0 };
35  librealsense::float2 pixel1 = { 1, 1 };
36  librealsense::float2 pixel2 = { 0, 0 };
37  float depth = 10.5;
38  rs2_deproject_pixel_to_point( point, &intrin, (float*)&pixel1, depth );
39  rs2_project_point_to_pixel((float*)&pixel2, &intrin, point );
40 
41  compare(pixel1, pixel2);
42 }
43 
44 TEST_CASE( "brown_conrady_deproject" )
45 {
46  float point[3] = { 0 };
47 
48  librealsense::float2 pixel1 = { 1, 1 };
49  librealsense::float2 pixel2 = { 0, 0 };
50  float depth = 10.5;
51  rs2_deproject_pixel_to_point( point, &intrin, (float*)&pixel1, depth );
52  rs2_project_point_to_pixel((float*)&pixel2, &intrin, point );
53 
54  compare(pixel1, pixel2);
55 }
56 
57 #if 0 //TODO: check why sse tests fails on LibCi
58 TEST_CASE("inverse_brown_conrady_sse_deproject")
59 {
60  std::shared_ptr<librealsense::pointcloud_sse> pc_sse = std::make_shared<librealsense::pointcloud_sse >();
61 
62  librealsense::float2 pixel[4] = { {1, 1}, {0,2},{1,3},{1,4} };
63  float depth = 10.5;
65 
66  // deproject with native code because sse deprojection doesn't implement distortion
67  for (auto i = 0; i < 4; i++)
68  {
69  rs2_deproject_pixel_to_point((float*)&points[i], &intrin, (float*)&pixel[i], depth);
70  }
71 
72  std::vector<librealsense::float2> res(4, { 0,0 });
73  std::vector<librealsense::float2> unnormalized_res(4, { 0,0 });
74  rs2_extrinsics extrin = { {1,0,0,
75  0,1,0,
76  0,0,1},{0,0,0} };
77 
78  pc_sse->get_texture_map_sse((librealsense::float2*)res.data(), points, 4, 1, intrin, extrin, (librealsense::float2*)unnormalized_res.data());
79 
80  for (auto i = 0; i < 4; i++)
81  {
82  compare(unnormalized_res[i], pixel[i]);
83  }
84 }
85 
86 TEST_CASE("brown_conrady_sse_deproject")
87 {
88  std::shared_ptr<librealsense::pointcloud_sse> pc_sse = std::make_shared<librealsense::pointcloud_sse >();
89 
90  librealsense::float2 pixel[4] = { {1, 1}, {0,2},{1,3},{1,4} };
91  float depth = 10.5;
93 
94  // deproject with native code because sse deprojection doesn't implement distortion
95  for (auto i = 0; i < 4; i++)
96  {
97  rs2_deproject_pixel_to_point((float*)&points[i], &intrin, (float*)&pixel[i], depth);
98  }
99 
100  std::vector<librealsense::float2> res(4, { 0,0 });
101  std::vector<librealsense::float2> unnormalized_res(4, { 0,0 });
102  rs2_extrinsics extrin = { {1,0,0,
103  0,1,0,
104  0,0,1},{0,0,0} };
105 
106  pc_sse->get_texture_map_sse((librealsense::float2*)res.data(), points, 4, 1, intrin, extrin, (librealsense::float2*)unnormalized_res.data());
107 
108  for (auto i = 0; i < 4; i++)
109  {
110  compare(unnormalized_res[i], pixel[i]);
111  }
112 }
113 #endif
114 
115 #ifdef RS2_USE_CUDA
116 TEST_CASE("inverse_brown_conrady_cuda_deproject")
117 {
118  std::vector<float3> point(1280 * 720, { 0,0,0 });
119 
120  librealsense::float2 pixel = { 0, 0 };
121 
122  std::vector<uint16_t> depth(1280 * 720, 1000);
123  rscuda::deproject_depth_cuda((float*)point.data(), intrin, depth.data(), 1);
124  for (auto i = 0; i < 720; i++)
125  {
126  for (auto j = 0; j < 1280; j++)
127  {
128  CAPTURE(i, j);
129  rs2_project_point_to_pixel((float*)&pixel, &intrin, (float*)&point[i* 1280 +j]);
130  compare({ (float)j,(float)i }, pixel);
131  }
132  }
133 }
134 
135 TEST_CASE("brown_conrady_cuda_deproject")
136 {
137  std::vector<float3> point(1280 * 720, { 0,0,0 });
138 
139  librealsense::float2 pixel = { 0, 0 };
140 
141  std::vector<uint16_t> depth(1280 * 720, 1000);
142  rscuda::deproject_depth_cuda((float*)point.data(), intrin, depth.data(), 1);
143  for (auto i = 0; i < 720; i++)
144  {
145  for (auto j = 0; j < 1280; j++)
146  {
147  CAPTURE(i, j);
148  rs2_project_point_to_pixel((float*)&pixel, &intrin, (float*)&point[i * 1280 + j]);
149  compare({ (float)j,(float)i }, pixel);
150  }
151  }
152 }
153 #endif
rs2_intrinsics intrin
GLint GLint GLsizei GLsizei GLsizei depth
point
Definition: rmse.py:166
TEST_CASE("inverse_brown_conrady_deproject")
static void rs2_deproject_pixel_to_point(float point[3], const struct rs2_intrinsics *intrin, const float pixel[2], float depth)
Definition: rsutil.h:83
#define CAPTURE(...)
Definition: catch.hpp:17432
REQUIRE(n_callbacks==1)
GLint j
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented...
Definition: rs_sensor.h:96
static void rs2_project_point_to_pixel(float pixel[2], const struct rs2_intrinsics *intrin, const float point[3])
Definition: rsutil.h:19
std::pair< int, int > pixel
Definition: rs-measure.cpp:18
Video stream intrinsics.
Definition: rs_types.h:58
void compare(librealsense::float2 pixel1, librealsense::float2 pixel2)
int i
GLuint res
Definition: glext.h:8856
GLdouble GLdouble GLint GLint const GLdouble * points


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:50:11