Skip to content

Commit

Permalink
DSO-13626
Browse files Browse the repository at this point in the history
Sensor refactoring

- Add Synthetic Sensor class.
- Replace unpackers with processing blocks.
- Add composite processing blocks.
  • Loading branch information
alowenst01 committed Oct 23, 2019
1 parent 7c6a84d commit 7fc0f74
Show file tree
Hide file tree
Showing 75 changed files with 4,119 additions and 2,491 deletions.
15 changes: 15 additions & 0 deletions include/librealsense2/h/rs_frame.h
Original file line number Diff line number Diff line change
Expand Up @@ -255,6 +255,21 @@ int rs2_is_frame_extendable_to(const rs2_frame* frame, rs2_extension extension_t
rs2_frame* rs2_allocate_synthetic_video_frame(rs2_source* source, const rs2_stream_profile* new_stream, rs2_frame* original,
int new_bpp, int new_width, int new_height, int new_stride, rs2_extension frame_type, rs2_error** error);

/**
* Allocate new motion frame using a frame-source provided form a processing block
* \param[in] source Frame pool to allocate the frame from
* \param[in] new_stream New stream profile to assign to newly created frame
* \param[in] original A reference frame that can be used to fill in auxilary information like format, width, height, bpp, stride (if applicable)
* \param[in] new_width New value for width for the allocated frame
* \param[in] new_height New value for height for the allocated frame
* \param[in] frame_type New value for frame type for the allocated frame
* \param[out] error If non-null, receives any error that occurs during this call, otherwise, errors are ignored
* \return reference to a newly allocated frame, must be released with release_frame
* memory for the frame is likely to be re-used from previous frame, but in lack of available frames in the pool will be allocated from the free store
*/
rs2_frame* rs2_allocate_synthetic_motion_frame(rs2_source* source, const rs2_stream_profile* new_stream, rs2_frame* original,
int new_width, int new_height, rs2_extension frame_type, rs2_error** error);

/**
* Allocate new points frame using a frame-source provided from a processing block
* \param[in] source Frame pool to allocate the frame from
Expand Down
5 changes: 5 additions & 0 deletions include/librealsense2/h/rs_sensor.h
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,11 @@ typedef enum rs2_format
RS2_FORMAT_Y10BPACK , /**< 16-bit per-pixel grayscale image unpacked from 10 bits per pixel packed ([8:8:8:8:2222]) grey-scale image. The data is unpacked to LSB and padded with 6 zero bits */
RS2_FORMAT_DISTANCE , /**< 32-bit float-point depth distance value. */
RS2_FORMAT_MJPEG , /**< Bitstream encoding for video in which an image of each frame is encoded as JPEG-DIB */
RS2_FORMAT_Y8I , /**< 8-bit per pixel interleaved. 8-bit left, 8-bit right. */
RS2_FORMAT_Y12I , /**< 12-bit per pixel interleaved. 12-bit left, 12-bit right. Each pixel is stored in a 24-bit word in little-endian order. */
RS2_FORMAT_INZI , /**< multi-planar Depth 16bit + IR 10bit. */
RS2_FORMAT_INVI , /**< 8-bit IR stream. */
RS2_FORMAT_W10 , /**< Grey-scale image as a bit-packed array. 4 pixel data stream taking 5 bytes */
RS2_FORMAT_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */
} rs2_format;
const char* rs2_format_to_string(rs2_format format);
Expand Down
25 changes: 24 additions & 1 deletion include/librealsense2/hpp/rs_processing.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ namespace rs2
* \param[in] new_bpp Frame bit per pixel to create.
* \param[in] new_width Frame width to create.
* \param[in] new_height Frame height to create.
* \param[in] new_stride Frame stride to crate.
* \param[in] new_stride Frame stride to create.
* \param[in] frame_type Which frame type are going to create.
* \return The allocated frame
*/
Expand All @@ -45,6 +45,29 @@ namespace rs2
return result;
}

/**
* Allocate a new motion frame with given params
*
* \param[in] profile Stream profile going to allocate.
* \param[in] original Original frame.
* \param[in] new_width Frame width to create.
* \param[in] new_height Frame height to create.
* \param[in] frame_type Which frame type are going to create.
* \return The allocated frame
*/
frame allocate_motion_frame(const stream_profile& profile,
const frame& original,
int new_width = 0,
int new_height = 0,
rs2_extension frame_type = RS2_EXTENSION_MOTION_FRAME) const
{
rs2_error* e = nullptr;
auto result = rs2_allocate_synthetic_motion_frame(_source, profile.get(),
original.get(), new_width, new_height, frame_type, &e);
error::handle(e);
return result;
}

