Ejemplo n.º 1
0
    void pointcloud::inspect_depth_frame(const rs2::frame& depth)
    {
        if (!_output_stream || _depth_stream.get_profile().get() != depth.get_profile().get())
        {
            _output_stream = depth.get_profile().as<rs2::video_stream_profile>().clone(
                RS2_STREAM_DEPTH, depth.get_profile().stream_index(), RS2_FORMAT_XYZ32F);
            _depth_stream = depth;
            _depth_intrinsics = optional_value<rs2_intrinsics>();
            _depth_units = optional_value<float>();
            _extrinsics = optional_value<rs2_extrinsics>();
        }

        bool found_depth_intrinsics = false;
        bool found_depth_units = false;

        if (!_depth_intrinsics)
        {
            auto stream_profile = depth.get_profile();
            if (auto video = stream_profile.as<rs2::video_stream_profile>())
            {
                _depth_intrinsics = video.get_intrinsics();
                _pixels_map.resize(_depth_intrinsics->height*_depth_intrinsics->width);
                _occlusion_filter->set_depth_intrinsics(_depth_intrinsics.value());

                preprocess();

                found_depth_intrinsics = true;
            }
        }

        if (!_depth_units)
        {
            auto sensor = ((frame_interface*)depth.get())->get_sensor().get();
            _depth_units = sensor->get_option(RS2_OPTION_DEPTH_UNITS).query();
            found_depth_units = true;
        }

        set_extrinsics();
    }
    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;
        }
    }