Skip to content

Commit b8fc6d4

Browse files
dorodnicdorodnic
authored
Merge pull request #2371 from matkatz/fix_pointcloud_map_to
fix pointcloud map_to
2 parents 6825841 + 613beae commit b8fc6d4

9 files changed

Lines changed: 92 additions & 57 deletions

‎src/proc/colorizer.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -126,8 +126,8 @@ namespace librealsense
126126
colorizer::colorizer()
127127
: _min(0.f), _max(6.f), _equalize(true), _stream()
128128
{
129-
_stream_filter = RS2_STREAM_DEPTH;
130-
_stream_format_filter = RS2_FORMAT_Z16;
129+
_stream_filter.stream = RS2_STREAM_DEPTH;
130+
_stream_filter.format = RS2_FORMAT_Z16;
131131

132132
_maps = { &jet, &classic, &grayscale, &inv_grayscale, &biomes, &cold, &warm, &quantized, &pattern };
133133

‎src/proc/decimation-filter.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -209,8 +209,8 @@ namespace librealsense
209209
_recalc_profile(false),
210210
_options_changed(false)
211211
{
212-
_stream_filter = RS2_STREAM_DEPTH;
213-
_stream_format_filter = RS2_FORMAT_Z16;
212+
_stream_filter.stream = RS2_STREAM_DEPTH;
213+
_stream_filter.format = RS2_FORMAT_Z16;
214214

215215
auto decimation_control = std::make_shared<ptr_option<uint8_t>>(
216216
decimation_min_val,

‎src/proc/hole-filling-filter.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -25,8 +25,8 @@ namespace librealsense
2525
_current_frm_size_pixels(0),
2626
_hole_filling_mode(hole_fill_def)
2727
{
28-
_stream_filter = RS2_STREAM_DEPTH;
29-
_stream_format_filter = RS2_FORMAT_Z16;
28+
_stream_filter.stream = RS2_STREAM_DEPTH;
29+
_stream_filter.format = RS2_FORMAT_Z16;
3030

3131
auto hole_filling_mode = std::make_shared<ptr_option<uint8_t>>(
3232
hole_fill_min,

‎src/proc/pointcloud.cpp‎

Lines changed: 16 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -72,7 +72,6 @@ namespace librealsense
7272
_depth_intrinsics = optional_value<rs2_intrinsics>();
7373
_depth_units = optional_value<float>();
7474
_extrinsics = optional_value<rs2_extrinsics>();
75-
_other_stream = nullptr;
7675
}
7776

7877
bool found_depth_intrinsics = false;
@@ -115,17 +114,20 @@ namespace librealsense
115114

116115
void pointcloud::inspect_other_frame(const rs2::frame& other)
117116
{
118-
if (_other_stream != nullptr && other.get_profile().as<rs2::video_stream_profile>() == *_other_stream.get())
119-
return;
120-
121-
if (!_other_stream.get())
117+
if (_stream_filter != _prev_stream_filter)
122118
{
123-
auto osp = other.get_profile().as<rs2::video_stream_profile>();
124-
_other_stream = std::make_shared<rs2::video_stream_profile>(osp.clone(osp.stream_type(), osp.stream_index(), osp.format()));
125-
_other_intrinsics = optional_value<rs2_intrinsics>();
126-
_extrinsics = optional_value<rs2_extrinsics>();
119+
_other_stream = nullptr;
120+
_prev_stream_filter = _stream_filter;
127121
}
128122

123+
if (_extrinsics.has_value() && (_other_stream && other.get_profile().as<rs2::video_stream_profile>() == *_other_stream.get()))
124+
return;
125+
126+
auto osp = other.get_profile().get();
127+
_other_stream = std::make_shared<rs2::video_stream_profile>(rs2::stream_profile(osp).as<rs2::video_stream_profile>());
128+
_other_intrinsics = optional_value<rs2_intrinsics>();
129+
_extrinsics = optional_value<rs2_extrinsics>();
130+
129131
if (!_other_intrinsics)
130132
{
131133
if (auto video = _other_stream->as<rs2::video_stream_profile>())
@@ -464,8 +466,6 @@ namespace librealsense
464466
pointcloud::pointcloud() :
465467
_other_stream(nullptr)
466468
{
467-
_stream_filter = RS2_STREAM_ANY;
468-
469469
_occlusion_filter = std::make_shared<occlusion_filter>();
470470

471471
auto occlusion_invalidation = std::make_shared<ptr_option<uint8_t>>(
@@ -500,10 +500,10 @@ namespace librealsense
500500
if (set)
501501
{
502502
//process composite frame only if it contains both a depth frame and the requested texture frame
503-
if (_stream_filter == RS2_STREAM_ANY)
503+
if (_stream_filter.stream == RS2_STREAM_ANY)
504504
return false;
505505

506-
auto tex = set.first_or_default(_stream_filter, _stream_format_filter);
506+
auto tex = set.first_or_default(_stream_filter.stream, _stream_filter.format);
507507
if (!tex)
508508
return false;
509509
auto depth = set.first_or_default(RS2_STREAM_DEPTH, RS2_FORMAT_Z16);
@@ -516,7 +516,7 @@ namespace librealsense
516516
return true;
517517

518518
auto p = frame.get_profile();
519-
if (p.stream_type() == _stream_filter && p.format() == _stream_format_filter && p.stream_index() == _stream_index_filter)
519+
if (p.stream_type() == _stream_filter.stream && p.format() == _stream_filter.format && p.stream_index() == _stream_filter.index)
520520
return true;
521521
return false;
522522

@@ -540,7 +540,7 @@ namespace librealsense
540540
inspect_depth_frame(depth);
541541
rv = process_depth_frame(source, depth);
542542

543-
auto texture = composite.first(_stream_filter);
543+
auto texture = composite.first(_stream_filter.stream);
544544
inspect_other_frame(texture);
545545
}
546546
else
@@ -551,7 +551,7 @@ namespace librealsense
551551
inspect_depth_frame(f);
552552
rv = process_depth_frame(source, f);
553553
}
554-
if (f.get_profile().stream_type() == _stream_filter && f.get_profile().format() == _stream_format_filter)
554+
if (f.get_profile().stream_type() == _stream_filter.stream && f.get_profile().format() == _stream_filter.format)
555555
{
556556
inspect_other_frame(f);
557557
}

‎src/proc/pointcloud.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
namespace librealsense
77
{
88
class occlusion_filter;
9+
910
class pointcloud : public stream_filter_processing_block
1011
{
1112
public:
@@ -37,5 +38,6 @@ namespace librealsense
3738
std::vector<float> _pre_compute_map_y;
3839

3940
void pre_compute_x_y_map();
41+
stream_filter _prev_stream_filter;
4042
};
4143
}

‎src/proc/spatial-filter.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -61,8 +61,8 @@ namespace librealsense
6161
_holes_filling_mode(holes_fill_def),
6262
_holes_filling_radius(0)
6363
{
64-
_stream_filter = RS2_STREAM_DEPTH;
65-
_stream_format_filter = RS2_FORMAT_Z16;
64+
_stream_filter.stream = RS2_STREAM_DEPTH;
65+
_stream_filter.format = RS2_FORMAT_Z16;
6666

6767
auto spatial_filter_alpha = std::make_shared<ptr_option<float>>(
6868
alpha_min_val,

‎src/proc/synthetic-stream.cpp‎

Lines changed: 13 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -118,15 +118,12 @@ namespace librealsense
118118
return source.allocate_composite_frame(results);
119119
}
120120

121-
stream_filter_processing_block::stream_filter_processing_block() :
122-
_stream_filter(RS2_STREAM_ANY),
123-
_stream_format_filter(RS2_FORMAT_ANY),
124-
_stream_index_filter(-1)
121+
stream_filter_processing_block::stream_filter_processing_block()
125122
{
126123
register_option(RS2_OPTION_FRAMES_QUEUE_SIZE, _source.get_published_size_option());
127124
_source.init(std::shared_ptr<metadata_parser_map>());
128125

129-
auto stream_selector = std::make_shared<ptr_option<int>>(RS2_STREAM_ANY, RS2_STREAM_FISHEYE, 1, RS2_STREAM_ANY, (int*)&_stream_filter, "Stream type");
126+
auto stream_selector = std::make_shared<ptr_option<int>>(RS2_STREAM_ANY, RS2_STREAM_FISHEYE, 1, RS2_STREAM_ANY, (int*)&_stream_filter.stream, "Stream type");
130127
for (int s = RS2_STREAM_ANY; s < RS2_STREAM_COUNT; s++)
131128
{
132129
stream_selector->set_description(s, "Process - " + std::string (rs2_stream_to_string((rs2_stream)s)));
@@ -139,10 +136,10 @@ namespace librealsense
139136
throw invalid_value_exception(to_string()
140137
<< "Unsupported stream filter, " << val << " is out of range.");
141138

142-
_stream_filter = static_cast<rs2_stream>((int)val);
139+
_stream_filter.stream = static_cast<rs2_stream>((int)val);
143140
});
144141

145-
auto format_selector = std::make_shared<ptr_option<int>>(RS2_FORMAT_ANY, RS2_FORMAT_DISPARITY32, 1, RS2_FORMAT_ANY, (int*)&_stream_format_filter, "Stream format");
142+
auto format_selector = std::make_shared<ptr_option<int>>(RS2_FORMAT_ANY, RS2_FORMAT_DISPARITY32, 1, RS2_FORMAT_ANY, (int*)&_stream_filter.format, "Stream format");
146143
for (int f = RS2_FORMAT_ANY; f < RS2_FORMAT_COUNT; f++)
147144
{
148145
format_selector->set_description(f, "Process - " + std::string(rs2_format_to_string((rs2_format)f)));
@@ -155,10 +152,10 @@ namespace librealsense
155152
throw invalid_value_exception(to_string()
156153
<< "Unsupported stream format filter, " << val << " is out of range.");
157154

158-
_stream_format_filter = static_cast<rs2_format>((int)val);
155+
_stream_filter.format = static_cast<rs2_format>((int)val);
159156
});
160157

161-
auto index_selector = std::make_shared<ptr_option<int>>(0, std::numeric_limits<int>::max(), 1, -1, &_stream_index_filter, "Stream index");
158+
auto index_selector = std::make_shared<ptr_option<int>>(0, std::numeric_limits<int>::max(), 1, -1, &_stream_filter.index, "Stream index");
162159
index_selector->on_set([this, index_selector](float val)
163160
{
164161
std::lock_guard<std::mutex> lock(_mutex);
@@ -167,7 +164,7 @@ namespace librealsense
167164
throw invalid_value_exception(to_string()
168165
<< "Unsupported stream index filter, " << val << " is out of range.");
169166

170-
_stream_index_filter = (int)val;
167+
_stream_filter.index = (int)val;
171168
});
172169

173170
register_option(RS2_OPTION_STREAM_FILTER, stream_selector);
@@ -191,20 +188,20 @@ namespace librealsense
191188
rs2_format format = profile.format();
192189
int index = profile.stream_index();
193190

194-
if (_stream_filter != RS2_STREAM_ANY && _stream_filter != stream)
191+
if (_stream_filter.stream != RS2_STREAM_ANY && _stream_filter.stream != stream)
195192
return false;
196-
if (is_z_or_disparity(_stream_format_filter))
193+
if (is_z_or_disparity(_stream_filter.format))
197194
{
198-
if (_stream_format_filter != RS2_FORMAT_ANY && !is_z_or_disparity(format))
195+
if (_stream_filter.format != RS2_FORMAT_ANY && !is_z_or_disparity(format))
199196
return false;
200197
}
201198
else
202199
{
203-
if (_stream_format_filter != RS2_FORMAT_ANY && _stream_format_filter != format)
200+
if (_stream_filter.format != RS2_FORMAT_ANY && _stream_filter.format != format)
204201
return false;
205202
}
206203

207-
if (_stream_index_filter != -1 && _stream_index_filter != index)
204+
if (_stream_filter.index != -1 && _stream_filter.index != index)
208205
return false;
209206
return true;
210207
}
@@ -214,19 +211,7 @@ namespace librealsense
214211
if (!frame || frame.is<rs2::frameset>())
215212
return false;
216213
auto profile = frame.get_profile();
217-
rs2_stream stream = profile.stream_type();
218-
rs2_format format = profile.format();
219-
int index = profile.stream_index();
220-
221-
if (_stream_filter != RS2_STREAM_ANY && _stream_filter != stream)
222-
return false;
223-
224-
if (_stream_format_filter != RS2_FORMAT_ANY && _stream_format_filter != format)
225-
return false;
226-
227-
if (_stream_index_filter != -1 && _stream_index_filter != index)
228-
return false;
229-
return true;
214+
return _stream_filter.match(frame);
230215
}
231216

232217
void synthetic_source::frame_ready(frame_holder result)

‎src/proc/synthetic-stream.h‎

Lines changed: 51 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -71,16 +71,64 @@ namespace librealsense
7171
virtual rs2::frame process_frame(const rs2::frame_source& source, const rs2::frame& f) = 0;
7272
};
7373

74+
struct stream_filter
75+
{
76+
rs2_stream stream;
77+
rs2_format format;
78+
int index;
79+
80+
stream_filter() : stream(RS2_STREAM_ANY), format(RS2_FORMAT_ANY), index(-1) {}
81+
stream_filter(rs2_stream s, rs2_format f, int i) : stream(s), format(f), index(i) {}
82+
83+
bool match(const rs2::frame& frame)
84+
{
85+
stream_filter filter(frame.get_profile().stream_type(), frame.get_profile().format(), frame.get_profile().stream_index());
86+
return match(filter);
87+
}
88+
89+
bool match(const stream_filter& other)
90+
{
91+
if (stream != RS2_STREAM_ANY && stream != other.stream)
92+
return false;
93+
if (format != RS2_FORMAT_ANY && format != other.format)
94+
return false;
95+
if (index != -1 && index != other.index)
96+
return false;
97+
return true;
98+
}
99+
100+
bool operator==(const stream_filter& other)
101+
{
102+
if (stream != other.stream)
103+
return false;
104+
if (format != other.format)
105+
return false;
106+
if (index != other.index)
107+
return false;
108+
return true;
109+
}
110+
111+
bool operator!=(const stream_filter& other)
112+
{
113+
return !(*this == other);
114+
}
115+
116+
void operator=(const stream_filter& other)
117+
{
118+
stream = other.stream;
119+
format = other.format;
120+
index = other.index;
121+
}
122+
};
123+
74124
class stream_filter_processing_block : public generic_processing_block
75125
{
76126
public:
77127
stream_filter_processing_block();
78128
virtual ~stream_filter_processing_block() { _source.flush(); }
79129

80130
protected:
81-
rs2_stream _stream_filter;
82-
rs2_format _stream_format_filter;
83-
int _stream_index_filter;
131+
stream_filter _stream_filter;
84132

85133
bool should_process(const rs2::frame& frame) override;
86134
};

‎src/proc/temporal-filter.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -40,8 +40,8 @@ namespace librealsense
4040
_extension_type(RS2_EXTENSION_DEPTH_FRAME),
4141
_current_frm_size_pixels(0)
4242
{
43-
_stream_filter = RS2_STREAM_DEPTH;
44-
_stream_format_filter = RS2_FORMAT_Z16;
43+
_stream_filter.stream = RS2_STREAM_DEPTH;
44+
_stream_filter.format = RS2_FORMAT_Z16;
4545

4646
auto temporal_persistence_control = std::make_shared<ptr_option<uint8_t>>(
4747
persistence_min,

0 commit comments

Comments
 (0)