pointcloud::pointcloud()
:stream_filter_processing_block("Pointcloud")
{
_occlusion_filter = std::make_shared<occlusion_filter>();
auto occlusion_invalidation = std::make_shared<ptr_option<uint8_t>>(
occlusion_none,
occlusion_max - 1, 1,
occlusion_none,
(uint8_t*)&_occlusion_filter->_occlusion_filter,
"Occlusion removal");
occlusion_invalidation->on_set([this, occlusion_invalidation](float val)
{
if (!occlusion_invalidation->is_valid(val))
throw invalid_value_exception(to_string()
<< "Unsupported occlusion filtering requiested " << val << " is out of range.");
_occlusion_filter->set_mode(static_cast<uint8_t>(val));
});
occlusion_invalidation->set_description(0.f, "Off");
occlusion_invalidation->set_description(1.f, "Heuristic");
occlusion_invalidation->set_description(2.f, "Exhaustive");
register_option(RS2_OPTION_FILTER_MAGNITUDE, occlusion_invalidation);
}
disparity_transform::disparity_transform(bool transform_to_disparity):
generic_processing_block(transform_to_disparity ? "Depth to Disparity" : "Disparity to Depth"),
_transform_to_disparity(transform_to_disparity),
_update_target(false),
_stereoscopic_depth(false),
_focal_lenght_mm(0.f),
_stereo_baseline_meter(0.f),
_depth_units(0.f),
_d2d_convert_factor(0.f),
_width(0), _height(0), _bpp(0)
{
auto transform_opt = std::make_shared<ptr_option<bool>>(
false,true,true,true,
&_transform_to_disparity,
"Stereoscopic Transformation Mode");
transform_opt->set_description(false, "Disparity to Depth");
transform_opt->set_description(true, "Depth to Disparity");
transform_opt->on_set([this, transform_opt](float val)
{
std::lock_guard<std::mutex> lock(_mutex);
if (!transform_opt->is_valid(val))
throw invalid_value_exception(to_string() << "Unsupported transformation mode" << (int)val << " is out of range.");
on_set_mode(static_cast<bool>(!!int(val)));
});
unregister_option(RS2_OPTION_FRAMES_QUEUE_SIZE);
on_set_mode(_transform_to_disparity);
}
hole_filling_filter::hole_filling_filter() :
_width(0), _height(0), _stride(0), _bpp(0),
_extension_type(RS2_EXTENSION_DEPTH_FRAME),
_current_frm_size_pixels(0),
_hole_filling_mode(hole_fill_def)
{
_stream_filter.stream = RS2_STREAM_DEPTH;
_stream_filter.format = RS2_FORMAT_Z16;
auto hole_filling_mode = std::make_shared<ptr_option<uint8_t>>(
hole_fill_min,
hole_fill_max,
hole_fill_step,
hole_fill_def,
&_hole_filling_mode, "Hole Filling mode");
hole_filling_mode->set_description(hf_fill_from_left, "Fill from Left");
hole_filling_mode->set_description(hf_farest_from_around, "Farest from around");
hole_filling_mode->set_description(hf_nearest_from_around, "Nearest from around");
hole_filling_mode->on_set([this, hole_filling_mode](float val)
{
std::lock_guard<std::mutex> lock(_mutex);
if (!hole_filling_mode->is_valid(val))
throw invalid_value_exception(to_string()
<< "Unsupported mode for hole filling selected: value " << val << " is out of range.");
_hole_filling_mode = static_cast<uint8_t>(val);
});
register_option(RS2_OPTION_HOLES_FILL, hole_filling_mode);
}
colorizer::colorizer()
: _min(0.f), _max(6.f), _equalize(true), _stream()
{
_maps = { &jet, &classic, &grayscale, &inv_grayscale, &biomes, &cold, &warm, &quantized, &pattern };
auto min_opt = std::make_shared<ptr_option<float>>(0.f, 16.f, 0.1f, 0.f, &_min, "Min range in meters");
register_option(RS2_OPTION_MIN_DISTANCE, min_opt);
auto max_opt = std::make_shared<ptr_option<float>>(0.f, 16.f, 0.1f, 6.f, &_max, "Max range in meters");
register_option(RS2_OPTION_MAX_DISTANCE, max_opt);
auto color_map = std::make_shared<ptr_option<int>>(0, (int)_maps.size() - 1, 1, 0, &_map_index, "Color map");
color_map->set_description(0.f, "Jet");
color_map->set_description(1.f, "Classic");
color_map->set_description(2.f, "White to Black");
color_map->set_description(3.f, "Black to White");
color_map->set_description(4.f, "Bio");
color_map->set_description(5.f, "Cold");
color_map->set_description(6.f, "Warm");
color_map->set_description(7.f, "Quantized");
color_map->set_description(8.f, "Pattern");
register_option(RS2_OPTION_COLOR_SCHEME, color_map);
auto preset_opt = std::make_shared<ptr_option<int>>(0, 3, 1, 0, &_preset, "Preset depth colorization");
preset_opt->set_description(0.f, "Dynamic");
preset_opt->set_description(1.f, "Fixed");
preset_opt->set_description(2.f, "Near");
preset_opt->set_description(3.f, "Far");
preset_opt->on_set([this](float val)
{
if (fabs(val - 0.f) < 1e-6)
{
// Dynamic
_equalize = true;
_map_index = 0;
}
if (fabs(val - 1.f) < 1e-6)
{
// Fixed
_equalize = false;
_map_index = 0;
_min = 0.f;
_max = 6.f;
}
if (fabs(val - 2.f) < 1e-6)
{
// Near
_equalize = false;
_map_index = 1;
_min = 0.3f;
_max = 1.5f;
}
if (fabs(val - 3.f) < 1e-6)
{
// Far
_equalize = false;
_map_index = 0;
_min = 1.f;
_max = 16.f;
}
});
register_option(RS2_OPTION_VISUAL_PRESET, preset_opt);
auto hist_opt = std::make_shared<ptr_option<bool>>(false, true, true, true, &_equalize, "Perform histogram equalization");
register_option(RS2_OPTION_HISTOGRAM_EQUALIZATION_ENABLED, hist_opt);
auto on_frame = [this](rs2::frame f, const rs2::frame_source& source)
{
auto process_frame = [this, &source](const rs2::frame f)
{
{
std::lock_guard<std::mutex> lock(_mutex);
if (!_stream)
{
_stream = std::make_shared<rs2::stream_profile>(f.get_profile().clone(RS2_STREAM_DEPTH, 0, RS2_FORMAT_RGB8));
environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*_stream->get()->profile, *f.get_profile().get()->profile);
}
}
auto make_equalized_histogram = [this](const rs2::video_frame& depth, rs2::video_frame rgb)
{
const auto max_depth = 0x10000;
static uint32_t histogram[max_depth];
memset(histogram, 0, sizeof(histogram));
const auto w = depth.get_width(), h = depth.get_height();
const auto depth_data = reinterpret_cast<const uint16_t*>(depth.get_data());
auto rgb_data = reinterpret_cast<uint8_t*>(const_cast<void *>(rgb.get_data()));
for (auto i = 0; i < w*h; ++i) ++histogram[depth_data[i]];
for (auto i = 2; i < max_depth; ++i) histogram[i] += histogram[i - 1]; // Build a cumulative histogram for the indices in [1,0xFFFF]
auto cm = _maps[_map_index];
for (auto i = 0; i < w*h; ++i)
{
auto d = depth_data[i];
if (d)
{
auto f = histogram[d] / (float)histogram[0xFFFF]; // 0-255 based on histogram location
auto c = cm->get(f);
rgb_data[i * 3 + 0] = (uint8_t)c.x;
rgb_data[i * 3 + 1] = (uint8_t)c.y;
rgb_data[i * 3 + 2] = (uint8_t)c.z;
}
else
{
rgb_data[i * 3 + 0] = 0;
rgb_data[i * 3 + 1] = 0;
rgb_data[i * 3 + 2] = 0;
}
}
};
auto make_value_cropped_frame = [this](const rs2::video_frame& depth, rs2::video_frame rgb)
{
const auto max_depth = 0x10000;
const auto w = depth.get_width(), h = depth.get_height();
const auto depth_data = reinterpret_cast<const uint16_t*>(depth.get_data());
auto rgb_data = reinterpret_cast<uint8_t*>(const_cast<void *>(rgb.get_data()));
auto fi = (frame_interface*)depth.get();
auto df = dynamic_cast<librealsense::depth_frame*>(fi);
auto depth_units = df->get_units();
for (auto i = 0; i < w*h; ++i)
{
auto d = depth_data[i];
if (d)
{
auto f = (d * depth_units - _min) / (_max - _min);
auto c = _maps[_map_index]->get(f);
rgb_data[i * 3 + 0] = (uint8_t)c.x;
rgb_data[i * 3 + 1] = (uint8_t)c.y;
rgb_data[i * 3 + 2] = (uint8_t)c.z;
}
else
{
rgb_data[i * 3 + 0] = 0;
rgb_data[i * 3 + 1] = 0;
rgb_data[i * 3 + 2] = 0;
}
}
};
rs2::frame ret = f;
if (f.get_profile().stream_type() == RS2_STREAM_DEPTH)
{
auto vf = f.as<rs2::video_frame>();
rs2_extension ext = f.is<rs2::disparity_frame>() ? RS2_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME;
ret = source.allocate_video_frame(*_stream, f, 3, vf.get_width(), vf.get_height(), vf.get_width() * 3, ext);
if (_equalize) make_equalized_histogram(f, ret);
else make_value_cropped_frame(f, ret);
}
source.frame_ready(ret);
};
if (auto composite = f.as<rs2::frameset>()) composite.foreach(process_frame);
else process_frame(f);
};
auto callback = new rs2::frame_processor_callback<decltype(on_frame)>(on_frame);
processing_block::set_processing_callback(std::shared_ptr<rs2_frame_processor_callback>(callback));
}
disparity_transform::disparity_transform(bool transform_to_disparity):
_transform_to_disparity(transform_to_disparity),
_update_target(false),
_stereoscopic_depth(false),
_focal_lenght_mm(0.f),
_stereo_baseline(0.f),
_depth_units(0.f),
_d2d_convert_factor(0.f),
_width(0), _height(0), _bpp(0)
{
auto transform_opt = std::make_shared<ptr_option<bool>>(
false,true,true,true,
&_transform_to_disparity,
"Stereoscopic Transformation Mode");
transform_opt->set_description(false, "Disparity to Depth");
transform_opt->set_description(true, "Depth to Disparity");
transform_opt->on_set([this, transform_opt](float val)
{
std::lock_guard<std::mutex> lock(_mutex);
if (!transform_opt->is_valid(val))
throw invalid_value_exception(to_string() << "Unsupported transformation mode" << (int)val << " is out of range.");
on_set_mode(static_cast<bool>(!!int(val)));
});
unregister_option(RS2_OPTION_FRAMES_QUEUE_SIZE);
auto on_frame = [this](rs2::frame f, const rs2::frame_source& source)
{
std::lock_guard<std::mutex> lock(_mutex);
rs2::frame out = f;
rs2::frame tgt, depth_data;
bool composite = f.is<rs2::frameset>();
tgt = depth_data = (composite) ? f.as<rs2::frameset>().first_or_default(RS2_STREAM_DEPTH) : f;
// Verify that the input depth format is aligned with the block's configuration
if (depth_data && (f.is<rs2::disparity_frame>() != _transform_to_disparity))
{
update_transformation_profile(depth_data);
if (_stereoscopic_depth && (tgt = prepare_target_frame(depth_data, source)))
{
auto src = depth_data.as<rs2::video_frame>();
if (_transform_to_disparity)
convert<uint16_t, float>(src.get_data(), const_cast<void*>(tgt.get_data()));
else
convert<float, uint16_t>(src.get_data(), const_cast<void*>(tgt.get_data()));
}
}
out = composite ? source.allocate_composite_frame({ tgt }) : tgt;
source.frame_ready(out);
};
auto callback = new rs2::frame_processor_callback<decltype(on_frame)>(on_frame);
processing_block::set_processing_callback(std::shared_ptr<rs2_frame_processor_callback>(callback));
on_set_mode(_transform_to_disparity);
}
colorizer::colorizer()
: _min(0.f), _max(6.f), _equalize(true), _target_stream_profile()
{
_stream_filter.stream = RS2_STREAM_DEPTH;
_stream_filter.format = RS2_FORMAT_Z16;
_maps = { &jet, &classic, &grayscale, &inv_grayscale, &biomes, &cold, &warm, &quantized, &pattern };
auto min_opt = std::make_shared<ptr_option<float>>(0.f, 16.f, 0.1f, 0.f, &_min, "Min range in meters");
register_option(RS2_OPTION_MIN_DISTANCE, min_opt);
auto max_opt = std::make_shared<ptr_option<float>>(0.f, 16.f, 0.1f, 6.f, &_max, "Max range in meters");
register_option(RS2_OPTION_MAX_DISTANCE, max_opt);
auto color_map = std::make_shared<ptr_option<int>>(0, (int)_maps.size() - 1, 1, 0, &_map_index, "Color map");
color_map->set_description(0.f, "Jet");
color_map->set_description(1.f, "Classic");
color_map->set_description(2.f, "White to Black");
color_map->set_description(3.f, "Black to White");
color_map->set_description(4.f, "Bio");
color_map->set_description(5.f, "Cold");
color_map->set_description(6.f, "Warm");
color_map->set_description(7.f, "Quantized");
color_map->set_description(8.f, "Pattern");
register_option(RS2_OPTION_COLOR_SCHEME, color_map);
auto preset_opt = std::make_shared<ptr_option<int>>(0, 3, 1, 0, &_preset, "Preset depth colorization");
preset_opt->set_description(0.f, "Dynamic");
preset_opt->set_description(1.f, "Fixed");
preset_opt->set_description(2.f, "Near");
preset_opt->set_description(3.f, "Far");
preset_opt->on_set([this](float val)
{
if (fabs(val - 0.f) < 1e-6)
{
// Dynamic
_equalize = true;
_map_index = 0;
}
if (fabs(val - 1.f) < 1e-6)
{
// Fixed
_equalize = false;
_map_index = 0;
_min = 0.f;
_max = 6.f;
}
if (fabs(val - 2.f) < 1e-6)
{
// Near
_equalize = false;
_map_index = 1;
_min = 0.3f;
_max = 1.5f;
}
if (fabs(val - 3.f) < 1e-6)
{
// Far
_equalize = false;
_map_index = 0;
_min = 1.f;
_max = 16.f;
}
});
register_option(RS2_OPTION_VISUAL_PRESET, preset_opt);
auto hist_opt = std::make_shared<ptr_option<bool>>(false, true, true, true, &_equalize, "Perform histogram equalization");
register_option(RS2_OPTION_HISTOGRAM_EQUALIZATION_ENABLED, hist_opt);
}
