quadsubpix.cpp 12.2 KB
Newer Older
wester committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49
/*M///////////////////////////////////////////////////////////////////////////////////////
 //
 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
 //
 //  By downloading, copying, installing or using the software you agree to this license.
 //  If you do not agree to this license, do not download, install,
 //  copy or use the software.
 //
 //
 //                        Intel License Agreement
 //                For Open Source Computer Vision Library
 //
 // Copyright (C) 2000, Intel Corporation, all rights reserved.
 // Third party copyrights are property of their respective owners.
 //
 // Redistribution and use in source and binary forms, with or without modification,
 // are permitted provided that the following conditions are met:
 //
 //   * Redistribution's of source code must retain the above copyright notice,
 //     this list of conditions and the following disclaimer.
 //
 //   * Redistribution's in binary form must reproduce the above copyright notice,
 //     this list of conditions and the following disclaimer in the documentation
 //     and/or other materials provided with the distribution.
 //
 //   * The name of Intel Corporation may not be used to endorse or promote products
 //     derived from this software without specific prior written permission.
 //
 // This software is provided by the copyright holders and contributors "as is" and
 // any express or implied warranties, including, but not limited to, the implied
 // warranties of merchantability and fitness for a particular purpose are disclaimed.
 // In no event shall the Intel Corporation or contributors be liable for any direct,
 // indirect, incidental, special, exemplary, or consequential damages
 // (including, but not limited to, procurement of substitute goods or services;
 // loss of use, data, or profits; or business interruption) however caused
 // and on any theory of liability, whether in contract, strict liability,
 // or tort (including negligence or otherwise) arising in any way out of
 // the use of this software, even if advised of the possibility of such damage.
 //
 //M*/

#include "precomp.hpp"

#include <limits>
#include <utility>
#include <algorithm>

#include <math.h>

wester committed
50 51 52 53
//#define _SUBPIX_VERBOSE

#undef max

wester committed
54 55
namespace cv {

wester committed
56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83

// static void drawCircles(Mat& img, const vector<Point2f>& corners, const vector<float>& radius)
// {
//     for(size_t i = 0; i < corners.size(); i++)
//     {
//         circle(img, corners[i], cvRound(radius[i]), CV_RGB(255, 0, 0));
//     }
// }

// static int histQuantile(const Mat& hist, float quantile)
// {
//     if(hist.dims > 1) return -1; // works for 1D histograms only

//     float cur_sum = 0;
//     float total_sum = (float)sum(hist).val[0];
//     float quantile_sum = total_sum*quantile;
//     for(int j = 0; j < hist.size[0]; j++)
//     {
//         cur_sum += (float)hist.at<float>(j);
//         if(cur_sum > quantile_sum)
//         {
//             return j;
//         }
//     }

//     return hist.size[0] - 1;
// }

wester committed
84 85 86 87 88
inline bool is_smaller(const std::pair<int, float>& p1, const std::pair<int, float>& p2)
{
    return p1.second < p2.second;
}

wester committed
89
static void orderContours(const vector<vector<Point> >& contours, Point2f point, vector<std::pair<int, float> >& order)
wester committed
90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108
{
    order.clear();
    size_t i, j, n = contours.size();
    for(i = 0; i < n; i++)
    {
        size_t ni = contours[i].size();
        double min_dist = std::numeric_limits<double>::max();
        for(j = 0; j < ni; j++)
        {
            double dist = norm(Point2f((float)contours[i][j].x, (float)contours[i][j].y) - point);
            min_dist = MIN(min_dist, dist);
        }
        order.push_back(std::pair<int, float>((int)i, (float)min_dist));
    }

    std::sort(order.begin(), order.end(), is_smaller);
}

// fit second order curve to a set of 2D points
wester committed
109
inline void fitCurve2Order(const vector<Point2f>& /*points*/, vector<float>& /*curve*/)
wester committed
110 111 112 113
{
    // TBD
}

wester committed
114
inline void findCurvesCross(const vector<float>& /*curve1*/, const vector<float>& /*curve2*/, Point2f& /*cross_point*/)
wester committed
115 116 117 118 119 120 121 122 123 124 125 126
{
}

static void findLinesCrossPoint(Point2f origin1, Point2f dir1, Point2f origin2, Point2f dir2, Point2f& cross_point)
{
    float det = dir2.x*dir1.y - dir2.y*dir1.x;
    Point2f offset = origin2 - origin1;

    float alpha = (dir2.x*offset.y - dir2.y*offset.x)/det;
    cross_point = origin1 + dir1*alpha;
}

wester committed
127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150
// static void findCorner(const vector<Point>& contour, Point2f point, Point2f& corner)
// {
//     // find the nearest point
//     double min_dist = std::numeric_limits<double>::max();
//     int min_idx = -1;

//     // find corner idx
//     for(size_t i = 0; i < contour.size(); i++)
//     {
//         double dist = norm(Point2f((float)contour[i].x, (float)contour[i].y) - point);
//         if(dist < min_dist)
//         {
//             min_dist = dist;
//             min_idx = (int)i;
//         }
//     }
//     assert(min_idx >= 0);

//     // temporary solution, have to make something more precise
//     corner = contour[min_idx];
//     return;
// }

static void findCorner(const vector<Point2f>& contour, Point2f point, Point2f& corner)
wester committed
151 152 153 154 155 156 157 158 159 160 161 162 163 164 165
{
    // find the nearest point
    double min_dist = std::numeric_limits<double>::max();
    int min_idx = -1;

    // find corner idx
    for(size_t i = 0; i < contour.size(); i++)
    {
        double dist = norm(contour[i] - point);
        if(dist < min_dist)
        {
            min_dist = dist;
            min_idx = (int)i;
        }
    }
wester committed
166
    assert(min_idx >= 0);
wester committed
167 168 169 170 171 172 173 174 175

    // temporary solution, have to make something more precise
    corner = contour[min_idx];
    return;
}

static int segment_hist_max(const Mat& hist, int& low_thresh, int& high_thresh)
{
    Mat bw;
wester committed
176 177 178
    //const double max_bell_width = 20; // we expect two bells with width bounded above
    //const double min_bell_width = 5; // and below

wester committed
179
    double total_sum = sum(hist).val[0];
wester committed
180 181 182
    //double thresh = total_sum/(2*max_bell_width)*0.25f; // quarter of a bar inside a bell

//    threshold(hist, bw, thresh, 255.0, CV_THRESH_BINARY);
wester committed
183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235

    double quantile_sum = 0.0;
    //double min_quantile = 0.2;
    double low_sum = 0;
    double max_segment_length = 0;
    int max_start_x = -1;
    int max_end_x = -1;
    int start_x = 0;
    const double out_of_bells_fraction = 0.1;
    for(int x = 0; x < hist.size[0]; x++)
    {
        quantile_sum += hist.at<float>(x);
        if(quantile_sum < 0.2*total_sum) continue;

        if(quantile_sum - low_sum > out_of_bells_fraction*total_sum)
        {
            if(max_segment_length < x - start_x)
            {
                max_segment_length = x - start_x;
                max_start_x = start_x;
                max_end_x = x;
            }

            low_sum = quantile_sum;
            start_x = x;
        }
    }

    if(start_x == -1)
    {
        return 0;
    }
    else
    {
        low_thresh = cvRound(max_start_x + 0.25*(max_end_x - max_start_x));
        high_thresh = cvRound(max_start_x + 0.75*(max_end_x - max_start_x));
        return 1;
    }
}

}

