diff --git a/wrappers/matlab/align.m b/wrappers/matlab/align.m index 1becf84310..58ffbc25e7 100644 --- a/wrappers/matlab/align.m +++ b/wrappers/matlab/align.m @@ -1,5 +1,5 @@ % Wraps librealsense2 align class -classdef align < handle +classdef align < librealsense.processing_block properties (SetAccess = private, Hidden = true) objectHandle; end @@ -9,14 +9,10 @@ narginchk(1, 1); validateattributes(align_to, {'realsense.stream', 'numeric'}, {'scalar', 'nonnegative', 'real', 'integer', '<=', realsense.stream.count}); this.objectHandle = realsense.librealsense_mex('rs2::align', 'new', int64(align_to)); + this = this@realsense.processing_block(out); end - % Destructor - function delete(this) - if (this.objectHandle ~= 0) - realsense.librealsense_mex('rs2::align', 'delete', this.objectHandle); - end - end + % Destructor (uses base class destructor) % Functions function frames = process(this, frame) diff --git a/wrappers/matlab/colorizer.m b/wrappers/matlab/colorizer.m index bc4ed97fba..f5978741e0 100644 --- a/wrappers/matlab/colorizer.m +++ b/wrappers/matlab/colorizer.m @@ -1,10 +1,15 @@ % Wraps librealsense2 colorizer class -classdef colorizer < realsense.options +classdef colorizer < realsense.processing_block methods % Constructor - function this = colorizer() - out = realsense.librealsense_mex('rs2::colorizer', 'new'); - this = this@realsense.options(out); + function this = colorizer(color_scheme) + if (nargin == 0) + out = realsense.librealsense_mex('rs2::colorizer', 'new'); + else + validateattributes(color_scheme, {'numeric'}, {'scalar', 'nonnegative', 'real'}); + out = realsense.librealsense_mex('rs2::colorizer', 'new', double(color_scheme)); + end + this = this@realsense.processing_block(out); end % Destructor (uses base class destructor) diff --git a/wrappers/matlab/decimation_filter.m b/wrappers/matlab/decimation_filter.m index f48264c985..1fec8cfbe7 100644 --- a/wrappers/matlab/decimation_filter.m +++ b/wrappers/matlab/decimation_filter.m @@ -1,10 +1,15 @@ % Wraps librealsense2 decimation_filter class -classdef decimation_filter < realsense.process_interface +classdef decimation_filter < realsense.processing_block methods % Constructor - function this = decimation_filter() - out = realsense.librealsense_mex('rs2::decimation_filter', 'new'); - this = this@realsense.process_interface(out); + function this = decimation_filter(magnitude) + if (nargin == 0) + out = realsense.librealsense_mex('rs2::decimation_filter', 'new'); + else + validateattributes(magnitude, {'numeric'}, {'scalar', 'nonnegative', 'real'}); + out = realsense.librealsense_mex('rs2::decimation_filter', 'new', double(magnitude)); + end + this = this@realsense.processing_block(out); end % Destructor (uses base class destructor) diff --git a/wrappers/matlab/disparity_transform.m b/wrappers/matlab/disparity_transform.m index 2ba613e50e..7a8061102a 100644 --- a/wrappers/matlab/disparity_transform.m +++ b/wrappers/matlab/disparity_transform.m @@ -1,5 +1,5 @@ % Wraps librealsense2 disparity_transform class -classdef disparity_transform < realsense.process_interface +classdef disparity_transform < realsense.processing_block methods % Constructor function this = disparity_transform(transform_to_disparity) @@ -9,7 +9,7 @@ validateattributes(transform_to_disparity, {'logical', 'numeric'}, {'scalar', 'real'}); out = realsense.librealsense_mex('rs2::disparity_transform', 'new', logical(transform_to_disparity)); end - this = this@realsense.process_interface(out); + this = this@realsense.processing_block(out); end % Destructor (uses base class destructor) diff --git a/wrappers/matlab/hole_filling_filter.m b/wrappers/matlab/hole_filling_filter.m index a888994a74..438a4af5f5 100644 --- a/wrappers/matlab/hole_filling_filter.m +++ b/wrappers/matlab/hole_filling_filter.m @@ -1,10 +1,15 @@ % Wraps librealsense2 hole_filling_filter class -classdef hole_filling_filter < realsense.process_interface +classdef hole_filling_filter < realsense.processing_block methods % Constructor - function this = hole_filling_filter() - out = realsense.librealsense_mex('rs2::hole_filling_filter', 'new'); - this = this@realsense.process_interface(out); + function this = hole_filling_filter(mode) + if (nargin == 0) + out = realsense.librealsense_mex('rs2::hole_filling_filter', 'new'); + else + validateattributes(mode, {'numeric'}, {'scalar', 'real', 'integer'}); + out = realsense.librealsense_mex('rs2::hole_filling_filter', 'new', int64(mode)); + end + this = this@realsense.processing_block(out); end % Destructor (uses base class destructor) diff --git a/wrappers/matlab/librealsense_mex.cpp b/wrappers/matlab/librealsense_mex.cpp index c435202654..8ac2b33557 100644 --- a/wrappers/matlab/librealsense_mex.cpp +++ b/wrappers/matlab/librealsense_mex.cpp @@ -919,11 +919,32 @@ void make_factory(){ }); factory->record(frame_queue_factory); } + { + ClassFactory processing_block_factory("rs2::processing_block"); + processing_block_factory.record("process", 1, 2, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) + { + auto thiz = MatlabParamParser::parse(inv[0]); + auto frame = MatlabParamParser::parse(inv[1]); + outv[0] = MatlabParamParser::wrap(thiz.process(frame)); + }); + // TODO: expose more functions? + factory->record(processing_block_factory); + } { ClassFactory pointcloud_factory("rs2::pointcloud"); - pointcloud_factory.record("new", 1, 0, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) + pointcloud_factory.record("new", 1, 0, 2, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) { - outv[0] = MatlabParamParser::wrap(rs2::pointcloud()); + if (inc == 0) { + outv[0] = MatlabParamParser::wrap(rs2::pointcloud()); + return; + } + auto stream = MatlabParamParser::parse(inv[0]); + if (inc == 1) + outv[0] = MatlabParamParser::wrap(rs2::pointcloud(stream)); + else { + auto index = MatlabParamParser::parse(inv[1]); + outv[0] = MatlabParamParser::wrap(rs2::pointcloud(stream, index)); + } }); pointcloud_factory.record("calculate", 1, 2, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) { @@ -976,10 +997,7 @@ void make_factory(){ auto align_to = MatlabParamParser::parse(inv[0]); outv[0] = MatlabParamParser::wrap(rs2::align(align_to)); }); - align_factory.record("delete", 0, 1, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) - { - MatlabParamParser::destroy(inv[0]); - }); + // TODO: how does this interact with the processing_block variant? Why are the separate? align_factory.record("process", 1, 2, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) { auto thiz = MatlabParamParser::parse(inv[0]); @@ -990,9 +1008,13 @@ void make_factory(){ } { ClassFactory colorizer_factory("rs2::colorizer"); - colorizer_factory.record("new", 1, 0, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) + colorizer_factory.record("new", 1, 0, 1, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) { - outv[0] = MatlabParamParser::wrap(rs2::colorizer()); + if (inc == 0) outv[0] = MatlabParamParser::wrap(rs2::colorizer()); + else { + auto color_scheme = MatlabParamParser::parse(inv[0]); + outv[0] = MatlabParamParser::wrap(rs2::colorizer(color_scheme)); + } }); colorizer_factory.record("colorize", 1, 2, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) { @@ -1002,38 +1024,46 @@ void make_factory(){ }); factory->record(colorizer_factory); } - { - ClassFactory process_interface_factory("rs2::process_interface"); - process_interface_factory.record("process", 1, 2, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) - { - // TODO: will this work? - auto *thiz = MatlabParamParser::parse(inv[0]); - auto frame = MatlabParamParser::parse(inv[1]); - outv[0] = MatlabParamParser::wrap(thiz->process(frame)); - }); - factory->record(process_interface_factory); - } { ClassFactory decimation_filter_factory("rs2::decimation_filter"); - decimation_filter_factory.record("new", 1, 0, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) + decimation_filter_factory.record("new", 1, 0, 1, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) { - outv[0] = MatlabParamParser::wrap(rs2::decimation_filter()); + if (inc == 0) outv[0] = MatlabParamParser::wrap(rs2::decimation_filter()); + else { + auto magnitude = MatlabParamParser::parse(inv[0]); + outv[0] = MatlabParamParser::wrap(rs2::decimation_filter(magnitude)); + } }); factory->record(decimation_filter_factory); } { ClassFactory temporal_filter_factory("rs2::temporal_filter"); - temporal_filter_factory.record("new", 1, 0, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) + temporal_filter_factory.record("new", 1, 0, 3, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) { - outv[0] = MatlabParamParser::wrap(rs2::temporal_filter()); + if (inc == 0) outv[0] = MatlabParamParser::wrap(rs2::temporal_filter()); + else if (inc == 3) { + auto smooth_alpha = MatlabParamParser::parse(inv[0]); + auto smooth_delta = MatlabParamParser::parse(inv[1]); + auto persistence_control = MatlabParamParser::parse(inv[2]); + outv[0] = MatlabParamParser::wrap(rs2::temporal_filter(smooth_alpha, smooth_delta, persistence_control)); + } + else mexErrMsgTxt("rs2::temporal_filter::new: Wrong number of Inputs"); }); factory->record(temporal_filter_factory); } { ClassFactory spatial_filter_factory("rs2::spatial_filter"); - spatial_filter_factory.record("new", 1, 0, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) - { - outv[0] = MatlabParamParser::wrap(rs2::spatial_filter()); + spatial_filter_factory.record("new", 1, 0, 4, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) + { + if (inc == 0) outv[0] = MatlabParamParser::wrap(rs2::spatial_filter()); + else if (inc == 4) { + auto smooth_alpha = MatlabParamParser::parse(inv[0]); + auto smooth_delta = MatlabParamParser::parse(inv[1]); + auto magnitude = MatlabParamParser::parse(inv[2]); + auto hole_fill = MatlabParamParser::parse(inv[3]); + outv[0] = MatlabParamParser::wrap(rs2::spatial_filter(smooth_alpha, smooth_delta, magnitude, hole_fill)); + } + else mexErrMsgTxt("rs2::spatial_filter::new: Wrong number of Inputs"); }); factory->record(spatial_filter_factory); } @@ -1053,9 +1083,13 @@ void make_factory(){ } { ClassFactory hole_filling_filter_factory("rs2::hole_filling_filter"); - hole_filling_filter_factory.record("new", 1, 0, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) + hole_filling_filter_factory.record("new", 1, 0, 1, [](int outc, mxArray* outv[], int inc, const mxArray* inv[]) { - outv[0] = MatlabParamParser::wrap(rs2::hole_filling_filter()); + if (inc == 0) outv[0] = MatlabParamParser::wrap(rs2::hole_filling_filter()); + else { + auto mode = MatlabParamParser::parse(inv[0]); + outv[0] = MatlabParamParser::wrap(rs2::hole_filling_filter(mode)); + } }); factory->record(hole_filling_filter_factory); } diff --git a/wrappers/matlab/librealsense_mex.vcxproj b/wrappers/matlab/librealsense_mex.vcxproj index 3837d72fec..92093976b9 100644 --- a/wrappers/matlab/librealsense_mex.vcxproj +++ b/wrappers/matlab/librealsense_mex.vcxproj @@ -203,7 +203,7 @@ - + diff --git a/wrappers/matlab/librealsense_mex.vcxproj.filters b/wrappers/matlab/librealsense_mex.vcxproj.filters index 0ec340fc33..a877e2f48c 100644 --- a/wrappers/matlab/librealsense_mex.vcxproj.filters +++ b/wrappers/matlab/librealsense_mex.vcxproj.filters @@ -148,9 +148,6 @@ Matlab Files\Classes - - Matlab Files\Classes - Matlab Files\Classes @@ -181,5 +178,8 @@ Matlab Files\Examples + + Matlab Files\Classes + \ No newline at end of file diff --git a/wrappers/matlab/pointcloud.m b/wrappers/matlab/pointcloud.m index 508d26649e..667ec84570 100644 --- a/wrappers/matlab/pointcloud.m +++ b/wrappers/matlab/pointcloud.m @@ -1,10 +1,20 @@ % Wraps librealsense2 pointcloud class -classdef pointcloud < realsense.options +classdef pointcloud < realsense.processing_block methods % Constructor - function this = pointcloud() - out = realsense.librealsense_mex('rs2::pointcloud', 'new'); - this = this@realsense.options(out); + function this = pointcloud(stream, index) + switch nargin + case 0 + out = realsense.librealsense_mex('rs2::pointcloud', 'new'); + case 1 + validateattributes(stream, {'realsense.stream', 'numeric'}, {'scalar', 'nonnegative', 'real', 'integer', '<=', realsense.stream.count}); + out = realsense.librealsense_mex('rs2::pointcloud', 'new', int64(stream)); + case 2 + validateattributes(stream, {'realsense.stream', 'numeric'}, {'scalar', 'nonnegative', 'real', 'integer', '<=', realsense.stream.count}); + validateattributes(index, {'numeric'}, {'scalar', 'nonnegative', 'real', 'integer'}); + out = realsense.librealsense_mex('rs2::pointcloud', 'new', int64(stream), int64(index)); + end + this = this@realsense.processing_block(out); end % Destructor (uses base class destructor) diff --git a/wrappers/matlab/process_interface.m b/wrappers/matlab/process_interface.m deleted file mode 100644 index 0ba9e589b2..0000000000 --- a/wrappers/matlab/process_interface.m +++ /dev/null @@ -1,19 +0,0 @@ -% Wraps librealsense2 process_interface class -classdef process_interface < realsense.options - methods - % Constructor - function this = process_interface(handle) - this = this@realsense.options(handle); - end - - % Destructor (uses base class destructor) - - % Functions - function value = process(this, frame) - narginchk(2, 2) - validateattributes(frame, {'realsense.frame'}, {'scalar'}, '', 'frame', 2); - out = realsense.librealsense_mex('rs2::process_interface', 'process', this.objectHandle, frame.objectHandle); - value = realsense.frame(out); - end - end -end \ No newline at end of file diff --git a/wrappers/matlab/processing_block.m b/wrappers/matlab/processing_block.m new file mode 100644 index 0000000000..3695e8ba93 --- /dev/null +++ b/wrappers/matlab/processing_block.m @@ -0,0 +1,19 @@ +% Wraps librealsense2 processing_block class +classdef processing_block < realsense.options + methods + % Constructor + function this = processing_block(handle) + this = this@realsense.options(handle); + end + + % Destructor (uses base class destructor) + + % Functions + function out_frame = process(this, frame) + narginchk(2, 2) + validateattributes(frame, {'realsense.frame'}, {'scalar'}, '', 'frame', 2); + out = realsense.librealsense_mex('rs2::processing_block', 'process', this.objectHandle, frame.objectHandle); + out_frame = realsense.frame(out); + end + end +end \ No newline at end of file diff --git a/wrappers/matlab/rs2_type_traits.h b/wrappers/matlab/rs2_type_traits.h index b1ebe0a7d8..38a291cc6c 100644 --- a/wrappers/matlab/rs2_type_traits.h +++ b/wrappers/matlab/rs2_type_traits.h @@ -23,7 +23,7 @@ template struct MatlabParamParser::type_traits struct MatlabParamParser::type_traits { struct carrier { void * ptr; - enum class types { rs2_sensor, rs2_process_interface, rs2_colorizer, rs2_pointcloud } type; + enum class types { rs2_sensor, rs2_processing_block } type; carrier(void *ptr_, types t) : ptr(ptr_), type(t) {} ~carrier(); // implemented at the bottom with an explanation as to why }; @@ -62,34 +62,18 @@ template struct over_wrapper { static T from_internal(rs2_internal_t * ptr) { return T(**ptr); } static rs2_internal_t* to_internal(T&& val) { mexLock(); return new rs2_internal_t(new T(val)); } }; -template ::carrier::types E> struct options_over_wrapper +template struct MatlabParamParser::type_traits::value>::type> : MatlabParamParser::type_traits { - using carrier_t = std::shared_ptr; - using carrier_enum = std::integral_constant; + using carrier_t = std::shared_ptr; + using carrier_enum = std::integral_constant; static T from_internal(rs2_internal_t * ptr) { - if (ptr->type == carrier_enum::value) return T(**static_cast(ptr->ptr)); - mexErrMsgTxt("Error parsing argument, wrong branch of rs2::options inheritance"); + if (ptr->type == carrier_enum::value) return *std::dynamic_pointer_cast(*static_cast(ptr->ptr)); + mexErrMsgTxt("Error parsing argument, object is not a processing block"); } static rs2_internal_t* to_internal(T&& var) { mexLock(); return new rs2_internal_t(new carrier_t(new T(var)), carrier_enum::value); } }; -template<> struct MatlabParamParser::type_traits : over_wrapper {}; -template<> struct MatlabParamParser::type_traits - : options_over_wrapper::carrier::types::rs2_colorizer> {}; template<> struct MatlabParamParser::type_traits : over_wrapper {}; template<> struct MatlabParamParser::type_traits : over_wrapper {}; -template<> struct MatlabParamParser::type_traits - : options_over_wrapper::carrier::types::rs2_pointcloud> {}; - - -template<> struct MatlabParamParser::type_traits : type_traits { - using carrier_t = std::shared_ptr; - using carrier_enum = std::integral_constant; -}; -template struct MatlabParamParser::type_traits::value>::type> - : MatlabParamParser::type_traits { - static T from_internal(rs2_internal_t * ptr) { mexErrMsgTxt("from_internal(): This shouldn't happen"); } - static rs2_internal_t* to_internal(T&& var) { mexLock(); return new rs2_internal_t(new carrier_t(new T(var)), carrier_enum::value); } -}; // rs_context.hpp // rs2::event_information [?] @@ -105,9 +89,7 @@ template<> struct MatlabParamParser::type_traits { using MatlabParamParser::type_traits::carrier::~carrier() { switch (type) { case types::rs2_sensor: delete reinterpret_cast::carrier_t*>(ptr); break; - case types::rs2_process_interface: delete reinterpret_cast::carrier_t*>(ptr); break; - case types::rs2_colorizer: delete reinterpret_cast::carrier_t*>(ptr); break; - case types::rs2_pointcloud: delete reinterpret_cast::carrier_t*>(ptr); break; + case types::rs2_processing_block: delete reinterpret_cast::carrier_t*>(ptr); break; } } @@ -115,9 +97,7 @@ rs2::options MatlabParamParser::type_traits::from_internal(rs2_int switch (ptr->type) { case carrier::types::rs2_sensor: return traits_trampoline::from_internal(ptr).as(); // TODO: Fix - //case carrier::types::rs2_process_interface: return *std::shared_ptr(*static_cast::carrier_t*>(ptr->ptr)); - case carrier::types::rs2_colorizer: return *std::shared_ptr(*static_cast::carrier_t*>(ptr->ptr)); - case carrier::types::rs2_pointcloud: return *std::shared_ptr(*static_cast::carrier_t*>(ptr->ptr)); + case carrier::types::rs2_processing_block: return *std::shared_ptr(*static_cast::carrier_t*>(ptr->ptr)); default: mexErrMsgTxt("Error parsing argument of type rs2::options: unrecognized carrier type"); } diff --git a/wrappers/matlab/spatial_filter.m b/wrappers/matlab/spatial_filter.m index 3149089312..b12f9cef7b 100644 --- a/wrappers/matlab/spatial_filter.m +++ b/wrappers/matlab/spatial_filter.m @@ -1,10 +1,20 @@ % Wraps librealsense2 spatial_filter class -classdef spatial_filter < realsense.process_interface +classdef spatial_filter < realsense.processing_block methods % Constructor - function this = spatial_filter() - out = realsense.librealsense_mex('rs2::spatial_filter', 'new'); - this = this@realsense.process_interface(out); + function this = spatial_filter(smooth_alpha, smooth_delta, magnitude, hole_fill) + if (nargin == 0) + out = realsense.librealsense_mex('rs2::spatial_filter', 'new'); + else if (nargin == 4) + validateattributes(smooth_alpha, {'numeric'}, {'scalar', 'real'}); + validateattributes(smooth_delta, {'numeric'}, {'scalar', 'real'}); + validateattributes(magnitude, {'numeric'}, {'scalar', 'real'}); + validateattributes(hole_fill, {'numeric'}, {'scalar', 'real'}); + out = realsense.librealsense_mex('rs2::spatial_filter', 'new', double(smooth_alpha), double(smooth_delta), double(magnitude), double(hole_fill)); + else + % TODO: Error out on bad arg count + end + this = this@realsense.processing_block(out); end % Destructor (uses base class destructor) diff --git a/wrappers/matlab/temporal_filter.m b/wrappers/matlab/temporal_filter.m index cd53c35364..ca72041b24 100644 --- a/wrappers/matlab/temporal_filter.m +++ b/wrappers/matlab/temporal_filter.m @@ -1,10 +1,19 @@ % Wraps librealsense2 temporal_filter class -classdef temporal_filter < realsense.process_interface +classdef temporal_filter < realsense.processing_block methods % Constructor - function this = temporal_filter() - out = realsense.librealsense_mex('rs2::temporal_filter', 'new'); - this = this@realsense.process_interface(out); + function this = temporal_filter(smooth_alpha, smooth_delta, persistence_control) + if (nargin == 0) + out = realsense.librealsense_mex('rs2::temporal_filter', 'new'); + else if (nargin == 3) + validateattributes(smooth_alpha, {'numeric'}, {'scalar', 'real'}); + validateattributes(smooth_delta, {'numeric'}, {'scalar', 'real'}); + validateattributes(persistence_control, {'numeric'}, {'scalar', 'real', 'integer'}); + out = realsense.librealsense_mex('rs2::temporal_filter', 'new', double(smooth_alpha), double(smooth_delta), int64(persistence_control)); + else + % TODO: Error out on bad arg count + end + this = this@realsense.processing_block(out); end % Destructor (uses base class destructor) diff --git a/wrappers/matlab/types.h b/wrappers/matlab/types.h index 2bd2ec28b6..94e3df9355 100644 --- a/wrappers/matlab/types.h +++ b/wrappers/matlab/types.h @@ -235,12 +235,12 @@ template <> mxArray* MatlabParamParser::wrap_array(cons } // rs_processing.hpp -template<> rs2::process_interface* MatlabParamParser::mx_wrapper_fns::parse(const mxArray* cell) -{ - using traits_t = type_traits; - auto ptr = static_cast(mxGetData(cell)); - if (ptr->type == traits_t::carrier_enum::value) return reinterpret_cast(ptr->ptr)->get(); - mexErrMsgTxt("Error parsing argument, object is not a process_interface"); -} +//template<> rs2::process_interface* MatlabParamParser::mx_wrapper_fns::parse(const mxArray* cell) +//{ +// using traits_t = type_traits; +// auto ptr = static_cast(mxGetData(cell)); +// if (ptr->type == traits_t::carrier_enum::value) return reinterpret_cast(ptr->ptr)->get(); +// mexErrMsgTxt("Error parsing argument, object is not a process_interface"); +//} // rs_pipeline.hpp