00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
00052 #define xstr(s) str(s)
00053 #define str(s) #s
00054 static const string test_path = xstr(TEST_PATH);
00055
00056 calibration_msgs::DenseLaserSnapshot getSnapshot(const string& filename)
00057 {
00058 IplImage* image;
00059 image = cvLoadImage(filename.c_str(), 0);
00060 EXPECT_TRUE(image) << "could not open image file [" << filename << "]" << endl;
00061
00062
00063 IplImage* float_image;
00064 float_image = cvCreateImage( cvSize(image->width, image->height), IPL_DEPTH_32F, 1);
00065 cvConvert(image, float_image);
00066
00067 calibration_msgs::DenseLaserSnapshot snapshot;
00068 snapshot.readings_per_scan = image->width;
00069 snapshot.num_scans = image->height;
00070 snapshot.intensities.resize(image->height * image->width);
00071
00072 for (int i=0; i<float_image->height; i++)
00073 {
00074 memcpy(&snapshot.intensities[i*snapshot.readings_per_scan],
00075 (float_image->imageData + i*float_image->widthStep),
00076 sizeof(float)*snapshot.readings_per_scan);
00077 }
00078
00079 if (DEBUG)
00080 {
00081 for (unsigned int i=0; i<snapshot.num_scans; i++)
00082 {
00083 for (unsigned int j=0; j<snapshot.readings_per_scan; j++)
00084 {
00085 if (snapshot.intensities[i*snapshot.readings_per_scan + j] < 100.0)
00086 printf(" ");
00087 else
00088 printf("X");
00089 }
00090 printf("\n");
00091 }
00092 }
00093
00094 cvReleaseImage(&float_image);
00095 cvReleaseImage(&image);
00096
00097 return snapshot;
00098 }
00099
00100 ConfigGoal config3x4()
00101 {
00102 ConfigGoal config;
00103
00104 config.num_x = 3;
00105 config.num_y = 4;
00106 config.spacing_x = 1.0;
00107 config.spacing_y = 1.0;
00108
00109 config.width_scaling = 1.0;
00110 config.height_scaling = 1.0;
00111
00112 config.min_intensity = 0.0;
00113 config.max_intensity = 100.0;
00114
00115 config.subpixel_window = 3;
00116 config.subpixel_zero_zone = 1;
00117
00118 config.flip_horizontal = 0;
00119
00120 return config;
00121 }
00122
00123
00124 void check3x4(const calibration_msgs::CalibrationPattern& result, double pix_eps, bool flipped)
00125 {
00126 EXPECT_TRUE(result.success);
00127
00128
00129 EXPECT_NEAR(result.object_points[0].x, 0.0, eps);
00130 EXPECT_NEAR(result.object_points[0].y, 0.0, eps);
00131 EXPECT_NEAR(result.object_points[0].z, 0.0, eps);
00132
00133 EXPECT_NEAR(result.object_points[2].x, 2.0, eps);
00134 EXPECT_NEAR(result.object_points[2].y, 0.0, eps);
00135 EXPECT_NEAR(result.object_points[2].z, 0.0, eps);
00136
00137 EXPECT_NEAR(result.object_points[5].x, 2.0, eps);
00138 EXPECT_NEAR(result.object_points[5].y, 1.0, eps);
00139 EXPECT_NEAR(result.object_points[5].z, 0.0, eps);
00140
00141
00142 if (flipped)
00143 {
00144 EXPECT_NEAR(result.image_points[0].x, 81.5, pix_eps);
00145 EXPECT_NEAR(result.image_points[0].y, 72.0, pix_eps);
00146
00147 EXPECT_NEAR(result.image_points[1].x, 81.5, pix_eps);
00148 EXPECT_NEAR(result.image_points[1].y, 118.5, pix_eps);
00149
00150 EXPECT_NEAR(result.image_points[11].x, 223.5, pix_eps);
00151 EXPECT_NEAR(result.image_points[11].y, 166.5, pix_eps);
00152 }
00153 else
00154 {
00155 EXPECT_NEAR(result.image_points[0].x, 223.0, pix_eps);
00156 EXPECT_NEAR(result.image_points[0].y, 72.0, pix_eps);
00157
00158 EXPECT_NEAR(result.image_points[1].x, 223.0, pix_eps);
00159 EXPECT_NEAR(result.image_points[1].y, 119.0, pix_eps);
00160
00161 EXPECT_NEAR(result.image_points[11].x, 81.0, pix_eps);
00162 EXPECT_NEAR(result.image_points[11].y, 166.0, pix_eps);
00163 }
00164 }
00165
00166 TEST(LaserCbDetector, easy_cb_3x4)
00167 {
00168 LaserCbDetector detector;
00169 calibration_msgs::DenseLaserSnapshot snapshot;
00170
00171 snapshot = getSnapshot( test_path + "/data/cb_3x4.png");
00172
00173 ASSERT_EQ(snapshot.readings_per_scan, (unsigned int) 303);
00174 ASSERT_EQ(snapshot.num_scans, (unsigned int) 325);
00175
00176 detector.configure(config3x4());
00177
00178 bool return_result;
00179 calibration_msgs::CalibrationPattern result;
00180 return_result = detector.detect(snapshot, result);
00181 EXPECT_TRUE(return_result);
00182
00183 check3x4(result, pix_eps_, false);
00184 }
00185
00186 TEST(LaserCbDetector, scaled_cb_3x4)
00187 {
00188 LaserCbDetector detector;
00189 calibration_msgs::DenseLaserSnapshot snapshot;
00190
00191 snapshot = getSnapshot( test_path + "/data/cb_3x4.png");
00192
00193 ASSERT_EQ(snapshot.readings_per_scan, (unsigned int) 303);
00194 ASSERT_EQ(snapshot.num_scans, (unsigned int) 325);
00195
00196 ConfigGoal config = config3x4();
00197 config.width_scaling = 2.0;
00198 config.height_scaling = 4.0;
00199 detector.configure(config);
00200
00201 bool return_result;
00202 calibration_msgs::CalibrationPattern result;
00203 return_result = detector.detect(snapshot, result);
00204 EXPECT_TRUE(return_result);
00205
00207 check3x4(result, 1.0, false);
00208 }
00209
00210 TEST(LaserCbDetector, reflected_cb_3x4)
00211 {
00212 LaserCbDetector detector;
00213 calibration_msgs::DenseLaserSnapshot snapshot;
00214
00215 snapshot = getSnapshot( test_path + "/data/cb_3x4.png");
00216
00217 ASSERT_EQ(snapshot.readings_per_scan, (unsigned int) 303);
00218 ASSERT_EQ(snapshot.num_scans, (unsigned int) 325);
00219
00220 ConfigGoal config = config3x4();
00221 config.flip_horizontal = 1;
00222 detector.configure(config);
00223
00224 bool return_result;
00225 calibration_msgs::CalibrationPattern result;
00226 return_result = detector.detect(snapshot, result);
00227 EXPECT_TRUE(return_result);
00228
00230 check3x4(result, 1.0, true);
00231 }
00232
00233
00234 TEST(LaserCbDetector, malformedTest)
00235 {
00236 LaserCbDetector detector;
00237
00238 calibration_msgs::DenseLaserSnapshot snapshot;
00239
00240 detector.configure(config3x4());
00241 snapshot = getSnapshot( test_path + "/data/cb_3x4.png");
00242 snapshot.num_scans = 10;
00243
00244 bool return_result;
00245 calibration_msgs::CalibrationPattern result;
00246 return_result = detector.detect(snapshot, result);
00247
00248 EXPECT_FALSE(return_result);
00249 }
00250
00251 TEST(LaserCbDetector, noCheckerboardTest)
00252 {
00253 LaserCbDetector detector;
00254
00255 calibration_msgs::DenseLaserSnapshot snapshot;
00256
00257 ConfigGoal config = config3x4();
00258 config.num_x = 4;
00259 detector.configure(config);
00260 snapshot = getSnapshot( test_path + "/data/cb_3x4.png");
00261
00262 bool return_result;
00263 calibration_msgs::CalibrationPattern result;
00264 return_result = detector.detect(snapshot, result);
00265
00266 EXPECT_TRUE(return_result);
00267 EXPECT_FALSE(result.success);
00268 }
00269
00270
00271
00272 int main(int argc, char **argv){
00273 testing::InitGoogleTest(&argc, argv);
00274 return RUN_ALL_TESTS();
00275 }