bool cv::find4QuadCornerSubpix(InputArray _img, InputOutputArray _corners, Size region_size)
{
    Mat img = _img.getMat(), cornersM = _corners.getMat();
    int ncorners = cornersM.checkVector(2, CV_32F);
    CV_Assert( ncorners >= 0 );
    Point2f* corners = cornersM.ptr<Point2f>();
    const int nbins = 256;
    float ranges[] = {0, 256};
    const float* _ranges = ranges;
    Mat hist;

wester committed
236 237 238 239 240 241
#if defined(_SUBPIX_VERBOSE)
    vector<float> radius;
    radius.assign(corners.size(), 0.0f);
#endif //_SUBPIX_VERBOSE


wester committed
242 243 244 245 246 247 248 249 250
    Mat black_comp, white_comp;
    for(int i = 0; i < ncorners; i++)
    {
        int channels = 0;
        Rect roi(cvRound(corners[i].x - region_size.width), cvRound(corners[i].y - region_size.height),
            region_size.width*2 + 1, region_size.height*2 + 1);
        Mat img_roi = img(roi);
        calcHist(&img_roi, 1, &channels, Mat(), hist, 1, &nbins, &_ranges);

wester committed
251 252 253 254
#if 0
        int black_thresh = histQuantile(hist, 0.45f);
        int white_thresh = histQuantile(hist, 0.55f);
#else
wester committed
255 256
        int black_thresh = 0, white_thresh = 0;
        segment_hist_max(hist, black_thresh, white_thresh);
wester committed
257
#endif
wester committed
258

wester committed
259 260
        threshold(img, black_comp, black_thresh, 255.0, CV_THRESH_BINARY_INV);
        threshold(img, white_comp, white_thresh, 255.0, CV_THRESH_BINARY);
wester committed
261 262 263 264 265

        const int erode_count = 1;
        erode(black_comp, black_comp, Mat(), Point(-1, -1), erode_count);
        erode(white_comp, white_comp, Mat(), Point(-1, -1), erode_count);

wester committed
266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283
#if defined(_SUBPIX_VERBOSE)
        namedWindow("roi", 1);
        imshow("roi", img_roi);
        imwrite("test.jpg", img);
        namedWindow("black", 1);
        imshow("black", black_comp);
        namedWindow("white", 1);
        imshow("white", white_comp);
        cvWaitKey(0);
        imwrite("black.jpg", black_comp);
        imwrite("white.jpg", white_comp);
#endif


        vector<vector<Point> > white_contours, black_contours;
        vector<Vec4i> white_hierarchy, black_hierarchy;
        findContours(black_comp, black_contours, black_hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
        findContours(white_comp, white_contours, white_hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
wester committed
284 285 286 287

        if(black_contours.size() < 5 || white_contours.size() < 5) continue;

        // find two white and black blobs that are close to the input point
wester committed
288
        vector<std::pair<int, float> > white_order, black_order;
wester committed
289 290 291 292 293 294 295 296 297 298
        orderContours(black_contours, corners[i], black_order);
        orderContours(white_contours, corners[i], white_order);

        const float max_dist = 10.0f;
        if(black_order[0].second > max_dist || black_order[1].second > max_dist ||
           white_order[0].second > max_dist || white_order[1].second > max_dist)
        {
            continue; // there will be no improvement in this corner position
        }

wester committed
299
        const vector<Point>* quads[4] = {&black_contours[black_order[0].first], &black_contours[black_order[1].first],
wester committed
300
                                         &white_contours[white_order[0].first], &white_contours[white_order[1].first]};
wester committed
301
        vector<Point2f> quads_approx[4];
wester committed
302 303 304
        Point2f quad_corners[4];
        for(int k = 0; k < 4; k++)
        {
wester committed
305 306
#if 1
            vector<Point2f> temp;
wester committed
307 308 309 310
            for(size_t j = 0; j < quads[k]->size(); j++) temp.push_back((*quads[k])[j]);
            approxPolyDP(Mat(temp), quads_approx[k], 0.5, true);

            findCorner(quads_approx[k], corners[i], quad_corners[k]);
wester committed
311 312 313
#else
            findCorner(*quads[k], corners[i], quad_corners[k]);
#endif
wester committed
314 315 316 317 318 319 320 321 322 323 324 325
            quad_corners[k] += Point2f(0.5f, 0.5f);
        }

        // cross two lines
        Point2f origin1 = quad_corners[0];
        Point2f dir1 = quad_corners[1] - quad_corners[0];
        Point2f origin2 = quad_corners[2];
        Point2f dir2 = quad_corners[3] - quad_corners[2];
        double angle = acos(dir1.dot(dir2)/(norm(dir1)*norm(dir2)));
        if(cvIsNaN(angle) || cvIsInf(angle) || angle < 0.5 || angle > CV_PI - 0.5) continue;

        findLinesCrossPoint(origin1, dir1, origin2, dir2, corners[i]);
wester committed
326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352

#if defined(_SUBPIX_VERBOSE)
        radius[i] = norm(corners[i] - ground_truth_corners[ground_truth_idx])*6;

#if 1
        Mat test(img.size(), CV_32FC3);
        cvtColor(img, test, CV_GRAY2RGB);
//        line(test, quad_corners[0] - corners[i] + Point2f(30, 30), quad_corners[1] - corners[i] + Point2f(30, 30), cvScalar(0, 255, 0));
//        line(test, quad_corners[2] - corners[i] + Point2f(30, 30), quad_corners[3] - corners[i] + Point2f(30, 30), cvScalar(0, 255, 0));
        vector<vector<Point> > contrs;
        contrs.resize(1);
        for(int k = 0; k < 4; k++)
        {
            //contrs[0] = quads_approx[k];
            contrs[0].clear();
            for(size_t j = 0; j < quads_approx[k].size(); j++) contrs[0].push_back(quads_approx[k][j]);
            drawContours(test, contrs, 0, CV_RGB(0, 0, 255), 1, 1, vector<Vec4i>(), 2);
            circle(test, quad_corners[k], 0.5, CV_RGB(255, 0, 0));
        }
        Mat test1 = test(Rect(corners[i].x - 30, corners[i].y - 30, 60, 60));
        namedWindow("1", 1);
        imshow("1", test1);
        imwrite("test.jpg", test);
        waitKey(0);
#endif
#endif //_SUBPIX_VERBOSE

wester committed
353 354
    }

wester committed
355 356 357 358 359 360 361 362 363 364
#if defined(_SUBPIX_VERBOSE)
    Mat test(img.size(), CV_32FC3);
    cvtColor(img, test, CV_GRAY2RGB);
    drawCircles(test, corners, radius);

    namedWindow("corners", 1);
    imshow("corners", test);
    waitKey();
#endif //_SUBPIX_VERBOSE

wester committed
365 366
    return true;
}