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
00036
00037 #include <ros/ros.h>
00038 #include <image_cb_detector/image_cb_detector_old.h>
00039
00040 using namespace image_cb_detector;
00041 using namespace std;
00042
00043 bool ImageCbDetectorOld::configure(const ConfigGoal& config)
00044 {
00045 config_ = config;
00046
00047 return true;
00048 }
00049
00050
00051
00052 bool ImageCbDetectorOld::detect(const sensor_msgs::ImageConstPtr& ros_image,
00053 calibration_msgs::CalibrationPattern& result)
00054 {
00055 IplImage *image = NULL;
00056 try
00057 {
00058 image = bridge_.imgMsgToCv(ros_image, "mono8");
00059 }
00060 catch (sensor_msgs::CvBridgeException error)
00061 {
00062 ROS_ERROR("error");
00063 return false;
00064 }
00065
00066
00067
00068
00069 const int scaled_width = (int) (.5 + image->width * config_.width_scaling);
00070 const int scaled_height = (int) (.5 + image->height * config_.height_scaling);
00071 IplImage* image_scaled = cvCreateImage(cvSize( scaled_width, scaled_height),
00072 image->depth,
00073 image->nChannels);
00074 cvResize(image, image_scaled, CV_INTER_LINEAR);
00075
00076
00077 vector<CvPoint2D32f> cv_corners;
00078 cv_corners.resize(config_.num_x*config_.num_y);
00079
00080
00081 CvSize board_size = cvSize(config_.num_x, config_.num_y);
00082 int num_corners ;
00083 int found = cvFindChessboardCorners( image_scaled, board_size, &cv_corners[0], &num_corners,
00084 CV_CALIB_CB_ADAPTIVE_THRESH) ;
00085
00086 if(found)
00087 {
00088 ROS_DEBUG("Found CB");
00089
00090
00091
00092 CvSize subpixel_window = cvSize(config_.subpixel_window,
00093 config_.subpixel_window);
00094 CvSize subpixel_zero_zone = cvSize(config_.subpixel_zero_zone,
00095 config_.subpixel_zero_zone);
00096
00097
00098 cvFindCornerSubPix(image_scaled, &cv_corners[0], num_corners,
00099 subpixel_window,
00100 subpixel_zero_zone,
00101 cvTermCriteria(CV_TERMCRIT_ITER,20,1e-2));
00102 }
00103 else
00104 ROS_DEBUG("Didn't find CB");
00105 cvReleaseImage(&image_scaled);
00106
00107
00108 result.header.stamp = ros_image->header.stamp;
00109 result.header.frame_id = ros_image->header.frame_id;
00110
00111 result.object_points.resize(config_.num_x * config_.num_y);
00112 for (unsigned int i=0; i < config_.num_y; i++)
00113 {
00114 for (unsigned int j=0; j < config_.num_x; j++)
00115 {
00116 result.object_points[i*config_.num_x + j].x = j*config_.spacing_x;
00117 result.object_points[i*config_.num_x + j].y = i*config_.spacing_y;
00118 result.object_points[i*config_.num_x + j].z = 0.0;
00119 }
00120 }
00121
00122 result.image_points.resize(num_corners);
00123
00124 for (int i=0; i < num_corners; i++)
00125 {
00126 result.image_points[i].x = cv_corners[i].x / config_.width_scaling;
00127 result.image_points[i].y = cv_corners[i].y / config_.height_scaling;
00128 }
00129
00130 result.success = found;
00131
00132 return true;
00133 }