utest.cpp
Go to the documentation of this file.
00001 #include "image_geometry/pinhole_camera_model.h"
00002 #include <sensor_msgs/distortion_models.h>
00003 #include <gtest/gtest.h>
00004 
00010 
00011 class PinholeTest : public testing::Test
00012 {
00013 protected:
00014   virtual void SetUp()
00015   {
00017     // These parameters taken from a real camera calibration
00018     double D[] = {-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0};
00019     double K[] = {430.15433020105519,                0.0, 311.71339830549732,
00020                                  0.0, 430.60920415473657, 221.06824942698509,
00021                                  0.0,                0.0,                1.0};
00022     double R[] = {0.99806560714807102, 0.0068562422224214027, 0.061790256276695904,
00023                   -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664,
00024                   -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516};
00025     double P[] = {295.53402059708782, 0.0, 285.55760765075684, 0.0,
00026                   0.0, 295.53402059708782, 223.29617881774902, 0.0,
00027                   0.0, 0.0, 1.0, 0.0};
00028 
00029     cam_info_.header.frame_id = "tf_frame";
00030     cam_info_.height = 480;
00031     cam_info_.width  = 640;
00032     // No ROI
00033     cam_info_.D.resize(5);
00034     std::copy(D, D+5, cam_info_.D.begin());
00035     std::copy(K, K+9, cam_info_.K.begin());
00036     std::copy(R, R+9, cam_info_.R.begin());
00037     std::copy(P, P+12, cam_info_.P.begin());
00038     cam_info_.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00039 
00040     model_.fromCameraInfo(cam_info_);
00041   }
00042   
00043   sensor_msgs::CameraInfo cam_info_;
00044   image_geometry::PinholeCameraModel model_;
00045 };
00046 
00047 TEST_F(PinholeTest, accessorsCorrect)
00048 {
00049   EXPECT_STREQ("tf_frame", model_.tfFrame().c_str());
00050   EXPECT_EQ(cam_info_.P[0], model_.fx());
00051   EXPECT_EQ(cam_info_.P[5], model_.fy());
00052   EXPECT_EQ(cam_info_.P[2], model_.cx());
00053   EXPECT_EQ(cam_info_.P[6], model_.cy());
00054 }
00055 
00056 TEST_F(PinholeTest, projectPoint)
00057 {
00058   // Spot test an arbitrary point.
00059   {
00060     cv::Point2d uv(100, 100);
00061     cv::Point3d xyz =  model_.projectPixelTo3dRay(uv);
00062     EXPECT_NEAR(-0.62787224048135637, xyz.x, 1e-8);
00063     EXPECT_NEAR(-0.41719792045817677, xyz.y, 1e-8);
00064     EXPECT_DOUBLE_EQ(1.0, xyz.z);
00065   }
00066 
00067   // Principal point should project straight out.
00068   {
00069     cv::Point2d uv(model_.cx(), model_.cy());
00070     cv::Point3d xyz = model_.projectPixelTo3dRay(uv);
00071     EXPECT_DOUBLE_EQ(0.0, xyz.x);
00072     EXPECT_DOUBLE_EQ(0.0, xyz.y);
00073     EXPECT_DOUBLE_EQ(1.0, xyz.z);
00074   }
00075   
00076   // Check projecting to 3d and back over entire image is accurate.
00077   const size_t step = 10;
00078   for (size_t row = 0; row <= cam_info_.height; row += step) {
00079     for (size_t col = 0; col <= cam_info_.width; col += step) {
00080       cv::Point2d uv(row, col), uv_back;
00081       cv::Point3d xyz = model_.projectPixelTo3dRay(uv);
00082       uv_back = model_.project3dToPixel(xyz);
00083       // Measured max error at 1.13687e-13
00084       EXPECT_NEAR(uv.x, uv_back.x, 1.14e-13) << "at (" << row << ", " << col << ")";
00085       EXPECT_NEAR(uv.y, uv_back.y, 1.14e-13) << "at (" << row << ", " << col << ")";
00086     }
00087   }
00088 }
00089 
00090 TEST_F(PinholeTest, rectifyPoint)
00091 {
00092   // Spot test an arbitrary point.
00093   {
00094     cv::Point2d uv_raw(100, 100), uv_rect;
00095     uv_rect = model_.rectifyPoint(uv_raw);
00096     EXPECT_DOUBLE_EQ(142.30311584472656, uv_rect.x);
00097     EXPECT_DOUBLE_EQ(132.061065673828, uv_rect.y);
00098   }
00099 
00101 #if 0
00102   // Test rectifyPoint takes (c'x, c'y) [from K] -> (cx, cy) [from P].
00103   double cxp = model_.intrinsicMatrix()(0,2), cyp = model_.intrinsicMatrix()(1,2);
00104   {
00105     cv::Point2d uv_raw(cxp, cyp), uv_rect;
00106     model_.rectifyPoint(uv_raw, uv_rect);
00107     EXPECT_NEAR(uv_rect.x, model_.cx(), 1e-4);
00108     EXPECT_NEAR(uv_rect.y, model_.cy(), 1e-4);
00109   }
00110 
00111   // Test unrectifyPoint takes (cx, cy) [from P] -> (c'x, c'y) [from K].
00112   {
00113     cv::Point2d uv_rect(model_.cx(), model_.cy()), uv_raw;
00114     model_.unrectifyPoint(uv_rect, uv_raw);
00115     EXPECT_NEAR(uv_raw.x, cxp, 1e-4);
00116     EXPECT_NEAR(uv_raw.y, cyp, 1e-4);
00117   }
00118 #endif
00119 
00120   // Check rectifying then unrectifying over most of the image is accurate.
00121   const size_t step = 5;
00122   const size_t border = 65; // Expect bad accuracy far from the center of the image.
00123   for (size_t row = border; row <= cam_info_.height - border; row += step) {
00124     for (size_t col = border; col <= cam_info_.width - border; col += step) {
00125       cv::Point2d uv_raw(row, col), uv_rect, uv_unrect;
00126       uv_rect = model_.rectifyPoint(uv_raw);
00127       uv_unrect = model_.unrectifyPoint(uv_rect);
00128       // Check that we're at least within a pixel...
00129       EXPECT_NEAR(uv_raw.x, uv_unrect.x, 1.0);
00130       EXPECT_NEAR(uv_raw.y, uv_unrect.y, 1.0);
00131     }
00132   }
00133 }
00134 
00135 TEST_F(PinholeTest, getDeltas)
00136 {
00137   double u = 100.0, v = 200.0, du = 17.0, dv = 23.0, Z = 2.0;
00138   cv::Point2d uv0(u, v), uv1(u + du, v + dv);
00139   cv::Point3d xyz0, xyz1;
00140   xyz0 = model_.projectPixelTo3dRay(uv0);
00141   xyz0 *= (Z / xyz0.z);
00142   xyz1 = model_.projectPixelTo3dRay(uv1);
00143   xyz1 *= (Z / xyz1.z);
00144 
00145   EXPECT_NEAR(model_.getDeltaU(xyz1.x - xyz0.x, Z), du, 1e-4);
00146   EXPECT_NEAR(model_.getDeltaV(xyz1.y - xyz0.y, Z), dv, 1e-4);
00147   EXPECT_NEAR(model_.getDeltaX(du, Z), xyz1.x - xyz0.x, 1e-4);
00148   EXPECT_NEAR(model_.getDeltaY(dv, Z), xyz1.y - xyz0.y, 1e-4);
00149 }
00150 
00151 TEST_F(PinholeTest, initialization)
00152 {
00153 
00154     sensor_msgs::CameraInfo info;
00155     image_geometry::PinholeCameraModel camera;
00156 
00157     camera.fromCameraInfo(info);
00158 
00159     EXPECT_EQ(camera.initialized(), 1);
00160     EXPECT_EQ(camera.projectionMatrix().rows, 3);
00161     EXPECT_EQ(camera.projectionMatrix().cols, 4);
00162 }
00163 
00164 TEST_F(PinholeTest, rectifyIfCalibrated)
00165 {
00167   // Ideally this test would have two images stored on disk
00168   // one which is distorted and the other which is rectified,
00169   // and then rectification would take place here and the output
00170   // image compared to the one on disk (which would mean if
00171   // the distortion coefficients above can't change once paired with
00172   // an image).
00173 
00174   // Later could incorporate distort code
00175   // (https://github.com/lucasw/vimjay/blob/master/src/standalone/distort_image.cpp)
00176   // to take any image distort it, then undistort with rectifyImage,
00177   // and given the distortion coefficients are consistent the input image
00178   // and final output image should be mostly the same (though some
00179   // interpolation error
00180   // creeps in), except for outside a masked region where information was lost.
00181   // The masked region can be generated with a pure white image that
00182   // goes through the same process (if it comes out completely black
00183   // then the distortion parameters are problematic).
00184 
00185   // For now generate an image and pass the test simply if
00186   // the rectified image does not match the distorted image.
00187   // Then zero out the first distortion coefficient and run
00188   // the test again.
00189   // Then zero out all the distortion coefficients and test
00190   // that the output image is the same as the input.
00191   cv::Mat distorted_image(cv::Size(cam_info_.width, cam_info_.height), CV_8UC3, cv::Scalar(0, 0, 0));
00192 
00193   // draw a grid
00194   const cv::Scalar color = cv::Scalar(255, 255, 255);
00195   // draw the lines thick so the proportion of error due to
00196   // interpolation is reduced
00197   const int thickness = 7;
00198   const int type = 8;
00199   for (size_t y = 0; y <= cam_info_.height; y += cam_info_.height/10)
00200   {
00201     cv::line(distorted_image,
00202              cv::Point(0, y), cv::Point(cam_info_.width, y),
00203              color, type, thickness);
00204   }
00205   for (size_t x = 0; x <= cam_info_.width; x += cam_info_.width/10)
00206   {
00207     // draw the lines thick so the prorportion of interpolation error is reduced
00208     cv::line(distorted_image,
00209              cv::Point(x, 0), cv::Point(x, cam_info_.height),
00210              color, type, thickness);
00211   }
00212 
00213   cv::Mat rectified_image;
00214   // Just making this number up, maybe ought to be larger
00215   // since a completely different image would be on the order of
00216   // width * height * 255 = 78e6
00217   const double diff_threshold = 10000.0;
00218   double error;
00219 
00220   // Test that rectified image is sufficiently different
00221   // using default distortion
00222   model_.rectifyImage(distorted_image, rectified_image);
00223   error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
00224   // Just making this number up, maybe ought to be larger
00225   EXPECT_GT(error, diff_threshold);
00226 
00227   // Test that rectified image is sufficiently different
00228   // using default distortion but with first element zeroed
00229   // out.
00230   sensor_msgs::CameraInfo cam_info_2 = cam_info_;
00231   cam_info_2.D[0] = 0.0;
00232   model_.fromCameraInfo(cam_info_2);
00233   model_.rectifyImage(distorted_image, rectified_image);
00234   error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
00235   EXPECT_GT(error, diff_threshold);
00236 
00237   // Test that rectified image is the same using zero distortion
00238   cam_info_2.D.assign(cam_info_2.D.size(), 0);
00239   model_.fromCameraInfo(cam_info_2);
00240   model_.rectifyImage(distorted_image, rectified_image);
00241   error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
00242   EXPECT_EQ(error, 0);
00243 
00244   // Test that rectified image is the same using empty distortion
00245   cam_info_2.D.clear();
00246   model_.fromCameraInfo(cam_info_2);
00247   model_.rectifyImage(distorted_image, rectified_image);
00248   error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
00249   EXPECT_EQ(error, 0);
00250 
00251   // restore original distortion
00252   model_.fromCameraInfo(cam_info_);
00253 }
00254 
00255 int main(int argc, char** argv)
00256 {
00257   testing::InitGoogleTest(&argc, argv);
00258   return RUN_ALL_TESTS();
00259 }


image_geometry
Author(s): Patrick Mihelich
autogenerated on Thu Jun 6 2019 21:23:35