Re: Clarification on RGB and Depth image has input from Stereo camera

Posted by FeKl on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Clarification-on-RGB-and-Depth-image-has-input-from-Stereo-camera-tp2469p5083.html

Hi Mathieu,

thank you for your help.

Finally, we managed it to convert the disparity into depth via the following code lines:

for (int i = 0; i < cv_ptr->image.rows; i++)
{
  for (int j = 0; j < cv_ptr->image.cols; j++)
  {
    if (cv_ptr->image.at<float>(i,j) >= 0x0fff)
    {
      cv_ptr->image.at<float>(i,j) = -1;
    }
    else
      // depth = focal * baseline / (disparity/16) (from http://wiki.ros.org/nerian_stereo: "The disparity values have to be divided by 16 in order to receive the true pixel disparity.")
      // we use the absolute value of the baseline (because here it is negative)
      cv_ptr->image.at<float>(i,j) = cam_info->Q[11]*std::abs(cam_info->T_left_right[0])*16./cv_ptr->image.at<float>(i,j);
  }
}

Here is an example of the resulting depth:


Kind regards,

Felix

PS: By the way, originally the camera only publishs the camera info every second (see https://github.com/nerian-vision/nerian_stereo/blob/88b99b84a5c8d1a8be841968c42ab681d309aa50/src/nerian_stereo_node.cpp#L625 ); that's why we had to adjust the code a little bit.