utest.cpp
Go to the documentation of this file.
3 #include <gtest/gtest.h>
4 
10 
11 class PinholeTest : public testing::Test
12 {
13 protected:
14  virtual void SetUp()
15  {
17  // These parameters taken from a real camera calibration
18  double D[] = {-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0};
19  double K[] = {430.15433020105519, 0.0, 311.71339830549732,
20  0.0, 430.60920415473657, 221.06824942698509,
21  0.0, 0.0, 1.0};
22  double R[] = {0.99806560714807102, 0.0068562422224214027, 0.061790256276695904,
23  -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664,
24  -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516};
25  double P[] = {295.53402059708782, 0.0, 285.55760765075684, 0.0,
26  0.0, 295.53402059708782, 223.29617881774902, 0.0,
27  0.0, 0.0, 1.0, 0.0};
28 
29  cam_info_.header.frame_id = "tf_frame";
30  cam_info_.height = 480;
31  cam_info_.width = 640;
32  // No ROI
33  cam_info_.D.resize(5);
34  std::copy(D, D+5, cam_info_.D.begin());
35  std::copy(K, K+9, cam_info_.K.begin());
36  std::copy(R, R+9, cam_info_.R.begin());
37  std::copy(P, P+12, cam_info_.P.begin());
39 
41  }
42 
43  sensor_msgs::CameraInfo cam_info_;
45 };
46 
47 TEST_F(PinholeTest, accessorsCorrect)
48 {
49  EXPECT_STREQ("tf_frame", model_.tfFrame().c_str());
50  EXPECT_EQ(cam_info_.P[0], model_.fx());
51  EXPECT_EQ(cam_info_.P[5], model_.fy());
52  EXPECT_EQ(cam_info_.P[2], model_.cx());
53  EXPECT_EQ(cam_info_.P[6], model_.cy());
54 }
55 
56 TEST_F(PinholeTest, projectPoint)
57 {
58  // Spot test an arbitrary point.
59  {
60  cv::Point2d uv(100, 100);
61  cv::Point3d xyz = model_.projectPixelTo3dRay(uv);
62  EXPECT_NEAR(-0.62787224048135637, xyz.x, 1e-8);
63  EXPECT_NEAR(-0.41719792045817677, xyz.y, 1e-8);
64  EXPECT_DOUBLE_EQ(1.0, xyz.z);
65  }
66 
67  // Principal point should project straight out.
68  {
69  cv::Point2d uv(model_.cx(), model_.cy());
70  cv::Point3d xyz = model_.projectPixelTo3dRay(uv);
71  EXPECT_DOUBLE_EQ(0.0, xyz.x);
72  EXPECT_DOUBLE_EQ(0.0, xyz.y);
73  EXPECT_DOUBLE_EQ(1.0, xyz.z);
74  }
75 
76  // Check projecting to 3d and back over entire image is accurate.
77  const size_t step = 10;
78  for (size_t row = 0; row <= cam_info_.height; row += step) {
79  for (size_t col = 0; col <= cam_info_.width; col += step) {
80  cv::Point2d uv(row, col), uv_back;
81  cv::Point3d xyz = model_.projectPixelTo3dRay(uv);
82  uv_back = model_.project3dToPixel(xyz);
83  // Measured max error at 1.13687e-13
84  EXPECT_NEAR(uv.x, uv_back.x, 1.14e-13) << "at (" << row << ", " << col << ")";
85  EXPECT_NEAR(uv.y, uv_back.y, 1.14e-13) << "at (" << row << ", " << col << ")";
86  }
87  }
88 }
89 
90 TEST_F(PinholeTest, rectifyPoint)
91 {
92  // Spot test an arbitrary point.
93  {
94  cv::Point2d uv_raw(100, 100), uv_rect;
95  uv_rect = model_.rectifyPoint(uv_raw);
96  EXPECT_DOUBLE_EQ(142.30311584472656, uv_rect.x);
97  EXPECT_DOUBLE_EQ(132.061065673828, uv_rect.y);
98  }
99 
101 #if 0
102  // Test rectifyPoint takes (c'x, c'y) [from K] -> (cx, cy) [from P].
103  double cxp = model_.intrinsicMatrix()(0,2), cyp = model_.intrinsicMatrix()(1,2);
104  {
105  cv::Point2d uv_raw(cxp, cyp), uv_rect;
106  model_.rectifyPoint(uv_raw, uv_rect);
107  EXPECT_NEAR(uv_rect.x, model_.cx(), 1e-4);
108  EXPECT_NEAR(uv_rect.y, model_.cy(), 1e-4);
109  }
110 
111  // Test unrectifyPoint takes (cx, cy) [from P] -> (c'x, c'y) [from K].
112  {
113  cv::Point2d uv_rect(model_.cx(), model_.cy()), uv_raw;
114  model_.unrectifyPoint(uv_rect, uv_raw);
115  EXPECT_NEAR(uv_raw.x, cxp, 1e-4);
116  EXPECT_NEAR(uv_raw.y, cyp, 1e-4);
117  }
118 #endif
119 
120  // Check rectifying then unrectifying over most of the image is accurate.
121  const size_t step = 5;
122  const size_t border = 65; // Expect bad accuracy far from the center of the image.
123  for (size_t row = border; row <= cam_info_.height - border; row += step) {
124  for (size_t col = border; col <= cam_info_.width - border; col += step) {
125  cv::Point2d uv_raw(row, col), uv_rect, uv_unrect;
126  uv_rect = model_.rectifyPoint(uv_raw);
127  uv_unrect = model_.unrectifyPoint(uv_rect);
128  // Check that we're at least within a pixel...
129  EXPECT_NEAR(uv_raw.x, uv_unrect.x, 1.0);
130  EXPECT_NEAR(uv_raw.y, uv_unrect.y, 1.0);
131  }
132  }
133 }
134 
135 TEST_F(PinholeTest, getDeltas)
136 {
137  double u = 100.0, v = 200.0, du = 17.0, dv = 23.0, Z = 2.0;
138  cv::Point2d uv0(u, v), uv1(u + du, v + dv);
139  cv::Point3d xyz0, xyz1;
140  xyz0 = model_.projectPixelTo3dRay(uv0);
141  xyz0 *= (Z / xyz0.z);
142  xyz1 = model_.projectPixelTo3dRay(uv1);
143  xyz1 *= (Z / xyz1.z);
144 
145  EXPECT_NEAR(model_.getDeltaU(xyz1.x - xyz0.x, Z), du, 1e-4);
146  EXPECT_NEAR(model_.getDeltaV(xyz1.y - xyz0.y, Z), dv, 1e-4);
147  EXPECT_NEAR(model_.getDeltaX(du, Z), xyz1.x - xyz0.x, 1e-4);
148  EXPECT_NEAR(model_.getDeltaY(dv, Z), xyz1.y - xyz0.y, 1e-4);
149 }
150 
151 TEST_F(PinholeTest, initialization)
152 {
153 
154  sensor_msgs::CameraInfo info;
156 
157  camera.fromCameraInfo(info);
158 
159  EXPECT_EQ(camera.initialized(), 1);
160  EXPECT_EQ(camera.projectionMatrix().rows, 3);
161  EXPECT_EQ(camera.projectionMatrix().cols, 4);
162 }
163 
164 TEST_F(PinholeTest, rectifyIfCalibrated)
165 {
167  // Ideally this test would have two images stored on disk
168  // one which is distorted and the other which is rectified,
169  // and then rectification would take place here and the output
170  // image compared to the one on disk (which would mean if
171  // the distortion coefficients above can't change once paired with
172  // an image).
173 
174  // Later could incorporate distort code
175  // (https://github.com/lucasw/vimjay/blob/master/src/standalone/distort_image.cpp)
176  // to take any image distort it, then undistort with rectifyImage,
177  // and given the distortion coefficients are consistent the input image
178  // and final output image should be mostly the same (though some
179  // interpolation error
180  // creeps in), except for outside a masked region where information was lost.
181  // The masked region can be generated with a pure white image that
182  // goes through the same process (if it comes out completely black
183  // then the distortion parameters are problematic).
184 
185  // For now generate an image and pass the test simply if
186  // the rectified image does not match the distorted image.
187  // Then zero out the first distortion coefficient and run
188  // the test again.
189  // Then zero out all the distortion coefficients and test
190  // that the output image is the same as the input.
191  cv::Mat distorted_image(cv::Size(cam_info_.width, cam_info_.height), CV_8UC3, cv::Scalar(0, 0, 0));
192 
193  // draw a grid
194  const cv::Scalar color = cv::Scalar(255, 255, 255);
195  // draw the lines thick so the proportion of error due to
196  // interpolation is reduced
197  const int thickness = 7;
198  const int type = 8;
199  for (size_t y = 0; y <= cam_info_.height; y += cam_info_.height/10)
200  {
201  cv::line(distorted_image,
202  cv::Point(0, y), cv::Point(cam_info_.width, y),
203  color, type, thickness);
204  }
205  for (size_t x = 0; x <= cam_info_.width; x += cam_info_.width/10)
206  {
207  // draw the lines thick so the prorportion of interpolation error is reduced
208  cv::line(distorted_image,
209  cv::Point(x, 0), cv::Point(x, cam_info_.height),
210  color, type, thickness);
211  }
212 
213  cv::Mat rectified_image;
214  // Just making this number up, maybe ought to be larger
215  // since a completely different image would be on the order of
216  // width * height * 255 = 78e6
217  const double diff_threshold = 10000.0;
218  double error;
219 
220  // Test that rectified image is sufficiently different
221  // using default distortion
222  model_.rectifyImage(distorted_image, rectified_image);
223  error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
224  // Just making this number up, maybe ought to be larger
225  EXPECT_GT(error, diff_threshold);
226 
227  // Test that rectified image is sufficiently different
228  // using default distortion but with first element zeroed
229  // out.
230  sensor_msgs::CameraInfo cam_info_2 = cam_info_;
231  cam_info_2.D[0] = 0.0;
232  model_.fromCameraInfo(cam_info_2);
233  model_.rectifyImage(distorted_image, rectified_image);
234  error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
235  EXPECT_GT(error, diff_threshold);
236 
237  // Test that rectified image is the same using zero distortion
238  cam_info_2.D.assign(cam_info_2.D.size(), 0);
239  model_.fromCameraInfo(cam_info_2);
240  model_.rectifyImage(distorted_image, rectified_image);
241  error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
242  EXPECT_EQ(error, 0);
243 
244  // Test that rectified image is the same using empty distortion
245  cam_info_2.D.clear();
246  model_.fromCameraInfo(cam_info_2);
247  model_.rectifyImage(distorted_image, rectified_image);
248  error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
249  EXPECT_EQ(error, 0);
250 }
251 
252 void testUnrectifyImage(const sensor_msgs::CameraInfo& cam_info, const image_geometry::PinholeCameraModel& model)
253 {
254  // test for unrectifyImage: call unrectifyImage, call unrectifyPoint in a loop, compare
255 
256  // prepare rectified_image
257  cv::Mat rectified_image(model.fullResolution(), CV_8UC3, cv::Scalar(0, 0, 0));
258 
259  // draw a grid
260  const cv::Scalar color = cv::Scalar(255, 255, 255);
261  const int thickness = 7;
262  const int type = 8;
263  for (size_t y = 0; y <= rectified_image.rows; y += rectified_image.rows / 10)
264  {
265  cv::line(rectified_image,
266  cv::Point(0, y), cv::Point(cam_info.width, y),
267  color, type, thickness);
268  }
269  for (size_t x = 0; x <= rectified_image.cols; x += rectified_image.cols / 10)
270  {
271  cv::line(rectified_image,
272  cv::Point(x, 0), cv::Point(x, cam_info.height),
273  color, type, thickness);
274  }
275 
276  // restrict rectified_image to ROI and resize to new binning
277  rectified_image = rectified_image(model.rawRoi());
278  cv::resize(rectified_image, rectified_image, cv::Size(), 1.0 / model.binningX(), 1.0 / model.binningY(),
279  cv::INTER_NEAREST);
280 
281  // unrectify image in one go using unrectifyImage
282  cv::Mat distorted_image;
283  // Just making this number up, maybe ought to be larger
284  // since a completely different image would be on the order of
285  // width * height * 255 = 78e6
286  const double diff_threshold = 10000.0;
287  double error;
288 
289  // Test that unrectified image is sufficiently different
290  // using default distortion
291  model.unrectifyImage(rectified_image, distorted_image);
292  error = cv::norm(rectified_image, distorted_image, cv::NORM_L1);
293  // Just making this number up, maybe ought to be larger
294  EXPECT_GT(error, diff_threshold);
295 
296  // unrectify image pixel by pixel using unrectifyPoint
297  assert(rectified_image.type() == CV_8UC3); // need this for at<cv::Vec3b> to be correct
298  cv::Mat distorted_image_by_pixel = cv::Mat::zeros(rectified_image.size(), rectified_image.type());
299  cv::Mat mask = cv::Mat::zeros(rectified_image.size(), CV_8UC1);
300  for (size_t y = 0; y < rectified_image.rows; y++)
301  {
302  for (size_t x = 0; x < rectified_image.cols; x++)
303  {
304  cv::Point2i uv_rect(x, y), uv_raw;
305 
306  uv_raw = model.unrectifyPoint(uv_rect);
307 
308  if (0 <= uv_raw.x && uv_raw.x < distorted_image_by_pixel.cols && 0 <= uv_raw.y
309  && uv_raw.y < distorted_image_by_pixel.rows)
310  {
311  distorted_image_by_pixel.at<cv::Vec3b>(uv_raw) = rectified_image.at<cv::Vec3b>(uv_rect);
312  mask.at<uchar>(uv_raw) = 255;
313  // Test that both methods produce similar values at the pixels that unrectifyPoint hits; don't test for all
314  // other pixels (the images will differ there, because unrectifyPoint doesn't interpolate missing pixels).
315  // Also don't check for absolute equality, but allow a color difference of up to 200. This still catches
316  // complete misses (color difference would be 255) while allowing for interpolation at the grid borders.
317  EXPECT_LT(distorted_image.at<cv::Vec3b>(uv_raw)[0] - distorted_image_by_pixel.at<cv::Vec3b>(uv_raw)[0], 200);
318  }
319  }
320  }
321 
322  // Test that absolute error (due to interpolation) is less than 6% of the maximum possible error
323  error = cv::norm(distorted_image, distorted_image_by_pixel, cv::NORM_L1, mask);
324  EXPECT_LT(error / (distorted_image.size[0] * distorted_image.size[1] * 255), 0.06);
325 
326  // Test that unrectifyPoint hits more than 50% of the output image
327  EXPECT_GT((double) cv::countNonZero(mask) / (distorted_image.size[0] * distorted_image.size[1]), 0.5);
328 };
329 
330 TEST_F(PinholeTest, unrectifyImage)
331 {
332  testUnrectifyImage(cam_info_, model_);
333 }
334 
335 TEST_F(PinholeTest, unrectifyImageWithBinning)
336 {
337  cam_info_.binning_x = 2;
338  cam_info_.binning_y = 2;
339  model_.fromCameraInfo(cam_info_);
340 
341  testUnrectifyImage(cam_info_, model_);
342 }
343 
344 TEST_F(PinholeTest, unrectifyImageWithRoi)
345 {
346  cam_info_.roi.x_offset = 100;
347  cam_info_.roi.y_offset = 50;
348  cam_info_.roi.width = 400;
349  cam_info_.roi.height = 300;
350  cam_info_.roi.do_rectify = true;
351  model_.fromCameraInfo(cam_info_);
352 
353  testUnrectifyImage(cam_info_, model_);
354 }
355 
356 TEST_F(PinholeTest, unrectifyImageWithBinningAndRoi)
357 {
358  cam_info_.binning_x = 2;
359  cam_info_.binning_y = 2;
360  cam_info_.roi.x_offset = 100;
361  cam_info_.roi.y_offset = 50;
362  cam_info_.roi.width = 400;
363  cam_info_.roi.height = 300;
364  cam_info_.roi.do_rectify = true;
365  model_.fromCameraInfo(cam_info_);
366 
367  testUnrectifyImage(cam_info_, model_);
368 }
369 
370 TEST_F(PinholeTest, rectifiedRoiSize) {
371 
372  cv::Rect rectified_roi = model_.rectifiedRoi();
373  cv::Size reduced_resolution = model_.reducedResolution();
374  EXPECT_EQ(0, rectified_roi.x);
375  EXPECT_EQ(0, rectified_roi.y);
376  EXPECT_EQ(640, rectified_roi.width);
377  EXPECT_EQ(480, rectified_roi.height);
378  EXPECT_EQ(640, reduced_resolution.width);
379  EXPECT_EQ(480, reduced_resolution.height);
380 
381  cam_info_.binning_x = 2;
382  cam_info_.binning_y = 2;
383  model_.fromCameraInfo(cam_info_);
384  rectified_roi = model_.rectifiedRoi();
385  reduced_resolution = model_.reducedResolution();
386  EXPECT_EQ(0, rectified_roi.x);
387  EXPECT_EQ(0, rectified_roi.y);
388  EXPECT_EQ(640, rectified_roi.width);
389  EXPECT_EQ(480, rectified_roi.height);
390  EXPECT_EQ(320, reduced_resolution.width);
391  EXPECT_EQ(240, reduced_resolution.height);
392 
393  cam_info_.binning_x = 1;
394  cam_info_.binning_y = 1;
395  cam_info_.roi.x_offset = 100;
396  cam_info_.roi.y_offset = 50;
397  cam_info_.roi.width = 400;
398  cam_info_.roi.height = 300;
399  cam_info_.roi.do_rectify = true;
400  model_.fromCameraInfo(cam_info_);
401  rectified_roi = model_.rectifiedRoi();
402  reduced_resolution = model_.reducedResolution();
403  EXPECT_EQ(137, rectified_roi.x);
404  EXPECT_EQ(82, rectified_roi.y);
405  EXPECT_EQ(321, rectified_roi.width);
406  EXPECT_EQ(242, rectified_roi.height);
407  EXPECT_EQ(321, reduced_resolution.width);
408  EXPECT_EQ(242, reduced_resolution.height);
409 
410  cam_info_.binning_x = 2;
411  cam_info_.binning_y = 2;
412  cam_info_.roi.x_offset = 100;
413  cam_info_.roi.y_offset = 50;
414  cam_info_.roi.width = 400;
415  cam_info_.roi.height = 300;
416  cam_info_.roi.do_rectify = true;
417  model_.fromCameraInfo(cam_info_);
418  rectified_roi = model_.rectifiedRoi();
419  reduced_resolution = model_.reducedResolution();
420  EXPECT_EQ(137, rectified_roi.x);
421  EXPECT_EQ(82, rectified_roi.y);
422  EXPECT_EQ(321, rectified_roi.width);
423  EXPECT_EQ(242, rectified_roi.height);
424  EXPECT_EQ(160, reduced_resolution.width);
425  EXPECT_EQ(121, reduced_resolution.height);
426 }
427 
428 TEST_F(PinholeTest, rectifiedRoiCaching)
429 {
430  // Test that the following sequence works correctly:
431  // 1. fromCameraInfo is called with ROI A. | rectified_roi_dirty = true
432  // (already happened in SetUp())
433 
434  // 2. rectifiedRoi is called | rectified_roi_dirty = false
435  cv::Rect actual_roi_a = model_.rectifiedRoi();
436  cv::Rect expected_roi_a(0, 0, 640, 480);
437  EXPECT_EQ(expected_roi_a, actual_roi_a);
438 
439  // 3. fromCameraInfo is called with ROI B. | rectified_roi_dirty = true
440  cam_info_.roi.x_offset = 100;
441  cam_info_.roi.y_offset = 50;
442  cam_info_.roi.width = 400;
443  cam_info_.roi.height = 300;
444  cam_info_.roi.do_rectify = true;
445  model_.fromCameraInfo(cam_info_);
446 
447  // 4. fromCameraInfo is called again with ROI B. | rectified_roi_dirty should still be true!
448  model_.fromCameraInfo(cam_info_);
449 
450  // 5. rectifiedRoi is called
451  // There was a bug before where rectified_roi_dirty was incorrectly set to `false` by step 4.
452  // If rectifiedRoi was called again, the cached rectified_roi for
453  // ROI A was returned, but it should be recalculated based on ROI B.
454  // This test checks that this behavior is correct.
455  cv::Rect actual_roi_b = model_.rectifiedRoi();
456  cv::Rect expected_roi_b(137, 82, 321, 242);
457  EXPECT_EQ(expected_roi_b, actual_roi_b);
458 }
459 
460 int main(int argc, char** argv)
461 {
462  testing::InitGoogleTest(&argc, argv);
463  return RUN_ALL_TESTS();
464 }
main
int main(int argc, char **argv)
Definition: utest.cpp:460
PinholeTest::model_
image_geometry::PinholeCameraModel model_
Definition: utest.cpp:44
image_geometry::PinholeCameraModel::binningX
uint32_t binningX() const
Returns the number of columns in each bin.
Definition: pinhole_camera_model.h:318
distortion_models.h
pinhole_camera_model.h
testUnrectifyImage
void testUnrectifyImage(const sensor_msgs::CameraInfo &cam_info, const image_geometry::PinholeCameraModel &model)
Definition: utest.cpp:252
image_geometry::PinholeCameraModel::binningY
uint32_t binningY() const
Returns the number of rows in each bin.
Definition: pinhole_camera_model.h:319
image_geometry::PinholeCameraModel
Simplifies interpreting images geometrically using the parameters from sensor_msgs/CameraInfo.
Definition: pinhole_camera_model.h:23
PinholeTest::SetUp
virtual void SetUp()
Definition: utest.cpp:14
image_geometry::PinholeCameraModel::fromCameraInfo
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
Set the camera parameters from the sensor_msgs/CameraInfo message.
Definition: pinhole_camera_model.cpp:97
PinholeTest::cam_info_
sensor_msgs::CameraInfo cam_info_
Definition: utest.cpp:43
image_geometry::PinholeCameraModel::fullResolution
cv::Size fullResolution() const
The resolution at which the camera was calibrated.
Definition: pinhole_camera_model.cpp:230
TEST_F
TEST_F(PinholeTest, accessorsCorrect)
Definition: utest.cpp:47
image_geometry::PinholeCameraModel::unrectifyImage
void unrectifyImage(const cv::Mat &rectified, cv::Mat &raw, int interpolation=cv::INTER_LINEAR) const
Apply camera distortion to a rectified image.
Definition: pinhole_camera_model.cpp:360
PinholeTest
Definition: utest.cpp:11
image_geometry::PinholeCameraModel::unrectifyPoint
cv::Point2d unrectifyPoint(const cv::Point2d &uv_rect) const
Compute the raw image coordinates of a pixel in the rectified image.
Definition: pinhole_camera_model.cpp:418
sensor_msgs::distortion_models::PLUMB_BOB
const std::string PLUMB_BOB
image_geometry::PinholeCameraModel::initialized
bool initialized() const
Returns true if the camera has been initialized.
Definition: pinhole_camera_model.h:264
image_geometry::PinholeCameraModel::rawRoi
cv::Rect rawRoi() const
The current raw ROI, as used for capture by the camera driver.
Definition: pinhole_camera_model.cpp:276
image_geometry::PinholeCameraModel::projectionMatrix
const cv::Matx34d & projectionMatrix() const
Returns the projection matrix.
Definition: pinhole_camera_model.h:307


image_geometry
Author(s): Patrick Mihelich
autogenerated on Wed Mar 2 2022 01:12:39