From 15ddb068988557e95d298e34c78a8f98c9115923 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Martin=20G=C3=BCnther?= Date: Tue, 23 Nov 2021 18:51:14 +0100 Subject: [PATCH] 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 | 11 +-- image_geometry/test/utest.cpp | 93 ++++++++++++++++++++- 2 files changed, 96 insertions(+), 8 deletions(-) diff --git a/image_geometry/src/pinhole_camera_model.cpp b/image_geometry/src/pinhole_camera_model.cpp index 78155000e..b60b2dd64 100644 --- a/image_geometry/src/pinhole_camera_model.cpp +++ b/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))); diff --git a/image_geometry/test/utest.cpp b/image_geometry/test/utest.cpp index 428d9e462..ee6eab3b3 100644 --- a/image_geometry/test/utest.cpp +++ b/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);