librealsense2 manual transform to depth from disparity and vice versa

main.cpp

// Retrieve Focal Length
rs2::video_stream_profile profile = pipeline_profile.get_stream( rs2_stream::RS2_STREAM_DEPTH ).as<rs2::video_stream_profile>();
const float focal_length = profile.get_intrinsics().fx;

// Retriece Base Line
const float baseline = disparity_frame.as<rs2::disparity_frame>().get_baseline();

// Target Pixel (e.g. Center of Image)
const uint32_t x = disparity_width / 2;
const uint32_t y = disparity_height / 2;

{
    // Transform to Depth from Disparity
    // depth = focal_length * baseline / disparity
    const float* disparity_data = reinterpret_cast<const float*>( disparity_frame.get_data() );
    const float disparity = disparity_data[y * disparity_width + x];
    const float depth = focal_length * baseline / disparity;
    std::cout << depth << std::endl;
}

{
    // Transform to Disparity from Depth
    // disparity = focal_length * baseline / depth
    const short* depth_data = reinterpret_cast<const short*>( depth_frame.get_data() );
    const short depth = depth_data[y * depth_width + x];
    const float disparity = focal_length * baseline / depth;
    std::cout << disparity << std::endl;
}