3 #include <gtest/gtest.h>
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,
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,
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());
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);
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);
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);
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 <<
")";
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);
103 double cxp = model_.intrinsicMatrix()(0,2), cyp = model_.intrinsicMatrix()(1,2);
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);
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);
121 const size_t step = 5;
122 const size_t border = 65;
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);
129 EXPECT_NEAR(uv_raw.x, uv_unrect.x, 1.0);
130 EXPECT_NEAR(uv_raw.y, uv_unrect.y, 1.0);
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);
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);
154 sensor_msgs::CameraInfo info;
191 cv::Mat distorted_image(cv::Size(cam_info_.width, cam_info_.height), CV_8UC3, cv::Scalar(0, 0, 0));
194 const cv::Scalar color = cv::Scalar(255, 255, 255);
197 const int thickness = 7;
199 for (
size_t y = 0; y <= cam_info_.height; y += cam_info_.height/10)
201 cv::line(distorted_image,
202 cv::Point(0, y), cv::Point(cam_info_.width, y),
203 color, type, thickness);
205 for (
size_t x = 0; x <= cam_info_.width; x += cam_info_.width/10)
208 cv::line(distorted_image,
209 cv::Point(x, 0), cv::Point(x, cam_info_.height),
210 color, type, thickness);
213 cv::Mat rectified_image;
217 const double diff_threshold = 10000.0;
222 model_.rectifyImage(distorted_image, rectified_image);
223 error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
225 EXPECT_GT(error, diff_threshold);
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);
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);
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);
257 cv::Mat rectified_image(model.
fullResolution(), CV_8UC3, cv::Scalar(0, 0, 0));
260 const cv::Scalar color = cv::Scalar(255, 255, 255);
261 const int thickness = 7;
263 for (
size_t y = 0; y <= rectified_image.rows; y += rectified_image.rows / 10)
265 cv::line(rectified_image,
266 cv::Point(0, y), cv::Point(cam_info.width, y),
267 color, type, thickness);
269 for (
size_t x = 0; x <= rectified_image.cols; x += rectified_image.cols / 10)
271 cv::line(rectified_image,
272 cv::Point(x, 0), cv::Point(x, cam_info.height),
273 color, type, thickness);
277 rectified_image = rectified_image(model.
rawRoi());
278 cv::resize(rectified_image, rectified_image, cv::Size(), 1.0 / model.
binningX(), 1.0 / model.
binningY(),
282 cv::Mat distorted_image;
286 const double diff_threshold = 10000.0;
292 error = cv::norm(rectified_image, distorted_image, cv::NORM_L1);
294 EXPECT_GT(error, diff_threshold);
297 assert(rectified_image.type() == CV_8UC3);
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++)
302 for (
size_t x = 0; x < rectified_image.cols; x++)
304 cv::Point2i uv_rect(x, y), uv_raw;
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)
311 distorted_image_by_pixel.at<cv::Vec3b>(uv_raw) = rectified_image.at<cv::Vec3b>(uv_rect);
312 mask.at<uchar>(uv_raw) = 255;
317 EXPECT_LT(distorted_image.at<cv::Vec3b>(uv_raw)[0] - distorted_image_by_pixel.at<cv::Vec3b>(uv_raw)[0], 200);
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);
327 EXPECT_GT((
double) cv::countNonZero(mask) / (distorted_image.size[0] * distorted_image.size[1]), 0.5);
337 cam_info_.binning_x = 2;
338 cam_info_.binning_y = 2;
339 model_.fromCameraInfo(cam_info_);
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_);
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_);
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);
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);
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);
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);
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);
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_);
448 model_.fromCameraInfo(cam_info_);
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);
460 int main(
int argc,
char** argv)
462 testing::InitGoogleTest(&argc, argv);
463 return RUN_ALL_TESTS();