Fix rectifyRoi when used with binning and/or ROI (#378)

* Fix rectifyRoi when used with binning and/or ROI

The previous implementation used K_ and P_ (the matrices that include
binning and ROI). However, the ROI is specified in unbinned and
untransformed raw image coordinates (see REP-104), so rectifyRoi has to
use K_full_ and P_full_.

* test: Remove useless restoring of original state

Each test runs in its own test fixture, so restoring the original state
at the end of a test is not necessary.

* Don't overwrite rectified_roi_dirty flag

This fixes a problem with the following sequence:

1. fromCameraInfo is called with ROI A.  | rectified_roi_dirty = true
2. rectifiedRoi is called                | rectified_roi_dirty = false
3. fromCameraInfo is called with ROI B.  | rectified_roi_dirty = true
4. fromCameraInfo is called with ROI B.  | rectified_roi_dirty = false

The bug is that rectified_roi_dirty was incorrectly set to `false` by the last
line. If now rectifiedRoi is called again, the cached rectified_roi for
ROI A will be returned, but it should be recalculated based on ROI B.

This commit fixes that.

image_geometry/src/pinhole_camera_model.cpp

@@ -147,7 +147,7 @@ bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)
   cache_->unrectify_reduced_maps_dirty |= cache_->unrectify_full_maps_dirty;
 
   // As is the rectified ROI
-  cache_->rectified_roi_dirty = reduced_dirty;
+  cache_->rectified_roi_dirty |= reduced_dirty;
 
   // Figure out how to handle the distortion
   if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
@@ -460,11 +460,12 @@ cv::Rect PinholeCameraModel::rectifyRoi(const cv::Rect& roi_raw) const
   /// @todo Actually implement "best fit" as described by REP 104.
 
   // For now, just unrectify the four corners and take the bounding box.
-  cv::Point2d rect_tl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y));
-  cv::Point2d rect_tr = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, roi_raw.y));
+  // Since ROI is specified in unbinned coordinates (see REP-104), this has to use K_full_ and P_full_.
+  cv::Point2d rect_tl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y), K_full_, P_full_);
+  cv::Point2d rect_tr = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width, roi_raw.y), K_full_, P_full_);
   cv::Point2d rect_br = rectifyPoint(cv::Point2d(roi_raw.x + roi_raw.width,
-                                                 roi_raw.y + roi_raw.height));
-  cv::Point2d rect_bl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y + roi_raw.height));
+                                                 roi_raw.y + roi_raw.height), K_full_, P_full_);
+  cv::Point2d rect_bl = rectifyPoint(cv::Point2d(roi_raw.x, roi_raw.y + roi_raw.height), K_full_, P_full_);
 
   cv::Point roi_tl(std::ceil (std::min(rect_tl.x, rect_bl.x)),
                    std::ceil (std::min(rect_tl.y, rect_tr.y)));

image_geometry/test/utest.cpp

@@ -247,9 +247,6 @@ TEST_F(PinholeTest, rectifyIfCalibrated)
   model_.rectifyImage(distorted_image, rectified_image);
   error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
   EXPECT_EQ(error, 0);
-
-  // restore original distortion
-  model_.fromCameraInfo(cam_info_);
 }
 
 void testUnrectifyImage(const sensor_msgs::CameraInfo& cam_info, const image_geometry::PinholeCameraModel& model)
@@ -370,6 +367,96 @@ TEST_F(PinholeTest, unrectifyImageWithBinningAndRoi)
   testUnrectifyImage(cam_info_, model_);
 }
 
