utest_equi.cpp
Go to the documentation of this file.
3 #include <gtest/gtest.h>
4 
10 
11 class EquidistantTest : public testing::Test
12 {
13 protected:
14  virtual void SetUp()
15  {
17  // These parameters are taken from a real camera calibration
18  double D[] = {-0.08857683871674071, 0.0708113094372378, -0.09127623055964429, 0.04006922269778478};
19  double K[] = {403.603063319358, 0.0, 306.15842863283063,
20  0.0, 403.7028851121003, 261.09715697592696,
21  0.0, 0.0, 1.0};
22  double R[] = {0.999963944103842, -0.008484152966323483, 0.00036005656766869323,
23  0.008484153516269438, 0.9999640089218772, 0.0,
24  -0.0003600436088446379, 3.0547751946422504e-06, 0.999999935179632};
25  double P[] = {347.2569964503485, 0.0, 350.5, 0.0,
26  0.0, 347.2569964503485, 256.0, 0.0,
27  0.0, 0.0, 1.0, 0.0};
28 
29  cam_info_.header.frame_id = "tf_frame";
30  cam_info_.height = 512;
31  cam_info_.width = 640;
32  // No ROI
33  cam_info_.D.resize(4);
34  std::copy(D, D+4, 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(EquidistantTest, 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(EquidistantTest, 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.72136775518018115, xyz.x, 1e-8);
63  EXPECT_NEAR(-0.449235009214005, 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(EquidistantTest, 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(135.45747375488281, uv_rect.x);
97  EXPECT_DOUBLE_EQ(84.945091247558594, 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 is accurate.
121  const size_t step = 5;
122  for (size_t row = 0; row <= cam_info_.height; row += step) {
123  for (size_t col = 0; col <= cam_info_.width; col += step) {
124  cv::Point2d uv_raw(row, col), uv_rect, uv_unrect;
125  uv_rect = model_.rectifyPoint(uv_raw);
126  uv_unrect = model_.unrectifyPoint(uv_rect);
127  EXPECT_NEAR(uv_raw.x, uv_unrect.x, 0.01);
128  EXPECT_NEAR(uv_raw.y, uv_unrect.y, 0.01);
129  }
130  }
131 }
132 
134 {
135  double u = 100.0, v = 200.0, du = 17.0, dv = 23.0, Z = 2.0;
136  cv::Point2d uv0(u, v), uv1(u + du, v + dv);
137  cv::Point3d xyz0, xyz1;
138  xyz0 = model_.projectPixelTo3dRay(uv0);
139  xyz0 *= (Z / xyz0.z);
140  xyz1 = model_.projectPixelTo3dRay(uv1);
141  xyz1 *= (Z / xyz1.z);
142 
143  EXPECT_NEAR(model_.getDeltaU(xyz1.x - xyz0.x, Z), du, 1e-4);
144  EXPECT_NEAR(model_.getDeltaV(xyz1.y - xyz0.y, Z), dv, 1e-4);
145  EXPECT_NEAR(model_.getDeltaX(du, Z), xyz1.x - xyz0.x, 1e-4);
146  EXPECT_NEAR(model_.getDeltaY(dv, Z), xyz1.y - xyz0.y, 1e-4);
147 }
148 
149 TEST_F(EquidistantTest, initialization)
150 {
151 
152  sensor_msgs::CameraInfo info;
154 
155  camera.fromCameraInfo(info);
156 
157  EXPECT_EQ(camera.initialized(), 1);
158  EXPECT_EQ(camera.projectionMatrix().rows, 3);
159  EXPECT_EQ(camera.projectionMatrix().cols, 4);
160 }
161 
162 TEST_F(EquidistantTest, rectifyIfCalibrated)
163 {
165  // Ideally this test would have two images stored on disk
166  // one which is distorted and the other which is rectified,
167  // and then rectification would take place here and the output
168  // image compared to the one on disk (which would mean if
169  // the distortion coefficients above can't change once paired with
170  // an image).
171 
172  // Later could incorporate distort code
173  // (https://github.com/lucasw/vimjay/blob/master/src/standalone/distort_image.cpp)
174  // to take any image distort it, then undistort with rectifyImage,
175  // and given the distortion coefficients are consistent the input image
176  // and final output image should be mostly the same (though some
177  // interpolation error
178  // creeps in), except for outside a masked region where information was lost.
179  // The masked region can be generated with a pure white image that
180  // goes through the same process (if it comes out completely black
181  // then the distortion parameters are problematic).
182 
183  // For now generate an image and pass the test simply if
184  // the rectified image does not match the distorted image.
185  // Then zero out the first distortion coefficient and run
186  // the test again.
187  // Then zero out all the distortion coefficients and test
188  // that the output image is the same as the input.
189  cv::Mat distorted_image(cv::Size(cam_info_.width, cam_info_.height), CV_8UC3, cv::Scalar(0, 0, 0));
190 
191  // draw a grid
192  const cv::Scalar color = cv::Scalar(255, 255, 255);
193  // draw the lines thick so the proportion of error due to
194  // interpolation is reduced
195  const int thickness = 7;
196  const int type = 8;
197  for (size_t y = 0; y <= cam_info_.height; y += cam_info_.height/10)
198  {
199  cv::line(distorted_image,
200  cv::Point(0, y), cv::Point(cam_info_.width, y),
201  color, type, thickness);
202  }
203  for (size_t x = 0; x <= cam_info_.width; x += cam_info_.width/10)
204  {
205  // draw the lines thick so the prorportion of interpolation error is reduced
206  cv::line(distorted_image,
207  cv::Point(x, 0), cv::Point(x, cam_info_.height),
208  color, type, thickness);
209  }
210 
211  cv::Mat rectified_image;
212  // Just making this number up, maybe ought to be larger
213  // since a completely different image would be on the order of
214  // width * height * 255 = 78e6
215  const double diff_threshold = 10000.0;
216  double error;
217 
218  // Test that rectified image is sufficiently different
219  // using default distortion
220  model_.rectifyImage(distorted_image, rectified_image);
221  error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
222  // Just making this number up, maybe ought to be larger
223  EXPECT_GT(error, diff_threshold);
224 
225  // Test that rectified image is sufficiently different
226  // using default distortion but with first element zeroed
227  // out.
228  sensor_msgs::CameraInfo cam_info_2 = cam_info_;
229  cam_info_2.D[0] = 0.0;
230  model_.fromCameraInfo(cam_info_2);
231  model_.rectifyImage(distorted_image, rectified_image);
232  error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
233  EXPECT_GT(error, diff_threshold);
234 
235  // Test that rectified image is the same using zero distortion
236  cam_info_2.D.assign(cam_info_2.D.size(), 0);
237  model_.fromCameraInfo(cam_info_2);
238  model_.rectifyImage(distorted_image, rectified_image);
239  error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
240  EXPECT_EQ(error, 0);
241 
242  // Test that rectified image is the same using empty distortion
243  cam_info_2.D.clear();
244  model_.fromCameraInfo(cam_info_2);
245  model_.rectifyImage(distorted_image, rectified_image);
246  error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
247  EXPECT_EQ(error, 0);
248 
249  // restore original distortion
250  model_.fromCameraInfo(cam_info_);
251 }
252 
253 int main(int argc, char** argv)
254 {
255  testing::InitGoogleTest(&argc, argv);
256  return RUN_ALL_TESTS();
257 }
distortion_models.h
sensor_msgs::distortion_models::EQUIDISTANT
const std::string EQUIDISTANT
pinhole_camera_model.h
image_geometry::PinholeCameraModel
Simplifies interpreting images geometrically using the parameters from sensor_msgs/CameraInfo.
Definition: pinhole_camera_model.h:24
EquidistantTest
Definition: utest_equi.cpp:11
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
EquidistantTest::model_
image_geometry::PinholeCameraModel model_
Definition: utest_equi.cpp:44
main
int main(int argc, char **argv)
Definition: utest_equi.cpp:253
EquidistantTest::SetUp
virtual void SetUp()
Definition: utest_equi.cpp:14
EquidistantTest::cam_info_
sensor_msgs::CameraInfo cam_info_
Definition: utest_equi.cpp:43
image_geometry::PinholeCameraModel::initialized
bool initialized() const
Returns true if the camera has been initialized.
Definition: pinhole_camera_model.h:275
TEST_F
TEST_F(EquidistantTest, accessorsCorrect)
Definition: utest_equi.cpp:47
image_geometry::PinholeCameraModel::projectionMatrix
const cv::Matx34d & projectionMatrix() const
Returns the projection matrix.
Definition: pinhole_camera_model.h:318


image_geometry
Author(s): Patrick Mihelich
autogenerated on Wed Aug 21 2024 02:47:03