modules/calib3d/src/quadsubpix.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2  //
   3  //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4  //
   5  //  By downloading, copying, installing or using the software you agree to this license.
   6  //  If you do not agree to this license, do not download, install,
   7  //  copy or use the software.
   8  //
   9  //
  10  //                        Intel License Agreement
  11  //                For Open Source Computer Vision Library
  12  //
  13  // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14  // Third party copyrights are property of their respective owners.
  15  //
  16  // Redistribution and use in source and binary forms, with or without modification,
  17  // are permitted provided that the following conditions are met:
  18  //
  19  //   * Redistribution's of source code must retain the above copyright notice,
  20  //     this list of conditions and the following disclaimer.
  21  //
  22  //   * Redistribution's in binary form must reproduce the above copyright notice,
  23  //     this list of conditions and the following disclaimer in the documentation
  24  //     and/or other materials provided with the distribution.
  25  //
  26  //   * The name of Intel Corporation may not be used to endorse or promote products
  27  //     derived from this software without specific prior written permission.
  28  //
  29  // This software is provided by the copyright holders and contributors "as is" and
  30  // any express or implied warranties, including, but not limited to, the implied
  31  // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32  // In no event shall the Intel Corporation or contributors be liable for any direct,
  33  // indirect, incidental, special, exemplary, or consequential damages
  34  // (including, but not limited to, procurement of substitute goods or services;
  35  // loss of use, data, or profits; or business interruption) however caused
  36  // and on any theory of liability, whether in contract, strict liability,
  37  // or tort (including negligence or otherwise) arising in any way out of
  38  // the use of this software, even if advised of the possibility of such damage.
  39  //
  40  //M*/
  41
  42 #include "precomp.hpp"
  43
  44 #include <limits>
  45 #include <utility>
  46 #include <algorithm>
  47
  48 #include <math.h>
  49
  50 //#define _SUBPIX_VERBOSE
  51
  52 #undef max
  53
  54 namespace cv {
  55
  56
  57 void drawCircles(Mat& img, const vector<Point2f>& corners, const vector<float>& radius)
  58 {
  59     for(size_t i = 0; i < corners.size(); i++)
  60     {
  61         circle(img, corners[i], cvRound(radius[i]), CV_RGB(255, 0, 0));
  62     }
  63 }
  64
  65 int histQuantile(const Mat& hist, float quantile)
  66 {
  67     if(hist.dims > 1) return -1; // works for 1D histograms only
  68
  69     float cur_sum = 0;
  70     float total_sum = (float)sum(hist).val[0];
  71     float quantile_sum = total_sum*quantile;
  72     for(int j = 0; j < hist.size[0]; j++)
  73     {
  74         cur_sum += (float)hist.at<float>(j);
  75         if(cur_sum > quantile_sum)
  76         {
  77             return j;
  78         }
  79     }
  80
  81     return hist.size[0] - 1;
  82 }
  83
  84 bool is_smaller(const std::pair<int, float>& p1, const std::pair<int, float>& p2)
  85 {
  86     return p1.second < p2.second;
  87 }
  88
  89 void orderContours(const vector<vector<Point> >& contours, Point2f point, vector<std::pair<int, float> >& order)
  90 {
  91     order.clear();
  92     size_t i, j, n = contours.size();
  93     for(i = 0; i < n; i++)
  94     {
  95         size_t ni = contours[i].size();
  96         double min_dist = std::numeric_limits<double>::max();
  97         for(j = 0; j < ni; j++)
  98         {
  99             double dist = norm(Point2f((float)contours[i][j].x, (float)contours[i][j].y) - point);
 100             min_dist = MIN(min_dist, dist);
 101         }
 102         order.push_back(std::pair<int, float>((int)i, (float)min_dist));
 103     }
 104
 105     std::sort(order.begin(), order.end(), is_smaller);
 106 }
 107
 108 // fit second order curve to a set of 2D points
 109 void fitCurve2Order(const vector<Point2f>& /*points*/, vector<float>& /*curve*/)
 110 {
 111     // TBD
 112 }
 113
 114 void findCurvesCross(const vector<float>& /*curve1*/, const vector<float>& /*curve2*/, Point2f& /*cross_point*/)
 115 {
 116 }
 117
 118 void findLinesCrossPoint(Point2f origin1, Point2f dir1, Point2f origin2, Point2f dir2, Point2f& cross_point)
 119 {
 120     float det = dir2.x*dir1.y - dir2.y*dir1.x;
 121     Point2f offset = origin2 - origin1;
 122
 123     float alpha = (dir2.x*offset.y - dir2.y*offset.x)/det;
 124     cross_point = origin1 + dir1*alpha;
 125 }
 126
 127 void findCorner(const vector<Point>& contour, Point2f point, Point2f& corner)
 128 {
 129     // find the nearest point
 130     double min_dist = std::numeric_limits<double>::max();
 131     int min_idx = -1;
 132
 133     // find corner idx
 134     for(size_t i = 0; i < contour.size(); i++)
 135     {
 136         double dist = norm(Point2f((float)contour[i].x, (float)contour[i].y) - point);
 137         if(dist < min_dist)
 138         {
 139             min_dist = dist;
 140             min_idx = (int)i;
 141         }
 142     }
 143     assert(min_idx >= 0);
 144
 145     // temporary solution, have to make something more precise
 146     corner = contour[min_idx];
 147     return;
 148 }
 149
 150 void findCorner(const vector<Point2f>& contour, Point2f point, Point2f& corner)
 151 {
 152     // find the nearest point
 153     double min_dist = std::numeric_limits<double>::max();
 154     int min_idx = -1;
 155
 156     // find corner idx
 157     for(size_t i = 0; i < contour.size(); i++)
 158     {
 159         double dist = norm(contour[i] - point);
 160         if(dist < min_dist)
 161         {
 162             min_dist = dist;
 163             min_idx = (int)i;
 164         }
 165     }
 166     assert(min_idx >= 0);
 167
 168     // temporary solution, have to make something more precise
 169     corner = contour[min_idx];
 170     return;
 171 }
 172
 173 int segment_hist_max(const Mat& hist, int& low_thresh, int& high_thresh)
 174 {
 175     Mat bw;
 176     //const double max_bell_width = 20; // we expect two bells with width bounded above
 177     //const double min_bell_width = 5; // and below
 178
 179     double total_sum = sum(hist).val[0];
 180     //double thresh = total_sum/(2*max_bell_width)*0.25f; // quarter of a bar inside a bell
 181
 182 //    threshold(hist, bw, thresh, 255.0, CV_THRESH_BINARY);
 183
 184     double quantile_sum = 0.0;
 185     //double min_quantile = 0.2;
 186     double low_sum = 0;
 187     double max_segment_length = 0;
 188     int max_start_x = -1;
 189     int max_end_x = -1;
 190     int start_x = 0;
 191     const double out_of_bells_fraction = 0.1;
 192     for(int x = 0; x < hist.size[0]; x++)
 193     {
 194         quantile_sum += hist.at<float>(x);
 195         if(quantile_sum < 0.2*total_sum) continue;
 196
 197         if(quantile_sum - low_sum > out_of_bells_fraction*total_sum)
 198         {
 199             if(max_segment_length < x - start_x)
 200             {
 201                 max_segment_length = x - start_x;
 202                 max_start_x = start_x;
 203                 max_end_x = x;
 204             }
 205
 206             low_sum = quantile_sum;
 207             start_x = x;
 208         }
 209     }
 210
 211     if(start_x == -1)
 212     {
 213         return 0;
 214     }
 215     else
 216     {
 217         low_thresh = cvRound(max_start_x + 0.25*(max_end_x - max_start_x));
 218         high_thresh = cvRound(max_start_x + 0.75*(max_end_x - max_start_x));
 219         return 1;
 220     }
 221 }
 222
 223 }
 224
 225 bool cv::find4QuadCornerSubpix(InputArray _img, InputOutputArray _corners, Size region_size)
 226 {
 227     Mat img = _img.getMat(), cornersM = _corners.getMat();
 228     int ncorners = cornersM.checkVector(2, CV_32F);
 229     CV_Assert( ncorners >= 0 );
 230     Point2f* corners = cornersM.ptr<Point2f>();
 231     const int nbins = 256;
 232     float ranges[] = {0, 256};
 233     const float* _ranges = ranges;
 234     Mat hist;
 235
 236 #if defined(_SUBPIX_VERBOSE)
 237     vector<float> radius;
 238     radius.assign(corners.size(), 0.0f);
 239 #endif //_SUBPIX_VERBOSE
 240
 241
 242     Mat black_comp, white_comp;
 243     for(int i = 0; i < ncorners; i++)
 244     {
 245         int channels = 0;
 246         Rect roi(cvRound(corners[i].x - region_size.width), cvRound(corners[i].y - region_size.height),
 247             region_size.width*2 + 1, region_size.height*2 + 1);
 248         Mat img_roi = img(roi);
 249         calcHist(&img_roi, 1, &channels, Mat(), hist, 1, &nbins, &_ranges);
 250
 251 #if 0
 252         int black_thresh = histQuantile(hist, 0.45f);
 253         int white_thresh = histQuantile(hist, 0.55f);
 254 #else
 255         int black_thresh, white_thresh;
 256         segment_hist_max(hist, black_thresh, white_thresh);
 257 #endif
 258
 259         threshold(img, black_comp, black_thresh, 255.0, CV_THRESH_BINARY_INV);
 260         threshold(img, white_comp, white_thresh, 255.0, CV_THRESH_BINARY);
 261
 262         const int erode_count = 1;
 263         erode(black_comp, black_comp, Mat(), Point(-1, -1), erode_count);
 264         erode(white_comp, white_comp, Mat(), Point(-1, -1), erode_count);
 265
 266 #if defined(_SUBPIX_VERBOSE)
 267         namedWindow("roi", 1);
 268         imshow("roi", img_roi);
 269         imwrite("test.jpg", img);
 270         namedWindow("black", 1);
 271         imshow("black", black_comp);
 272         namedWindow("white", 1);
 273         imshow("white", white_comp);
 274         cvWaitKey(0);
 275         imwrite("black.jpg", black_comp);
 276         imwrite("white.jpg", white_comp);
 277 #endif
 278
 279
 280         vector<vector<Point> > white_contours, black_contours;
 281         vector<Vec4i> white_hierarchy, black_hierarchy;
 282         findContours(black_comp, black_contours, black_hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
 283         findContours(white_comp, white_contours, white_hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
 284
 285         if(black_contours.size() < 5 || white_contours.size() < 5) continue;
 286
 287         // find two white and black blobs that are close to the input point
 288         vector<std::pair<int, float> > white_order, black_order;
 289         orderContours(black_contours, corners[i], black_order);
 290         orderContours(white_contours, corners[i], white_order);
 291
 292         const float max_dist = 10.0f;
 293         if(black_order[0].second > max_dist || black_order[1].second > max_dist ||
 294            white_order[0].second > max_dist || white_order[1].second > max_dist)
 295         {
 296             continue; // there will be no improvement in this corner position
 297         }
 298
 299         const vector<Point>* quads[4] = {&black_contours[black_order[0].first], &black_contours[black_order[1].first],
 300                                          &white_contours[white_order[0].first], &white_contours[white_order[1].first]};
 301         vector<Point2f> quads_approx[4];
 302         Point2f quad_corners[4];
 303         for(int k = 0; k < 4; k++)
 304         {
 305 #if 1
 306             vector<Point2f> temp;
 307             for(size_t j = 0; j < quads[k]->size(); j++) temp.push_back((*quads[k])[j]);
 308             approxPolyDP(Mat(temp), quads_approx[k], 0.5, true);
 309
 310             findCorner(quads_approx[k], corners[i], quad_corners[k]);
 311 #else
 312             findCorner(*quads[k], corners[i], quad_corners[k]);
 313 #endif
 314             quad_corners[k] += Point2f(0.5f, 0.5f);
 315         }
 316
 317         // cross two lines
 318         Point2f origin1 = quad_corners[0];
 319         Point2f dir1 = quad_corners[1] - quad_corners[0];
 320         Point2f origin2 = quad_corners[2];
 321         Point2f dir2 = quad_corners[3] - quad_corners[2];
 322         double angle = acos(dir1.dot(dir2)/(norm(dir1)*norm(dir2)));
 323         if(cvIsNaN(angle) || cvIsInf(angle) || angle < 0.5 || angle > CV_PI - 0.5) continue;
 324
 325         findLinesCrossPoint(origin1, dir1, origin2, dir2, corners[i]);
 326
 327 #if defined(_SUBPIX_VERBOSE)
 328         radius[i] = norm(corners[i] - ground_truth_corners[ground_truth_idx])*6;
 329
 330 #if 1
 331         Mat test(img.size(), CV_32FC3);
 332         cvtColor(img, test, CV_GRAY2RGB);
 333 //        line(test, quad_corners[0] - corners[i] + Point2f(30, 30), quad_corners[1] - corners[i] + Point2f(30, 30), cvScalar(0, 255, 0));
 334 //        line(test, quad_corners[2] - corners[i] + Point2f(30, 30), quad_corners[3] - corners[i] + Point2f(30, 30), cvScalar(0, 255, 0));
 335         vector<vector<Point> > contrs;
 336         contrs.resize(1);
 337         for(int k = 0; k < 4; k++)
 338         {
 339             //contrs[0] = quads_approx[k];
 340             contrs[0].clear();
 341             for(size_t j = 0; j < quads_approx[k].size(); j++) contrs[0].push_back(quads_approx[k][j]);
 342             drawContours(test, contrs, 0, CV_RGB(0, 0, 255), 1, 1, vector<Vec4i>(), 2);
 343             circle(test, quad_corners[k], 0.5, CV_RGB(255, 0, 0));
 344         }
 345         Mat test1 = test(Rect(corners[i].x - 30, corners[i].y - 30, 60, 60));
 346         namedWindow("1", 1);
 347         imshow("1", test1);
 348         imwrite("test.jpg", test);
 349         waitKey(0);
 350 #endif
 351 #endif //_SUBPIX_VERBOSE
 352
 353     }
 354
 355 #if defined(_SUBPIX_VERBOSE)
 356     Mat test(img.size(), CV_32FC3);
 357     cvtColor(img, test, CV_GRAY2RGB);
 358     drawCircles(test, corners, radius);
 359
 360     namedWindow("corners", 1);
 361     imshow("corners", test);
 362     waitKey();
 363 #endif //_SUBPIX_VERBOSE
 364
 365     return true;
 366 }