tzutalin / opencv-rgbdodometry Goto Github PK

odom = std::make_shared<rgbd::RgbdICPOdometry>(
          cameraMatrix, MIN_DEPTH, MAX_DEPTH, MAX_DEPTH_DIFF, MAX_POINTS_PART, iterCounts,
          minGradMagnitudes, rgbd::Odometry::RIGID_BODY_MOTION);
      /* 
      odom = std::make_shared<rgbd::ICPOdometry>(
          cameraMatrix, MIN_DEPTH, MAX_DEPTH, MAX_DEPTH_DIFF, MAX_POINTS_PART, iterCounts,
          rgbd::Odometry::RIGID_BODY_MOTION);
      */

      Could this rgbdOdometry run on KITTI dataset? thanks a lot!

	// 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);