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

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

Hi,

Following the documentation, I think the following is wrong:

disparity_map = CvBridge().imgmsg_to_cv2(disparity_map, "32FC1")/16

I am not sure how imgmsg_to_cv2 works for the conversion in python, but as the input is 16SC1 type, would it be "16SC1" type, then convert to float, then divide by 16? If you want to convert directly the input disparity image to depth, look at the following code (follow disparity.type() == CV_16SC1 type):

cv::Mat depthFromDisparity(const cv::Mat & disparity,
		float fx, float baseline,
		int type)
{
	UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
	UASSERT(type == CV_32FC1 || type == CV_16UC1);
	cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
	int countOverMax = 0;
	for (int i = 0; i < disparity.rows; i++)
	{
		for (int j = 0; j < disparity.cols; j++)
		{
			float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
			if (disparity_value > 0.0f)
			{
				// baseline * focal / disparity
				float d = baseline * fx / disparity_value;
				if(d>0)
				{
					if(depth.type() == CV_32FC1)
					{
						depth.at<float>(i,j) = d;
					}
					else
					{
						if(d*1000.0f <= (float)USHRT_MAX)
						{
							depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
						}
						else
						{
							++countOverMax;
						}
					}
				}
			}
		}
	}
	if(countOverMax)
	{
		UWARN("Depth conversion error, %d depth values ignored because they are over the maximum depth allowed (65535 mm).", countOverMax);
	}
	return depth;
}

Here we use two callback functions because our camera doesn't publish the camera info fast enough.

Looking at the camera code, they should be published at the same rate and with exact same time. Rtabmap assumes also that camera_info and images are published at exactly the same rate.

cheers,
Mathieu