+TEST_F(PinholeTest, rectifiedRoiSize) {
+
+  cv::Rect rectified_roi = model_.rectifiedRoi();
+  cv::Size reduced_resolution = model_.reducedResolution();
+  EXPECT_EQ(0, rectified_roi.x);
+  EXPECT_EQ(0, rectified_roi.y);
+  EXPECT_EQ(640, rectified_roi.width);
+  EXPECT_EQ(480, rectified_roi.height);
+  EXPECT_EQ(640, reduced_resolution.width);
+  EXPECT_EQ(480, reduced_resolution.height);
+
+  cam_info_.binning_x = 2;
+  cam_info_.binning_y = 2;
+  model_.fromCameraInfo(cam_info_);
+  rectified_roi = model_.rectifiedRoi();
+  reduced_resolution = model_.reducedResolution();
+  EXPECT_EQ(0, rectified_roi.x);
+  EXPECT_EQ(0, rectified_roi.y);
+  EXPECT_EQ(640, rectified_roi.width);
+  EXPECT_EQ(480, rectified_roi.height);
+  EXPECT_EQ(320, reduced_resolution.width);
+  EXPECT_EQ(240, reduced_resolution.height);
+
+  cam_info_.binning_x = 1;
+  cam_info_.binning_y = 1;
+  cam_info_.roi.x_offset = 100;
+  cam_info_.roi.y_offset = 50;
+  cam_info_.roi.width = 400;
+  cam_info_.roi.height = 300;
+  cam_info_.roi.do_rectify = true;
+  model_.fromCameraInfo(cam_info_);
+  rectified_roi = model_.rectifiedRoi();
+  reduced_resolution = model_.reducedResolution();
+  EXPECT_EQ(137, rectified_roi.x);
+  EXPECT_EQ(82, rectified_roi.y);
+  EXPECT_EQ(321, rectified_roi.width);
+  EXPECT_EQ(242, rectified_roi.height);
+  EXPECT_EQ(321, reduced_resolution.width);
+  EXPECT_EQ(242, reduced_resolution.height);
+
+  cam_info_.binning_x = 2;
+  cam_info_.binning_y = 2;
+  cam_info_.roi.x_offset = 100;
+  cam_info_.roi.y_offset = 50;
+  cam_info_.roi.width = 400;
+  cam_info_.roi.height = 300;
+  cam_info_.roi.do_rectify = true;
+  model_.fromCameraInfo(cam_info_);
+  rectified_roi = model_.rectifiedRoi();
+  reduced_resolution = model_.reducedResolution();
+  EXPECT_EQ(137, rectified_roi.x);
+  EXPECT_EQ(82, rectified_roi.y);
+  EXPECT_EQ(321, rectified_roi.width);
+  EXPECT_EQ(242, rectified_roi.height);
+  EXPECT_EQ(160, reduced_resolution.width);
+  EXPECT_EQ(121, reduced_resolution.height);
+}
+
+TEST_F(PinholeTest, rectifiedRoiCaching)
+{
+  // Test that the following sequence works correctly:
+  // 1. fromCameraInfo is called with ROI A.  | rectified_roi_dirty = true
+  // (already happened in SetUp())
+
+  // 2. rectifiedRoi is called                | rectified_roi_dirty = false
+  cv::Rect actual_roi_a = model_.rectifiedRoi();
+  cv::Rect expected_roi_a(0, 0, 640, 480);
+  EXPECT_EQ(expected_roi_a, actual_roi_a);
+
+  // 3. fromCameraInfo is called with ROI B.  | rectified_roi_dirty = true
+  cam_info_.roi.x_offset = 100;
+  cam_info_.roi.y_offset = 50;
+  cam_info_.roi.width = 400;
+  cam_info_.roi.height = 300;
+  cam_info_.roi.do_rectify = true;
+  model_.fromCameraInfo(cam_info_);
+
+  // 4. fromCameraInfo is called again with ROI B.  | rectified_roi_dirty should still be true!
+  model_.fromCameraInfo(cam_info_);
+
+  // 5. rectifiedRoi is called
+  // There was a bug before where rectified_roi_dirty was incorrectly set to `false` by step 4.
+  // If rectifiedRoi was called again, the cached rectified_roi for
+  // ROI A was returned, but it should be recalculated based on ROI B.
+  // This test checks that this behavior is correct.
+  cv::Rect actual_roi_b = model_.rectifiedRoi();
+  cv::Rect expected_roi_b(137, 82, 321, 242);
+  EXPECT_EQ(expected_roi_b, actual_roi_b);
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);