You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
153 lines
4.6 KiB
153 lines
4.6 KiB
// License: Apache 2.0. See LICENSE file in root directory.
|
|
// Copyright(c) 2021 Intel Corporation. All Rights Reserved.
|
|
|
|
//#cmake: static!
|
|
//#cmake:add-file ../../../src/proc/sse/sse-pointcloud.cpp
|
|
|
|
#include "../algo-common.h"
|
|
#include <librealsense2/rsutil.h>
|
|
#include <src/proc/sse/sse-pointcloud.h>
|
|
#include <src/cuda/cuda-pointcloud.cuh>
|
|
#include <src/types.h>
|
|
|
|
rs2_intrinsics intrin
|
|
= { 1280,
|
|
720,
|
|
643.720581f,
|
|
357.821259f,
|
|
904.170471f,
|
|
905.155090f,
|
|
RS2_DISTORTION_INVERSE_BROWN_CONRADY,
|
|
{ 0.180086836f, -0.534179211f, -0.00139013783f, 0.000118769123f, 0.470662683f } };
|
|
|
|
void compare(librealsense::float2 pixel1, librealsense::float2 pixel2)
|
|
{
|
|
for (auto i = 0; i < 2; i++)
|
|
{
|
|
CAPTURE(i);
|
|
REQUIRE(std::abs(pixel1[i] - pixel2[i]) <= 0.001);
|
|
}
|
|
}
|
|
|
|
TEST_CASE( "inverse_brown_conrady_deproject" )
|
|
{
|
|
float point[3] = { 0 };
|
|
librealsense::float2 pixel1 = { 1, 1 };
|
|
librealsense::float2 pixel2 = { 0, 0 };
|
|
float depth = 10.5;
|
|
rs2_deproject_pixel_to_point( point, &intrin, (float*)&pixel1, depth );
|
|
rs2_project_point_to_pixel((float*)&pixel2, &intrin, point );
|
|
|
|
compare(pixel1, pixel2);
|
|
}
|
|
|
|
TEST_CASE( "brown_conrady_deproject" )
|
|
{
|
|
float point[3] = { 0 };
|
|
|
|
librealsense::float2 pixel1 = { 1, 1 };
|
|
librealsense::float2 pixel2 = { 0, 0 };
|
|
float depth = 10.5;
|
|
rs2_deproject_pixel_to_point( point, &intrin, (float*)&pixel1, depth );
|
|
rs2_project_point_to_pixel((float*)&pixel2, &intrin, point );
|
|
|
|
compare(pixel1, pixel2);
|
|
}
|
|
|
|
#if 0 //TODO: check why sse tests fails on LibCi
|
|
TEST_CASE("inverse_brown_conrady_sse_deproject")
|
|
{
|
|
std::shared_ptr<librealsense::pointcloud_sse> pc_sse = std::make_shared<librealsense::pointcloud_sse >();
|
|
|
|
librealsense::float2 pixel[4] = { {1, 1}, {0,2},{1,3},{1,4} };
|
|
float depth = 10.5;
|
|
librealsense::float3 points[4] = {};
|
|
|
|
// deproject with native code because sse deprojection doesn't implement distortion
|
|
for (auto i = 0; i < 4; i++)
|
|
{
|
|
rs2_deproject_pixel_to_point((float*)&points[i], &intrin, (float*)&pixel[i], depth);
|
|
}
|
|
|
|
std::vector<librealsense::float2> res(4, { 0,0 });
|
|
std::vector<librealsense::float2> unnormalized_res(4, { 0,0 });
|
|
rs2_extrinsics extrin = { {1,0,0,
|
|
0,1,0,
|
|
0,0,1},{0,0,0} };
|
|
|
|
pc_sse->get_texture_map_sse((librealsense::float2*)res.data(), points, 4, 1, intrin, extrin, (librealsense::float2*)unnormalized_res.data());
|
|
|
|
for (auto i = 0; i < 4; i++)
|
|
{
|
|
compare(unnormalized_res[i], pixel[i]);
|
|
}
|
|
}
|
|
|
|
TEST_CASE("brown_conrady_sse_deproject")
|
|
{
|
|
std::shared_ptr<librealsense::pointcloud_sse> pc_sse = std::make_shared<librealsense::pointcloud_sse >();
|
|
|
|
librealsense::float2 pixel[4] = { {1, 1}, {0,2},{1,3},{1,4} };
|
|
float depth = 10.5;
|
|
librealsense::float3 points[4] = {};
|
|
|
|
// deproject with native code because sse deprojection doesn't implement distortion
|
|
for (auto i = 0; i < 4; i++)
|
|
{
|
|
rs2_deproject_pixel_to_point((float*)&points[i], &intrin, (float*)&pixel[i], depth);
|
|
}
|
|
|
|
std::vector<librealsense::float2> res(4, { 0,0 });
|
|
std::vector<librealsense::float2> unnormalized_res(4, { 0,0 });
|
|
rs2_extrinsics extrin = { {1,0,0,
|
|
0,1,0,
|
|
0,0,1},{0,0,0} };
|
|
|
|
pc_sse->get_texture_map_sse((librealsense::float2*)res.data(), points, 4, 1, intrin, extrin, (librealsense::float2*)unnormalized_res.data());
|
|
|
|
for (auto i = 0; i < 4; i++)
|
|
{
|
|
compare(unnormalized_res[i], pixel[i]);
|
|
}
|
|
}
|
|
#endif
|
|
|
|
#ifdef RS2_USE_CUDA
|
|
TEST_CASE("inverse_brown_conrady_cuda_deproject")
|
|
{
|
|
std::vector<float3> point(1280 * 720, { 0,0,0 });
|
|
|
|
librealsense::float2 pixel = { 0, 0 };
|
|
|
|
std::vector<uint16_t> depth(1280 * 720, 1000);
|
|
rscuda::deproject_depth_cuda((float*)point.data(), intrin, depth.data(), 1);
|
|
for (auto i = 0; i < 720; i++)
|
|
{
|
|
for (auto j = 0; j < 1280; j++)
|
|
{
|
|
CAPTURE(i, j);
|
|
rs2_project_point_to_pixel((float*)&pixel, &intrin, (float*)&point[i* 1280 +j]);
|
|
compare({ (float)j,(float)i }, pixel);
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST_CASE("brown_conrady_cuda_deproject")
|
|
{
|
|
std::vector<float3> point(1280 * 720, { 0,0,0 });
|
|
|
|
librealsense::float2 pixel = { 0, 0 };
|
|
|
|
std::vector<uint16_t> depth(1280 * 720, 1000);
|
|
rscuda::deproject_depth_cuda((float*)point.data(), intrin, depth.data(), 1);
|
|
for (auto i = 0; i < 720; i++)
|
|
{
|
|
for (auto j = 0; j < 1280; j++)
|
|
{
|
|
CAPTURE(i, j);
|
|
rs2_project_point_to_pixel((float*)&pixel, &intrin, (float*)&point[i * 1280 + j]);
|
|
compare({ (float)j,(float)i }, pixel);
|
|
}
|
|
}
|
|
}
|
|
#endif |