ConnectedComponents.cpp
Go to the documentation of this file.
00001 /*
00002  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
00003  *
00004  * Copyright 2007-2012 VTT Technical Research Centre of Finland
00005  *
00006  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
00007  *          <http://www.vtt.fi/multimedia/alvar.html>
00008  *
00009  * ALVAR is free software; you can redistribute it and/or modify it under the
00010  * terms of the GNU Lesser General Public License as published by the Free
00011  * Software Foundation; either version 2.1 of the License, or (at your option)
00012  * any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful, but WITHOUT
00015  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
00017  * for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public License
00020  * along with ALVAR; if not, see
00021  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
00022  */
00023 
00024 #include "ar_track_alvar/ConnectedComponents.h"
00025 #include "ar_track_alvar/Draw.h"
00026 #include <cassert>
00027 
00028 using namespace std;
00029 
00030 namespace alvar {
00031 using namespace std;
00032 
00033 Labeling::Labeling()
00034 {
00035         gray = 0;
00036         bw       = 0;
00037         cam  = 0;
00038         thresh_param1 = 31;
00039         thresh_param2 = 5;
00040 }
00041 
00042 Labeling::~Labeling()
00043 {
00044         if(gray)
00045                 cvReleaseImage(&gray);
00046         if(bw)
00047                 cvReleaseImage(&bw);
00048 }
00049 
00050 bool Labeling::CheckBorder(CvSeq* contour, int width, int height)
00051 {
00052         bool ret = true;
00053         for(int i = 0; i < contour->total; ++i)
00054         {
00055                 CvPoint* pt = (CvPoint*)cvGetSeqElem(contour, i);
00056                 if((pt->x <= 1) || (pt->x >= width-2) || (pt->y <= 1) || (pt->y >= height-2)) ret = false;
00057         }
00058         return ret;
00059 }
00060 
00061 LabelingCvSeq::LabelingCvSeq() : _n_blobs(0), _min_edge(20), _min_area(25)
00062 {
00063         SetOptions();
00064         storage = cvCreateMemStorage(0);
00065 }
00066 
00067 LabelingCvSeq::~LabelingCvSeq()
00068 {
00069         if(storage)
00070                 cvReleaseMemStorage(&storage);
00071 }
00072 
00073 void LabelingCvSeq::SetOptions(bool _detect_pose_grayscale) {
00074     detect_pose_grayscale = _detect_pose_grayscale;
00075 }
00076 
00077 void LabelingCvSeq::LabelSquares(IplImage* image, bool visualize)
00078 {
00079 
00080     if (gray && ((gray->width != image->width) || (gray->height != image->height))) {
00081         cvReleaseImage(&gray); gray=NULL;
00082         if (bw) cvReleaseImage(&bw); bw=NULL;
00083     }
00084     if (gray == NULL) {
00085         gray = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_8U, 1);
00086         gray->origin = image->origin;
00087         bw = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_8U, 1);
00088         bw->origin = image->origin;
00089     }
00090 
00091     // Convert grayscale and threshold
00092     if(image->nChannels == 4)
00093         cvCvtColor(image, gray, CV_RGBA2GRAY);
00094     else if(image->nChannels == 3)
00095         cvCvtColor(image, gray, CV_RGB2GRAY);
00096     else if(image->nChannels == 1)
00097         cvCopy(image, gray);
00098     else {
00099         cerr<<"Unsupported image format"<<endl;
00100     }
00101 
00102     cvAdaptiveThreshold(gray, bw, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY_INV, thresh_param1, thresh_param2);
00103     //cvThreshold(gray, bw, 127, 255, CV_THRESH_BINARY_INV);
00104 
00105     CvSeq* contours;
00106     CvSeq* squares = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvSeq), storage);
00107     CvSeq* square_contours = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvSeq), storage);
00108 
00109     cvFindContours(bw, storage, &contours, sizeof(CvContour),
00110         CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0,0));
00111 
00112     while(contours)
00113     {
00114         if(contours->total < _min_edge)
00115         {
00116             contours = contours->h_next;
00117             continue;
00118         }
00119 
00120         CvSeq* result = cvApproxPoly(contours, sizeof(CvContour), storage,
00121                                      CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.035, 0 ); // TODO: Parameters?
00122 
00123         if( result->total == 4 && CheckBorder(result, image->width, image->height) && 
00124             fabs(cvContourArea(result,CV_WHOLE_SEQ)) > _min_area && // TODO check limits
00125             cvCheckContourConvexity(result) ) // ttehop: Changed to 'contours' instead of 'result'
00126         {
00127                 cvSeqPush(squares, result);
00128                 cvSeqPush(square_contours, contours);
00129         }
00130         contours = contours->h_next;
00131     }
00132 
00133     _n_blobs = squares->total;
00134     blob_corners.resize(_n_blobs);
00135 
00136     // For every detected 4-corner blob
00137     for(int i = 0; i < _n_blobs; ++i)
00138     {
00139         vector<Line> fitted_lines(4);
00140         blob_corners[i].resize(4);
00141         CvSeq* sq = (CvSeq*)cvGetSeqElem(squares, i);
00142         CvSeq* square_contour = (CvSeq*)cvGetSeqElem(square_contours, i);
00143         
00144         for(int j = 0; j < 4; ++j)
00145         {
00146             CvPoint* pt0 = (CvPoint*)cvGetSeqElem(sq, j);
00147             CvPoint* pt1 = (CvPoint*)cvGetSeqElem(sq, (j+1)%4);
00148             int k0=-1, k1=-1;
00149             for (int k = 0; k<square_contour->total; k++) {
00150                 CvPoint* pt2 = (CvPoint*)cvGetSeqElem(square_contour, k);
00151                 if ((pt0->x == pt2->x) && (pt0->y == pt2->y)) k0=k;
00152                 if ((pt1->x == pt2->x) && (pt1->y == pt2->y)) k1=k;
00153             }
00154             int len;
00155             if (k1 >= k0) len = k1-k0-1; // neither k0 nor k1 are included
00156             else len = square_contour->total-k0+k1-1;
00157             if (len == 0) len = 1;
00158 
00159             CvMat* line_data = cvCreateMat(1, len, CV_32FC2);
00160             for (int l=0; l<len; l++) {
00161                 int ll = (k0+l+1)%square_contour->total;
00162                 CvPoint* p = (CvPoint*)cvGetSeqElem(square_contour, ll);
00163                 CvPoint2D32f pp;
00164                 pp.x = float(p->x);
00165                 pp.y = float(p->y);
00166 
00167                 // Undistort
00168                 if(cam)
00169                     cam->Undistort(pp);
00170 
00171                 CV_MAT_ELEM(*line_data, CvPoint2D32f, 0, l) = pp;
00172             }
00173 
00174             // Fit edge and put to vector of edges
00175             float params[4] = {0};
00176 
00177             // TODO: The detect_pose_grayscale is still under work...
00178             /*
00179             if (detect_pose_grayscale &&
00180                 (pt0->x > 3) && (pt0->y > 3) &&
00181                 (pt0->x < (gray->width-4)) &&
00182                 (pt0->y < (gray->height-4)))
00183             {
00184                 // ttehop: Grayscale experiment
00185                 FitLineGray(line_data, params, gray);
00186             }
00187             */
00188             cvFitLine(line_data, CV_DIST_L2, 0, 0.01, 0.01, params);
00189 
00190             //cvFitLine(line_data, CV_DIST_L2, 0, 0.01, 0.01, params);
00192             Line line = Line(params);
00193             if(visualize) DrawLine(image, line);
00194             fitted_lines[j] = line;
00195 
00196             cvReleaseMat(&line_data);
00197         }
00198 
00199         // Calculated four intersection points
00200         for(size_t j = 0; j < 4; ++j)
00201         {
00202             PointDouble intc = Intersection(fitted_lines[j],fitted_lines[(j+1)%4]);
00203 
00204             // TODO: Instead, test OpenCV find corner in sub-pix...
00205             //CvPoint2D32f pt = cvPoint2D32f(intc.x, intc.y);
00206             //cvFindCornerSubPix(gray, &pt,
00207             //                   1, cvSize(3,3), cvSize(-1,-1),
00208             //                   cvTermCriteria(
00209             //                   CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,10,1e-4));
00210             
00211             // TODO: Now there is a wierd systematic 0.5 pixel error that is fixed here...
00212             //intc.x += 0.5;
00213             //intc.y += 0.5;
00214 
00215             if(cam) cam->Distort(intc);
00216 
00217             // TODO: Should we make this always counter-clockwise or clockwise?
00218             /*
00219             if (image->origin && j == 1) blob_corners[i][3] = intc;
00220             else if (image->origin && j == 3) blob_corners[i][1] = intc;
00221             else blob_corners[i][j] = intc;
00222             */
00223             blob_corners[i][j] = intc;
00224         }
00225         if (visualize) {
00226             for(size_t j = 0; j < 4; ++j) {
00227                 PointDouble &intc = blob_corners[i][j];
00228                 if (j == 0) cvCircle(image, cvPoint(int(intc.x), int(intc.y)), 5, CV_RGB(255, 255, 255));
00229                 if (j == 1) cvCircle(image, cvPoint(int(intc.x), int(intc.y)), 5, CV_RGB(255, 0, 0));
00230                 if (j == 2) cvCircle(image, cvPoint(int(intc.x), int(intc.y)), 5, CV_RGB(0, 255, 0));
00231                 if (j == 3) cvCircle(image, cvPoint(int(intc.x), int(intc.y)), 5, CV_RGB(0, 0, 255));
00232             }
00233         }
00234     }
00235 
00236     cvClearMemStorage(storage);
00237 }
00238 
00239 CvSeq* LabelingCvSeq::LabelImage(IplImage* image, int min_size, bool approx)
00240 {
00241         assert(image->origin == 0); // Currently only top-left origin supported
00242         if (gray && ((gray->width != image->width) || (gray->height != image->height))) {
00243                 cvReleaseImage(&gray); gray=NULL;
00244                 if (bw) cvReleaseImage(&bw); bw=NULL;
00245         }
00246         if (gray == NULL) {
00247                 gray = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_8U, 1);
00248                 gray->origin = image->origin;
00249                 bw = cvCreateImage(cvSize(image->width, image->height), IPL_DEPTH_8U, 1);
00250                 bw->origin = image->origin;
00251         }
00252 
00253         // Convert grayscale and threshold
00254         if(image->nChannels == 4)
00255                 cvCvtColor(image, gray, CV_RGBA2GRAY);
00256         else if(image->nChannels == 3)
00257                 cvCvtColor(image, gray, CV_RGB2GRAY);
00258         else if(image->nChannels == 1)
00259                 cvCopy(image, gray);
00260         else {
00261                 cerr<<"Unsupported image format"<<endl;
00262         }
00263 
00264         cvAdaptiveThreshold(gray, bw, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY_INV, thresh_param1, thresh_param2);
00265 
00266         CvSeq* contours;
00267         CvSeq* edges = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvSeq), storage);
00268         CvSeq* squares = cvCreateSeq(0, sizeof(CvSeq), sizeof(CvSeq), storage);
00269 
00270         cvFindContours(bw, storage, &contours, sizeof(CvContour),
00271                 CV_RETR_LIST, CV_CHAIN_APPROX_NONE, cvPoint(0,0));
00272         //cvFindContours(bw, storage, &contours, sizeof(CvContour),
00273         //      CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0));
00274 
00275 
00276         while(contours)
00277         {
00278                 if(contours->total < min_size)
00279                 {
00280                         contours = contours->h_next;
00281                         continue;
00282                 }
00283                 
00284                 if(approx)
00285                 {
00286                         CvSeq* result = cvApproxPoly(contours, sizeof(CvContour), storage,
00287                                                                                 CV_POLY_APPROX_DP, cvContourPerimeter(contours)*0.02, 0 ); // TODO: Parameters?
00288 
00289                         if(cvCheckContourConvexity(result))
00290                         {
00291                                         cvSeqPush(squares, result);
00292                         }
00293                 }       
00294                 else
00295                         cvSeqPush(squares, contours);
00296         
00297                 contours = contours->h_next;
00298         }
00299 
00300         cvClearMemStorage(storage);
00301 
00302         return squares;
00303 }
00304 
00305 inline int round(double x) {
00306         return (x)>=0?(int)((x)+0.5):(int)((x)-0.5);
00307 }
00308 
00309 template<class T>
00310 inline T absdiff(T c1, T c2) {
00311         return (c2>c1?c2-c1:c1-c2);
00312 }
00313 
00314 //#define SHOW_DEBUG
00315 #ifdef SHOW_DEBUG
00316 #include "highgui.h"
00317 #endif
00318 
00319 // TODO: This should be in LabelingCvSeq ???
00320 void FitLineGray(CvMat *line_data, float params[4], IplImage *gray) {
00321         // this very simple approach works...
00322         /*
00323         float *cx = &(params[2]);
00324         float *cy = &(params[3]);
00325         float *sx = &(params[0]);
00326         float *sy = &(params[1]);
00327         CvPoint2D32f *p1 = (CvPoint2D32f*)CV_MAT_ELEM_PTR_FAST(*line_data, 0, 0, sizeof(CvPoint2D32f));
00328         CvPoint2D32f *p2 = (CvPoint2D32f*)CV_MAT_ELEM_PTR_FAST(*line_data, 0, line_data->cols-1, sizeof(CvPoint2D32f));
00329         *cx = p1->x; *cy = p1->y;
00330         *sx = p2->x - p1->x; *sy = p2->y - p1->y;
00331         return;
00332         */
00333 
00334 #ifdef SHOW_DEBUG
00335         IplImage *tmp = cvCreateImage(cvSize(gray->width, gray->height), IPL_DEPTH_8U, 3);
00336         IplImage *tmp2 = cvCreateImage(cvSize(gray->width*5, gray->height*5), IPL_DEPTH_8U, 3);
00337         cvCvtColor(gray, tmp, CV_GRAY2RGB);
00338         cvResize(tmp, tmp2, CV_INTER_NN);
00339 #endif
00340 
00341         // Discover 1st the line normal direction
00342         CvPoint2D32f *p1 = (CvPoint2D32f*)CV_MAT_ELEM_PTR_FAST(*line_data, 0, 0, sizeof(CvPoint2D32f));
00343         CvPoint2D32f *p2 = (CvPoint2D32f*)CV_MAT_ELEM_PTR_FAST(*line_data, 0, line_data->cols-1, sizeof(CvPoint2D32f));
00344         double dx = +(p2->y - p1->y);
00345         double dy = -(p2->x - p1->x);
00346         if ((dx == 0) && (dy == 0)) return;
00347         else if      (dx == 0) { dy /= dy; }
00348         else if (dy == 0) { dx /= dx; }
00349         else if (abs(dx) > abs(dy)) { dy /= dx; dx /= dx; }
00350         else              { dx /= dy; dy /= dy; }
00351 
00352         // Build normal search table
00353         const int win_size=5;
00354         const int win_mid=win_size/2;
00355         const int diff_win_size=win_size-1;
00356         double xx[win_size], yy[win_size];
00357         double dxx[diff_win_size], dyy[diff_win_size];
00358         xx[win_mid] = 0; yy[win_mid] = 0;
00359         for (int i=1; i<=win_size/2; i++) {
00360                 xx[win_mid + i] = round(i*dx);
00361                 xx[win_mid - i] = -xx[win_mid + i];
00362                 yy[win_mid + i] = round(i*dy);
00363                 yy[win_mid - i] = -yy[win_mid + i];
00364         }
00365         for (int i=0; i<diff_win_size; i++) {
00366                 dxx[i] = (xx[i]+xx[i+1])/2;
00367                 dyy[i] = (yy[i]+yy[i+1])/2;
00368         }
00369 
00370         // Adjust the points
00371         for (int l=0; l<line_data->cols; l++) {
00372                 CvPoint2D32f *p = (CvPoint2D32f*)CV_MAT_ELEM_PTR_FAST(*line_data, 0, l, sizeof(CvPoint2D32f));
00373 
00374                 double dx=0, dy=0, ww=0;
00375                 for (int i=0; i<diff_win_size; i++) {
00376                         unsigned char c1 = (unsigned char)gray->imageData[int((p->y+yy[i])*gray->widthStep+(p->x+xx[i]))];
00377                         unsigned char c2 = (unsigned char)gray->imageData[int((p->y+yy[i+1])*gray->widthStep+(p->x+xx[i+1]))];
00378 #ifdef SHOW_DEBUG
00379                         cvCircle(tmp2, cvPoint((p->x+xx[i])*5+2,(p->y+yy[i])*5+2), 0, CV_RGB(0,0,255));
00380                         cvCircle(tmp2, cvPoint((p->x+xx[i+1])*5+2,(p->y+yy[i+1])*5+2), 0, CV_RGB(0,0,255));
00381 #endif
00382                         double w = absdiff(c1, c2);
00383                         dx += dxx[i]*w;
00384                         dy += dyy[i]*w;
00385                         ww += w;
00386                 }
00387                 if (ww > 0) {
00388                         dx /= ww; dy /= ww;
00389                 }
00390 #ifdef SHOW_DEBUG
00391                 cvLine(tmp2, cvPoint(p->x*5+2,p->y*5+2), cvPoint((p->x+dx)*5+2, (p->y+dy)*5+2), CV_RGB(0,255,0));
00392                 p->x += float(dx); p->y += float(dy);
00393                 cvCircle(tmp2, cvPoint(p->x*5+2,p->y*5+2), 0, CV_RGB(255,0,0));
00394 #else
00395                 p->x += float(dx); p->y += float(dy);
00396 #endif
00397         }
00398 
00399 #ifdef SHOW_DEBUG
00400         cvNamedWindow("tmp");
00401         cvShowImage("tmp",tmp2);
00402         cvWaitKey(0);
00403         cvReleaseImage(&tmp);
00404         cvReleaseImage(&tmp2);
00405 #endif
00406 }
00407 
00408 } // namespace alvar


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02