std::shared_ptr<stream_profile_interface> software_sensor::add_video_stream(rs2_video_stream video_stream) { auto exist = (std::find_if(_profiles.begin(), _profiles.end(), [&](std::shared_ptr<stream_profile_interface> profile) { if (profile->get_unique_id() == video_stream.uid) { return true; } return false; } ) != _profiles.end()); if (exist) { LOG_WARNING("Stream unique ID already exist!"); throw rs2::error("Stream unique ID already exist!"); } auto profile = std::make_shared<video_stream_profile>( platform::stream_profile{ (uint32_t)video_stream.width, (uint32_t)video_stream.height, (uint32_t)video_stream.fps, 0 }); profile->set_dims(video_stream.width, video_stream.height); profile->set_format(video_stream.fmt); profile->set_framerate(video_stream.fps); profile->set_stream_index(video_stream.index); profile->set_stream_type(video_stream.type); profile->set_unique_id(video_stream.uid); profile->set_intrinsics([=]() {return video_stream.intrinsics; }); _profiles.push_back(profile); return profile; }
std::shared_ptr<stream_profile_interface> software_sensor::add_motion_stream(rs2_motion_stream motion_stream) { auto exist = (std::find_if(_profiles.begin(), _profiles.end(), [&](std::shared_ptr<stream_profile_interface> profile) { return profile->get_unique_id() == motion_stream.uid; }) != _profiles.end()); if (exist) { LOG_WARNING("Motion stream unique ID already exist!"); throw rs2::error("Stream unique ID already exist!"); } auto profile = std::make_shared<motion_stream_profile>( platform::stream_profile{ 0, 0, (uint32_t)motion_stream.fps, 0 }); profile->set_format(motion_stream.fmt); profile->set_framerate(motion_stream.fps); profile->set_stream_index(motion_stream.index); profile->set_stream_type(motion_stream.type); profile->set_unique_id(motion_stream.uid); profile->set_intrinsics([=]() {return motion_stream.intrinsics; }); _profiles.push_back(profile); return profile; }
void disparity_transform::update_transformation_profile(const rs2::frame& f) { if(f.get_profile().get() != _source_stream_profile.get()) { _source_stream_profile = f.get_profile(); // Check if the new frame originated from stereo-based depth sensor // and retrieve the stereo baseline parameter that will be used in transformations auto snr = ((frame_interface*)f.get())->get_sensor().get(); librealsense::depth_stereo_sensor* dss; // Playback sensor if (auto a = As<librealsense::extendable_interface>(snr)) { librealsense::depth_stereo_sensor* ptr; if (_stereoscopic_depth = a->extend_to(TypeToExtension<librealsense::depth_stereo_sensor>::value, (void**)&ptr)) { dss = ptr; _depth_units = dss->get_depth_scale(); _stereo_baseline_meter = dss->get_stereo_baseline_mm()*0.001f; } } else // Live sensor { _stereoscopic_depth = Is<librealsense::depth_stereo_sensor>(snr); if (_stereoscopic_depth) { dss = As<librealsense::depth_stereo_sensor>(snr); _depth_units = dss->get_depth_scale(); _stereo_baseline_meter = dss->get_stereo_baseline_mm()* 0.001f; } } if (_stereoscopic_depth) { auto vp = _source_stream_profile.as<rs2::video_stream_profile>(); _focal_lenght_mm = vp.get_intrinsics().fx; const uint8_t fractional_bits = 5; const uint8_t fractions = 1 << fractional_bits; _d2d_convert_factor = (_stereo_baseline_meter * _focal_lenght_mm * fractions) / _depth_units; _width = vp.width(); _height = vp.height(); _update_target = true; } } // Adjust the target profile if (_update_target) { auto tgt_format = _transform_to_disparity ? RS2_FORMAT_DISPARITY32 : RS2_FORMAT_Z16; _target_stream_profile = _source_stream_profile.clone(RS2_STREAM_DEPTH, 0, tgt_format); auto src_vspi = dynamic_cast<video_stream_profile_interface*>(_source_stream_profile.get()->profile); auto tgt_vspi = dynamic_cast<video_stream_profile_interface*>(_target_stream_profile.get()->profile); rs2_intrinsics src_intrin = src_vspi->get_intrinsics(); tgt_vspi->set_intrinsics([src_intrin]() { return src_intrin; }); tgt_vspi->set_dims(src_intrin.width, src_intrin.height); _update_target = false; } }