librealsense2 manual transform to depth from disparity and vice versa
// 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;
}