Skip to content

Commit

Permalink
Merge PR IntelRealSense#20 from Nir-Az/fix_depth_stream_on_QVGA_USB3
Browse files Browse the repository at this point in the history 
L515 - support dynamic size of depth intrinsic tables
  • Loading branch information
@maloel
maloel authored Jul 7, 2020
2 parents 10c6382 + 6de7dd8 commit 5d40288
Show file tree
Hide file tree
Showing 5 changed files with 79 additions and 39 deletions.
34 changes: 19 additions & 15 deletions src/l500/l500-color.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -184,33 +184,37 @@ namespace librealsense

auto num_of_res = intrinsic->resolution.num_of_resolutions;

for( auto i = 0; i < num_of_res; i++ )
if (num_of_res <= MAX_NUM_OF_RGB_RESOLUTIONS)
{
auto model = intrinsic->resolution.intrinsic_resolution[i];
if( model.height == profile.height && model.width == profile.width )
for (auto i = 0; i < num_of_res; i++)
{
rs2_intrinsics intrinsics;
intrinsics.width = model.width;
intrinsics.height = model.height;
intrinsics.fx = model.ipm.focal_length.x;
intrinsics.fy = model.ipm.focal_length.y;
intrinsics.ppx = model.ipm.principal_point.x;
intrinsics.ppy = model.ipm.principal_point.y;

if( model.distort.radial_k1 || model.distort.radial_k2 || model.distort.tangential_p1 || model.distort.tangential_p2 || model.distort.radial_k3 )
auto model = intrinsic->resolution.intrinsic_resolution[i];
if (model.height == profile.height && model.width == profile.width)
{
rs2_intrinsics intrinsics;
intrinsics.width = model.width;
intrinsics.height = model.height;
intrinsics.fx = model.ipm.focal_length.x;
intrinsics.fy = model.ipm.focal_length.y;
intrinsics.ppx = model.ipm.principal_point.x;
intrinsics.ppy = model.ipm.principal_point.y;

intrinsics.coeffs[0] = model.distort.radial_k1;
intrinsics.coeffs[1] = model.distort.radial_k2;
intrinsics.coeffs[2] = model.distort.tangential_p1;
intrinsics.coeffs[3] = model.distort.tangential_p2;
intrinsics.coeffs[4] = model.distort.radial_k3;

intrinsics.model = RS2_DISTORTION_INVERSE_BROWN_CONRADY;
}
intrinsics.model = RS2_DISTORTION_INVERSE_BROWN_CONRADY; // TODO - according to algo, should be changed to RS2_DISTORTION_BROWN_CONRADY

return intrinsics;
return intrinsics;
}
}
}
else
{
throw std::runtime_error(to_string() << "Firmware intrinsics tables count(" << num_of_res << "), is higher than maximum supported(" << MAX_NUM_OF_DEPTH_RESOLUTIONS << ")");
}
throw std::runtime_error( to_string() << "intrinsics for resolution " << profile.width << "," << profile.height << " don't exist" );
}

Expand Down
25 changes: 25 additions & 0 deletions src/l500/l500-depth.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -191,6 +191,31 @@ namespace librealsense
//register_option(static_cast<rs2_option>(RS2_OPTION_DEPTH_INVALIDATION_ENABLE), _depth_invalidation_option);
}

ivcam2::intrinsic_params l500_depth_sensor::get_intrinsic_params(const uint32_t width, const uint32_t height, ivcam2::intrinsic_depth intrinsic)
{
auto num_of_res = intrinsic.resolution.num_of_resolutions;

if (num_of_res <= MAX_NUM_OF_DEPTH_RESOLUTIONS)
{
for (auto i = 0; i < num_of_res; i++)
{
auto model_world = intrinsic.resolution.intrinsic_resolution[i].world;
auto model_raw = intrinsic.resolution.intrinsic_resolution[i].raw;

if (model_world.pinhole_cam_model.height == height && model_world.pinhole_cam_model.width == width)
return model_world;
else if (model_raw.pinhole_cam_model.height == height && model_raw.pinhole_cam_model.width == width)
return model_raw;
}
// Should not get here
throw std::runtime_error(to_string() << "intrinsics for resolution " << width << "," << height << " doesn't exist");
}
else
{
throw std::runtime_error(to_string() << "Firmware intrinsic tables count(" << num_of_res << "), is higher than maximum supported(" << MAX_NUM_OF_DEPTH_RESOLUTIONS << ")");

}
}
int l500_depth_sensor::read_algo_version()
{
const int algo_version_address = 0xa0020bd8;
Expand Down
17 changes: 1 addition & 16 deletions src/l500/l500-depth.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,22 +126,7 @@ namespace librealsense
return options_container::get_option_name(option);
}

