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
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
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
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
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
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
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
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
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
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
00121 const size_t step = 5;
00122 const size_t border = 65;
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
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 int main(int argc, char** argv)
00165 {
00166 testing::InitGoogleTest(&argc, argv);
00167 return RUN_ALL_TESTS();
00168 }