// constructor
Main(void) : priority(UNSPECIFIED) {
// register options
register_option(opt_version);
register_option(opt_help);
// register event listeners
opt_version.add_event_listener(this);
opt_help.add_event_listener(this);
}
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);
}
/* module initialization and will be executed automatically when loading. */
void module_init(){
shutdown = malloc(sizeof(shutdown_config_t));
register_callback(max_insn_callback, Step_callback);
register_callback(write_shutdown_callback, Bus_write_callback);
if(register_option("shutdown_device", do_shutdown_option, "Used to stop machine by writing a special address or set the max executed instruction number.") != No_exp)
fprintf(stderr,"Can not register shutdown_device option\n");
}
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);
}
void playback_sensor::register_sensor_options(const device_serializer::sensor_snapshot& sensor_snapshot)
{
auto options_snapshot = sensor_snapshot.get_sensor_extensions_snapshots().find(RS2_EXTENSION_OPTIONS);
if (options_snapshot == nullptr)
{
LOG_WARNING("Recorded file does not contain sensor options");
return;
}
auto options_api = As<options_interface>(options_snapshot);
if (options_api == nullptr)
{
throw invalid_value_exception("Failed to get options interface from sensor snapshots");
}
for (int i = 0; i < static_cast<int>(RS2_OPTION_COUNT); i++)
{
auto option_id = static_cast<rs2_option>(i);
try
{
if (options_api->supports_option(option_id))
{
auto&& option = options_api->get_option(option_id);
float value = option.query();
register_option(option_id, std::make_shared<const_value_option>(option.get_description(), option.query()));
LOG_DEBUG("Registered " << rs2_option_to_string(option_id) << " for sensor " << m_sensor_id << " with value: " << option.query());
}
}
catch (std::exception& e)
{
LOG_WARNING("Failed to register option " << option_id << ". Exception: " << e.what());
}
}
}
int cov_module_init()
{
register_option("code_coverage", cov_conf_parse, "code coverage module");
add_command("cov-on", cov_state_on, "turn on code coverage switch.\n");
add_command("cov-off", cov_state_off, "turn off code coverage switch.\n");
add_command("cov-state", cov_state_show, "show code coverage state.\n");
return 0;
}
int
skyeye_option_init (skyeye_config_t * config)
{
/* 2007-01-18 added by Anthony Lee: for new uart device frame */
config->uart.count = 0;
/* should move to uart module loading */
//atexit(skyeye_uart_cleanup);
/*ywc 2005-04-01 */
config->no_dbct = 1; /*default, dbct is off */
//teawater add for new tb manage function 2005.07.10----------------------------
config->tb_tbt_size = 0;
#if DBCT
config->tb_tbp_size = TB_TBP_DEFAULT;
#else
config->tb_tbp_size = 0;
#endif
skyeye_option_list = NULL;
register_option("cpu", do_deprecated_option, "Do not need to provide cpu option any more.\n");
}
void module_init(){
/* register the arm core to the common library */
init_arm_arch ();
#ifdef LLVM_EXIST
printf("arm LLVM EXIST \n");
init_arm_dyncom ();
#else
printf("arm Don't have LLVM\n");
#endif
/*
* register all the supported mach to the common library.
*/
int i = 0;
while(arm_machines[i].machine_name != NULL){
register_mach(arm_machines[i].machine_name, arm_machines[i].mach_init);
i++;
}
if(register_option("cpu", do_cpu_option, "Processor option for arm architecture.") != No_exp)
fprintf(stderr,"Can not register cpu option\n");
}
int
register_option (__opt_cptr func, uint8_t argc, char *match, __o_dbent pe)
{
size_t mlen = strlen (match);
uint8_t key = (uint8_t) match[0];
if (0 == key)
{
pe->opt.ac = argc;
pe->opt.op = func;
return 0;
}
if (glob_dbe == pe)
{
pe = &glob_dbe[key];
}
else
{
pe = &((__o_dbent) pe->n_dbent)[key];
}
if (0 == pe->l && 0 == mlen)
{
return 1;
}
pe->l++;
if (mlen > 0 && !pe->n_dbent)
{
pe->n_dbent = calloc (UCHAR_MAX, sizeof(_o_dbent));
}
return register_option (func, argc, &match[1], pe);
}
void init_mach(){
mach_list = NULL;
register_option("mach", do_mach_option, "machine option");
}
/**
* @brief the initialization of arch
*/
void init_arch(){
register_option("arch", do_arch_option, "support different architectures.\n");
}
void playback_sensor::update_option(rs2_option id, std::shared_ptr<option> option)
{
register_option(id, option);
}
void init_bus(){
register_option("mem_bank", do_mem_bank_option, "");
//register_option("mem_bank", do_bus_bank_option, "");
}
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));
}
void software_sensor::add_read_only_option(rs2_option option, float val)
{
register_option(option, std::make_shared<const_value_option>("bypass sensor read only option",
lazy<float>([=]() { return val; })));
}
processing_block::processing_block()
: _source_wrapper(_source)
{
register_option(RS2_OPTION_FRAMES_QUEUE_SIZE, _source.get_published_size_option());
_source.init(std::make_shared<metadata_parser_map>());
}
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);
}
