RgbdOdometry with Xtion

[wongfei]

Hi, i'm trying to integrate the library with OpenNI and Xtion sensor, but compute always fail without any message, please advice the most critical places in lib code to debug.

	// in what units should be focus and CX/CY?
	float vals[] = {FOCUS_LENGTH, 0., CX, 0., FOCUS_LENGTH, CY, 0., 0., 1.};

	new rgbd::RgbdOdometry() // like in example
	
	// loop..
	
	auto depth = oni_sensor_->get_depth_fd();
	auto color = oni_sensor_->get_color_fd();
	
	cv::Mat depth_16u(depth.height, depth.width, CV_16U, (char*)depth.data);
	cv::Mat depth_image;
	depth_16u.convertTo(depth_image, CV_32FC1, 0.001f); // mm -> m

	cv::Mat color_image(color.height, color.width, CV_8UC3, (char*)color.data);
	cv::Mat gray_image;
	cvtColor(color_image, gray_image, cv::COLOR_RGB2GRAY);

	cv::Mat normalizeDepth;
	depth_image.convertTo(normalizeDepth, CV_8UC1, 255.0f / 4.0f);
	cv::imshow("x0", normalizeDepth);
	cv::imshow("x1", gray_image);
	
	bool res = odom_->compute(image0_, depth0_, cv::Mat(), gray_image, depth_image, cv::Mat(), Rt);

	// next time use current frame as source	
	depth_image.copyTo(depth0_.get);
	gray_image.copyTo(image0_.get);