$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 #include <gtest/gtest.h> 00036 00037 00038 #include <laser_cb_detector/ConfigGoal.h> 00039 #include <laser_cb_detector/laser_cb_detector.h> 00040 #include <opencv/highgui.h> 00041 00042 using namespace laser_cb_detector; 00043 using namespace std; 00044 00045 static const double pix_eps_ = .6 ; 00046 static const double eps = 1e-6; 00047 00048 static const bool DEBUG=false; 00049 00050 00051 calibration_msgs::DenseLaserSnapshot getSnapshot(const string& filename) 00052 { 00053 IplImage* image; 00054 image = cvLoadImage(filename.c_str(), 0); // 0 -> Force image to grayscale 00055 EXPECT_TRUE(image) << "could not open image file [" << filename << "]" << endl; 00056 00057 00058 IplImage* float_image; 00059 float_image = cvCreateImage( cvSize(image->width, image->height), IPL_DEPTH_32F, 1); 00060 cvConvert(image, float_image); 00061 00062 calibration_msgs::DenseLaserSnapshot snapshot; 00063 snapshot.readings_per_scan = image->width; 00064 snapshot.num_scans = image->height; 00065 snapshot.intensities.resize(image->height * image->width); 00066 00067 for (int i=0; i<float_image->height; i++) 00068 { 00069 memcpy(&snapshot.intensities[i*snapshot.readings_per_scan], 00070 (float_image->imageData + i*float_image->widthStep), 00071 sizeof(float)*snapshot.readings_per_scan); 00072 } 00073 00074 if (DEBUG) 00075 { 00076 for (unsigned int i=0; i<snapshot.num_scans; i++) 00077 { 00078 for (unsigned int j=0; j<snapshot.readings_per_scan; j++) 00079 { 00080 if (snapshot.intensities[i*snapshot.readings_per_scan + j] < 100.0) 00081 printf(" "); 00082 else 00083 printf("X"); 00084 } 00085 printf("\n"); 00086 } 00087 } 00088 00089 cvReleaseImage(&float_image); 00090 cvReleaseImage(&image); 00091 00092 return snapshot; 00093 } 00094 00095 ConfigGoal config3x4() 00096 { 00097 ConfigGoal config; 00098 00099 config.num_x = 3; 00100 config.num_y = 4; 00101 config.spacing_x = 1.0; 00102 config.spacing_y = 1.0; 00103 00104 config.width_scaling = 1.0; 00105 config.height_scaling = 1.0; 00106 00107 config.min_intensity = 0.0; 00108 config.max_intensity = 100.0; 00109 00110 config.subpixel_window = 3; 00111 config.subpixel_zero_zone = 1; 00112 00113 config.flip_horizontal = 0; 00114 00115 return config; 00116 } 00117 00118 00119 void check3x4(const calibration_msgs::CalibrationPattern& result, double pix_eps, bool flipped) 00120 { 00121 EXPECT_TRUE(result.success); 00122 00123 00124 EXPECT_NEAR(result.object_points[0].x, 0.0, eps); 00125 EXPECT_NEAR(result.object_points[0].y, 0.0, eps); 00126 EXPECT_NEAR(result.object_points[0].z, 0.0, eps); 00127 00128 EXPECT_NEAR(result.object_points[2].x, 2.0, eps); 00129 EXPECT_NEAR(result.object_points[2].y, 0.0, eps); 00130 EXPECT_NEAR(result.object_points[2].z, 0.0, eps); 00131 00132 EXPECT_NEAR(result.object_points[5].x, 2.0, eps); 00133 EXPECT_NEAR(result.object_points[5].y, 1.0, eps); 00134 EXPECT_NEAR(result.object_points[5].z, 0.0, eps); 00135 00136 // Detected corners 00137 if (flipped) 00138 { 00139 EXPECT_NEAR(result.image_points[0].x, 81.5, pix_eps); 00140 EXPECT_NEAR(result.image_points[0].y, 72.0, pix_eps); 00141 00142 EXPECT_NEAR(result.image_points[1].x, 81.5, pix_eps); 00143 EXPECT_NEAR(result.image_points[1].y, 118.5, pix_eps); 00144 00145 EXPECT_NEAR(result.image_points[11].x, 223.5, pix_eps); 00146 EXPECT_NEAR(result.image_points[11].y, 166.5, pix_eps); 00147 } 00148 else 00149 { 00150 EXPECT_NEAR(result.image_points[0].x, 223.0, pix_eps); 00151 EXPECT_NEAR(result.image_points[0].y, 72.0, pix_eps); 00152 00153 EXPECT_NEAR(result.image_points[1].x, 223.0, pix_eps); 00154 EXPECT_NEAR(result.image_points[1].y, 119.0, pix_eps); 00155 00156 EXPECT_NEAR(result.image_points[11].x, 81.0, pix_eps); 00157 EXPECT_NEAR(result.image_points[11].y, 166.0, pix_eps); 00158 } 00159 } 00160 00161 TEST(LaserCbDetector, easy_cb_3x4) 00162 { 00163 LaserCbDetector detector; 00164 calibration_msgs::DenseLaserSnapshot snapshot; 00165 00166 snapshot = getSnapshot("test/data/cb_3x4.png"); 00167 00168 ASSERT_EQ(snapshot.readings_per_scan, (unsigned int) 303); 00169 ASSERT_EQ(snapshot.num_scans, (unsigned int) 325); 00170 00171 detector.configure(config3x4()); 00172 00173 bool return_result; 00174 calibration_msgs::CalibrationPattern result; 00175 return_result = detector.detect(snapshot, result); 00176 EXPECT_TRUE(return_result); 00177 00178 check3x4(result, pix_eps_, false); 00179 } 00180 00181 TEST(LaserCbDetector, scaled_cb_3x4) 00182 { 00183 LaserCbDetector detector; 00184 calibration_msgs::DenseLaserSnapshot snapshot; 00185 00186 snapshot = getSnapshot("test/data/cb_3x4.png"); 00187 00188 ASSERT_EQ(snapshot.readings_per_scan, (unsigned int) 303); 00189 ASSERT_EQ(snapshot.num_scans, (unsigned int) 325); 00190 00191 ConfigGoal config = config3x4(); 00192 config.width_scaling = 2.0; 00193 config.height_scaling = 4.0; 00194 detector.configure(config); 00195 00196 bool return_result; 00197 calibration_msgs::CalibrationPattern result; 00198 return_result = detector.detect(snapshot, result); 00199 EXPECT_TRUE(return_result); 00200 00202 check3x4(result, 1.0, false); 00203 } 00204 00205 TEST(LaserCbDetector, reflected_cb_3x4) 00206 { 00207 LaserCbDetector detector; 00208 calibration_msgs::DenseLaserSnapshot snapshot; 00209 00210 snapshot = getSnapshot("test/data/cb_3x4.png"); 00211 00212 ASSERT_EQ(snapshot.readings_per_scan, (unsigned int) 303); 00213 ASSERT_EQ(snapshot.num_scans, (unsigned int) 325); 00214 00215 ConfigGoal config = config3x4(); 00216 config.flip_horizontal = 1; 00217 detector.configure(config); 00218 00219 bool return_result; 00220 calibration_msgs::CalibrationPattern result; 00221 return_result = detector.detect(snapshot, result); 00222 EXPECT_TRUE(return_result); 00223 00225 check3x4(result, 1.0, true); 00226 } 00227 00228 00229 TEST(LaserCbDetector, malformedTest) 00230 { 00231 LaserCbDetector detector; 00232 00233 calibration_msgs::DenseLaserSnapshot snapshot; 00234 00235 detector.configure(config3x4()); 00236 snapshot = getSnapshot("test/data/cb_3x4.png"); 00237 snapshot.num_scans = 10; 00238 00239 bool return_result; 00240 calibration_msgs::CalibrationPattern result; 00241 return_result = detector.detect(snapshot, result); 00242 00243 EXPECT_FALSE(return_result); 00244 } 00245 00246 TEST(LaserCbDetector, noCheckerboardTest) 00247 { 00248 LaserCbDetector detector; 00249 00250 calibration_msgs::DenseLaserSnapshot snapshot; 00251 00252 ConfigGoal config = config3x4(); 00253 config.num_x = 4; 00254 detector.configure(config); 00255 snapshot = getSnapshot("test/data/cb_3x4.png"); 00256 00257 bool return_result; 00258 calibration_msgs::CalibrationPattern result; 00259 return_result = detector.detect(snapshot, result); 00260 00261 EXPECT_TRUE(return_result); 00262 EXPECT_FALSE(result.success); 00263 } 00264 00265 00266 00267 int main(int argc, char **argv){ 00268 testing::InitGoogleTest(&argc, argv); 00269 return RUN_ALL_TESTS(); 00270 }