frame allocate_points(const stream_profile& profile,
const frame& original) const
{
Expand Down
10 changes: 5 additions & 5 deletions src/algo.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -63,8 +63,8 @@ auto_exposure_mechanism::auto_exposure_mechanism(option& gain_option, option& ex
if (!_keep_alive)
return;

frame_and_callback frame_callback;
auto frame_sts = _data_queue.dequeue(&frame_callback, RS2_DEFAULT_TIMEOUT);
frame_holder f_holder;
auto frame_sts = _data_queue.dequeue(&f_holder, RS2_DEFAULT_TIMEOUT);

lk.unlock();

Expand All @@ -75,7 +75,7 @@ auto_exposure_mechanism::auto_exposure_mechanism(option& gain_option, option& ex
}
try
{
auto frame = std::move(frame_callback.f_holder);
auto frame = std::move(f_holder);

double values[2] = {};

Expand Down Expand Up @@ -147,7 +147,7 @@ void auto_exposure_mechanism::update_auto_exposure_roi(const region_of_interest&
_auto_exposure_algo.update_roi(roi);
}

void auto_exposure_mechanism::add_frame(frame_holder frame, callback_invocation_holder callback)
void auto_exposure_mechanism::add_frame(frame_holder frame)
{

if (!_keep_alive || (_skip_frames && (_frames_counter++) != _skip_frames))
Expand All @@ -159,7 +159,7 @@ void auto_exposure_mechanism::add_frame(frame_holder frame, callback_invocation_

{
std::lock_guard<std::mutex> lk(_queue_mtx);
_data_queue.enqueue({std::move(frame), std::move(callback)});
_data_queue.enqueue(std::move(frame));
}
_cv.notify_one();
}
Expand Down
11 changes: 2 additions & 9 deletions src/algo.h
Original file line number Diff line number Diff line change
Expand Up @@ -102,16 +102,11 @@ namespace librealsense
std::recursive_mutex state_mutex;
};

struct frame_and_callback{
frame_holder f_holder;
callback_invocation_holder callback;
};

class auto_exposure_mechanism {
public:
auto_exposure_mechanism(option& gain_option, option& exposure_option, const auto_exposure_state& auto_exposure_state);
~auto_exposure_mechanism();
void add_frame(frame_holder frame, callback_invocation_holder callback);
void add_frame(frame_holder frame);
void update_auto_exposure_state(const auto_exposure_state& auto_exposure_state);
void update_auto_exposure_roi(const region_of_interest& roi);

Expand All @@ -129,16 +124,14 @@ namespace librealsense
};

private:
bool try_pop_front_data(frame_and_callback* data);

static const int queue_size = 2;
option& _gain_option;
option& _exposure_option;
auto_exposure_algorithm _auto_exposure_algo;
std::shared_ptr<std::thread> _exposure_thread;
std::condition_variable _cv;
std::atomic<bool> _keep_alive;
single_consumer_queue<frame_and_callback> _data_queue;
single_consumer_queue<frame_holder> _data_queue;
std::mutex _queue_mtx;
std::atomic<unsigned> _frames_counter;
std::atomic<unsigned> _skip_frames;
Expand Down
67 changes: 43 additions & 24 deletions src/context.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,15 @@ bool contains(const std::shared_ptr<librealsense::device_info>& first,

namespace librealsense
{
std::map<uint32_t, rs2_format> platform_color_fourcc_to_rs2_format = {
{rs_fourcc('Y','U','Y','2'), RS2_FORMAT_YUYV},
{rs_fourcc('U','Y','V','Y'), RS2_FORMAT_UYVY}
};
std::map<uint32_t, rs2_stream> platform_color_fourcc_to_rs2_stream = {
{rs_fourcc('Y','U','Y','2'), RS2_STREAM_COLOR},
{rs_fourcc('U','Y','V','Y'), RS2_STREAM_COLOR}
};

context::context(backend_type type,
const char* filename,
const char* section,
Expand Down Expand Up @@ -210,14 +219,12 @@ namespace librealsense
std::vector<platform::uvc_device_info> _uvcs;
};

class platform_camera_sensor : public uvc_sensor
class platform_camera_sensor : public synthetic_sensor
{
public:
platform_camera_sensor(const std::shared_ptr<context>&,
device* owner,
std::shared_ptr<platform::uvc_device> uvc_device,
std::unique_ptr<frame_timestamp_reader> timestamp_reader)
: uvc_sensor("RGB Camera", uvc_device, move(timestamp_reader), owner),
platform_camera_sensor(device* owner,
std::shared_ptr<uvc_sensor> uvc_sensor)
: synthetic_sensor("RGB Camera", uvc_sensor, owner),
_default_stream(new stream(RS2_STREAM_COLOR))
{
}
Expand All @@ -226,7 +233,7 @@ namespace librealsense
{
auto lock = environment::get_instance().get_extrinsics_graph().lock();

auto results = uvc_sensor::init_stream_profiles();
auto results = synthetic_sensor::init_stream_profiles();

for (auto p : results)
{
Expand Down Expand Up @@ -255,9 +262,11 @@ namespace librealsense
std::vector<std::shared_ptr<platform::uvc_device>> devs;
for (auto&& info : uvc_infos)
devs.push_back(ctx->get_backend().create_uvc_device(info));
auto color_ep = std::make_shared<platform_camera_sensor>(ctx, this,
std::make_shared<platform::multi_pins_uvc_device>(devs),
std::unique_ptr<ds5_timestamp_reader>(new ds5_timestamp_reader(environment::get_instance().get_time_service())));
auto raw_color_ep = std::make_shared<uvc_sensor>("Raw RGB Camera",
std::make_shared<platform::multi_pins_uvc_device>(devs),
std::unique_ptr<ds5_timestamp_reader>(new ds5_timestamp_reader(environment::get_instance().get_time_service())),
this);
auto color_ep = std::make_shared<platform_camera_sensor>(this, raw_color_ep);
add_sensor(color_ep);

register_info(RS2_CAMERA_INFO_NAME, "Platform Camera");
Expand All @@ -268,20 +277,30 @@ namespace librealsense
register_info(RS2_CAMERA_INFO_PHYSICAL_PORT, uvc_infos.front().device_path);
register_info(RS2_CAMERA_INFO_PRODUCT_ID, pid_str);

color_ep->register_pixel_format(pf_yuy2);
color_ep->register_pixel_format(pf_yuyv);

color_ep->try_register_pu(RS2_OPTION_BACKLIGHT_COMPENSATION);
color_ep->try_register_pu(RS2_OPTION_BRIGHTNESS);
color_ep->try_register_pu(RS2_OPTION_CONTRAST);
color_ep->try_register_pu(RS2_OPTION_EXPOSURE);
color_ep->try_register_pu(RS2_OPTION_GAMMA);
color_ep->try_register_pu(RS2_OPTION_HUE);
color_ep->try_register_pu(RS2_OPTION_SATURATION);
color_ep->try_register_pu(RS2_OPTION_SHARPNESS);
color_ep->try_register_pu(RS2_OPTION_WHITE_BALANCE);
color_ep->try_register_pu(RS2_OPTION_ENABLE_AUTO_EXPOSURE);
color_ep->try_register_pu(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE);
color_ep->register_processing_block({ {RS2_FORMAT_YUYV} }, { {RS2_FORMAT_RGB8, RS2_STREAM_COLOR} }, []() { return std::make_shared<yuy2_converter>(RS2_FORMAT_RGB8); });
color_ep->register_processing_block({ {RS2_FORMAT_YUYV} }, { {RS2_FORMAT_RGBA8, RS2_STREAM_COLOR} }, []() { return std::make_shared<yuy2_converter>(RS2_FORMAT_RGBA8); });
color_ep->register_processing_block({ {RS2_FORMAT_YUYV} }, { {RS2_FORMAT_BGR8, RS2_STREAM_COLOR} }, []() { return std::make_shared<yuy2_converter>(RS2_FORMAT_BGR8); });
color_ep->register_processing_block({ {RS2_FORMAT_YUYV} }, { {RS2_FORMAT_BGRA8, RS2_STREAM_COLOR} }, []() { return std::make_shared<yuy2_converter>(RS2_FORMAT_BGRA8); });
color_ep->register_processing_block({ {RS2_FORMAT_YUYV} }, { {RS2_FORMAT_Y16, RS2_STREAM_COLOR} }, []() { return std::make_shared<yuy2_converter>(RS2_FORMAT_Y16); });
color_ep->register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_YUYV, RS2_STREAM_COLOR));

color_ep->register_processing_block({ {RS2_FORMAT_UYVY} }, { {RS2_FORMAT_RGB8, RS2_STREAM_COLOR} }, []() { return std::make_shared<uyvy_converter>(RS2_FORMAT_RGB8); });
color_ep->register_processing_block({ {RS2_FORMAT_UYVY} }, { {RS2_FORMAT_RGBA8, RS2_STREAM_COLOR} }, []() { return std::make_shared<uyvy_converter>(RS2_FORMAT_RGBA8); });
color_ep->register_processing_block({ {RS2_FORMAT_UYVY} }, { {RS2_FORMAT_BGR8, RS2_STREAM_COLOR} }, []() { return std::make_shared<uyvy_converter>(RS2_FORMAT_BGR8); });
color_ep->register_processing_block({ {RS2_FORMAT_UYVY} }, { {RS2_FORMAT_BGRA8, RS2_STREAM_COLOR} }, []() { return std::make_shared<uyvy_converter>(RS2_FORMAT_BGRA8); });
color_ep->register_processing_block(processing_block_factory::create_id_pbf(RS2_FORMAT_UYVY, RS2_STREAM_COLOR));

raw_color_ep->try_register_pu(RS2_OPTION_BACKLIGHT_COMPENSATION);
raw_color_ep->try_register_pu(RS2_OPTION_BRIGHTNESS);
raw_color_ep->try_register_pu(RS2_OPTION_CONTRAST);
raw_color_ep->try_register_pu(RS2_OPTION_EXPOSURE);
raw_color_ep->try_register_pu(RS2_OPTION_GAMMA);
raw_color_ep->try_register_pu(RS2_OPTION_HUE);
raw_color_ep->try_register_pu(RS2_OPTION_SATURATION);
raw_color_ep->try_register_pu(RS2_OPTION_SHARPNESS);
raw_color_ep->try_register_pu(RS2_OPTION_WHITE_BALANCE);
raw_color_ep->try_register_pu(RS2_OPTION_ENABLE_AUTO_EXPOSURE);
raw_color_ep->try_register_pu(RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE);
}

virtual rs2_intrinsics get_intrinsics(unsigned int, const stream_profile&) const
Expand Down
18 changes: 9 additions & 9 deletions src/core/advanced_mode.h
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ namespace librealsense
{
public:
explicit ds5_advanced_mode_base(std::shared_ptr<hw_monitor> hwm,
uvc_sensor& depth_sensor);
synthetic_sensor& depth_sensor);

void create_snapshot(std::shared_ptr<ds5_advanced_mode_interface>& snapshot) const override {};
void enable_recording(std::function<void(const ds5_advanced_mode_interface&)> recording_function) override {};
Expand Down Expand Up @@ -156,10 +156,10 @@ namespace librealsense
static const uint16_t HW_MONITOR_BUFFER_SIZE = 1024;

private:
void set_exposure(uvc_sensor& sensor, const exposure_control& val);
void set_auto_exposure(uvc_sensor& sensor, const auto_exposure_control& val);
void get_exposure(uvc_sensor& sensor, exposure_control* ptr) const;
void get_auto_exposure(uvc_sensor& sensor, auto_exposure_control* ptr) const;
void set_exposure(synthetic_sensor& sensor, const exposure_control& val);
void set_auto_exposure(synthetic_sensor& sensor, const auto_exposure_control& val);
void get_exposure(synthetic_sensor& sensor, exposure_control* ptr) const;
void get_auto_exposure(synthetic_sensor& sensor, auto_exposure_control* ptr) const;

void get_laser_power(laser_power_control* ptr) const;
void get_laser_state(laser_state_control* ptr) const;
Expand Down Expand Up @@ -201,10 +201,10 @@ namespace librealsense
void set_color_auto_white_balance(const auto_white_balance_control& val);
void set_color_power_line_frequency(const power_line_frequency_control& val);

bool supports_option(const uvc_sensor& sensor, rs2_option opt) const;
bool supports_option(const synthetic_sensor& sensor, rs2_option opt) const;

std::shared_ptr<hw_monitor> _hw_monitor;
uvc_sensor& _depth_sensor;
synthetic_sensor& _depth_sensor;
lazy<ds5_color_sensor*> _color_sensor;
lazy<bool> _enabled;
std::shared_ptr<advanced_mode_preset_option> _preset_opt;
Expand Down Expand Up @@ -258,7 +258,7 @@ namespace librealsense
class advanced_mode_preset_option : public option_base
{
public:
advanced_mode_preset_option(ds5_advanced_mode_base& advanced, uvc_sensor& ep,
advanced_mode_preset_option(ds5_advanced_mode_base& advanced, synthetic_sensor& ep,
const option_range& opt_range);

static rs2_rs400_visual_preset to_preset(float x);
Expand All @@ -273,7 +273,7 @@ namespace librealsense
firmware_version get_firmware_version(const uvc_sensor& sensor) const;

std::mutex _mtx;
uvc_sensor& _ep;
synthetic_sensor& _ep;
ds5_advanced_mode_base& _advanced;
rs2_rs400_visual_preset _last_preset;
};
Expand Down
6 changes: 6 additions & 0 deletions src/core/processing.h
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,12 @@ namespace librealsense
int new_stride = 0,
rs2_extension frame_type = RS2_EXTENSION_VIDEO_FRAME) = 0;

virtual frame_interface* allocate_motion_frame(std::shared_ptr<stream_profile_interface> stream,
frame_interface* original,
int width = 0,
int height = 0,
rs2_extension frame_type = RS2_EXTENSION_MOTION_FRAME) = 0;

virtual frame_interface* allocate_composite_frame(std::vector<frame_holder> frames) = 0;

virtual frame_interface* allocate_points(std::shared_ptr<stream_profile_interface> stream,
Expand Down
9 changes: 8 additions & 1 deletion src/core/streaming.h
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,6 @@ namespace librealsense
virtual bool supports_frame_metadata(const rs2_frame_metadata_value& frame_metadata) const = 0;
virtual int get_frame_data_size() const = 0;
virtual const byte* get_frame_data() const = 0;
//TODO: add virtual uint64_t get_frame_data_size() const = 0;
virtual rs2_time_t get_frame_timestamp() const = 0;
virtual rs2_timestamp_domain get_frame_timestamp_domain() const = 0;
virtual void set_timestamp(double new_ts) = 0;
Expand Down Expand Up @@ -165,6 +164,14 @@ namespace librealsense
using stream_profiles = std::vector<std::shared_ptr<stream_profile_interface>>;
using processing_blocks = std::vector<std::shared_ptr<processing_block_interface>>;

inline std::ostream& operator << (std::ostream& os, const stream_profiles& profiles)
{
for (auto&& p : profiles)
{
os << rs2_format_to_string(p->get_format()) << " " << rs2_stream_to_string(p->get_stream_type()) << ", ";
}
return os;
}

class recommended_proccesing_blocks_interface
{
Expand Down
4 changes: 2 additions & 2 deletions src/device.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -179,9 +179,9 @@ int device::assign_sensor(std::shared_ptr<sensor_interface> sensor_base, uint8_t
}
}

uvc_sensor& device::get_uvc_sensor(int sub)
synthetic_sensor& device::get_uvc_sensor(int sub)
{
return dynamic_cast<uvc_sensor&>(*_sensors[sub]);
return dynamic_cast<synthetic_sensor&>(*_sensors[sub]);
}

size_t device::get_sensors_count() const
Expand Down
2 changes: 1 addition & 1 deletion src/device.h
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ namespace librealsense
int add_sensor(std::shared_ptr<sensor_interface> sensor_base);
int assign_sensor(std::shared_ptr<sensor_interface> sensor_base, uint8_t idx);
void register_stream_to_extrinsic_group(const stream_interface& stream, uint32_t groupd_index);
uvc_sensor& get_uvc_sensor(int subdevice);
synthetic_sensor& get_uvc_sensor(int subdevice);

explicit device(std::shared_ptr<context> ctx,
const platform::backend_device_group group,
Expand Down
Loading

0 comments on commit 7fc0f74

Please sign in to comment.