static ivcam2::intrinsic_params get_intrinsic_params(const uint32_t width, const uint32_t height, ivcam2::intrinsic_depth intrinsic)
{
auto num_of_res = intrinsic.resolution.num_of_resolutions;

for (auto i = 0; i < num_of_res; i++)
{
auto model_world = intrinsic.resolution.intrinsic_resolution[i].world;
auto model_raw = intrinsic.resolution.intrinsic_resolution[i].raw;

if (model_world.pinhole_cam_model.height == height && model_world.pinhole_cam_model.width == width)
return model_world;
else if (model_raw.pinhole_cam_model.height == height && model_raw.pinhole_cam_model.width == width)
return model_raw;
}
throw std::runtime_error(to_string() << "intrinsics for resolution " << width << "," << height << " doesn't exist");
}
static ivcam2::intrinsic_params get_intrinsic_params(const uint32_t width, const uint32_t height, ivcam2::intrinsic_depth intrinsic);

rs2_intrinsics get_intrinsics(const stream_profile& profile) const override
{
Expand Down
40 changes: 33 additions & 7 deletions src/l500/l500-private.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@
#include "core/extension.h"
#include "fw-update/fw-update-unsigned.h"

static const int NUM_OF_RGB_RESOLUTIONS = 5;
static const int NUM_OF_DEPTH_RESOLUTIONS = 2;
static const int MAX_NUM_OF_RGB_RESOLUTIONS = 5;
static const int MAX_NUM_OF_DEPTH_RESOLUTIONS = 5;

namespace librealsense
{
Expand Down Expand Up @@ -266,10 +266,10 @@ namespace librealsense
using namespace std;

auto table = reinterpret_cast<const T*>(raw_data.data());

if (raw_data.size() < sizeof(T))
auto expected_size = table->get_expected_size();
if (raw_data.size() < expected_size)
{
throw invalid_value_exception(to_string() << "Calibration data invald, buffer too small : expected " << sizeof(T) << " , actual: " << raw_data.size());
throw invalid_value_exception(to_string() << "Calibration data invalid, buffer too small : expected " << expected_size << " , actual: " << raw_data.size());
}

return table;
Expand Down Expand Up @@ -317,7 +317,7 @@ namespace librealsense
uint16_t reserved16;
uint8_t reserved8;
uint8_t num_of_resolutions;
intrinsic_per_resolution intrinsic_resolution[NUM_OF_DEPTH_RESOLUTIONS]; //Dynamic number of entries according to num of resolutions
intrinsic_per_resolution intrinsic_resolution[MAX_NUM_OF_DEPTH_RESOLUTIONS]; //Dynamic number of entries according to num of resolutions
};

struct orientation
Expand All @@ -333,14 +333,26 @@ namespace librealsense
{
orientation orient;
resolutions_depth resolution;

// This function use the value inside the struct "num_of_resolutions" to calculate the expected size of the intrinsics table buffer recieved from the firmware.
// Note: call this function only with real values inside the struct.
size_t get_expected_size() const
{
size_t expected_size(0);

// Get full maximum size of the resolution array and deduct the unused resolutions size from it.
expected_size += sizeof(*this);
expected_size -= (MAX_NUM_OF_DEPTH_RESOLUTIONS - this->resolution.num_of_resolutions) * sizeof(intrinsic_per_resolution);
return expected_size;
}
};

struct resolutions_rgb
{
uint16_t reserved16;
uint8_t reserved8;
uint8_t num_of_resolutions;
pinhole_camera_model intrinsic_resolution[NUM_OF_RGB_RESOLUTIONS]; //Dynamic number of entries according to num of resolutions
pinhole_camera_model intrinsic_resolution[MAX_NUM_OF_RGB_RESOLUTIONS]; //Dynamic number of entries according to num of resolutions
};

struct rgb_common
Expand All @@ -353,6 +365,20 @@ namespace librealsense
{
rgb_common common;
resolutions_rgb resolution;

// This function use the value inside the struct "num_of_resolutions"
// to calculate the struct utilize size got from the firmware raw intrinsics table data.
// Note: call this function only with real values inside the struct.
size_t get_expected_size() const
{
size_t expected_size(0);

// Get full maximum size of the resolution array and deduct the unused resolutions size from it.
expected_size += sizeof(*this);
expected_size -= (MAX_NUM_OF_RGB_RESOLUTIONS - this->resolution.num_of_resolutions) * sizeof(intrinsic_per_resolution);
return expected_size;
}

};

struct temperatures {
Expand Down
2 changes: 1 addition & 1 deletion src/media/ros/ros_reader.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,7 @@ namespace librealsense
struct l500_depth_data
{
float num_of_resolution;
l500_data_per_resolution data[NUM_OF_DEPTH_RESOLUTIONS];
l500_data_per_resolution data[MAX_NUM_OF_DEPTH_RESOLUTIONS];
float baseline;
};

Expand Down

0 comments on commit 5d40288

Please sign in to comment.