git.maemo.org Git - opencv/blob - apps/VMDemo/SubdivMorpher.cpp

   1 #include "stdafx.h"
   2 #include "SubdivMorpher.h"
   3
   4 typedef struct
   5 {
   6     CV_SUBDIV2D_POINT_FIELDS()
   7     CvPoint2D32f* another;
   8 }
   9 StereoSubdivPoint;
  10
  11 CCvSubdivMorpher::CCvSubdivMorpher()
  12 {
  13     m_subdiv = cvCreateSubdiv2D( 0, sizeof(*m_subdiv),
  14                                  sizeof(StereoSubdivPoint),
  15                                  sizeof(CvQuadEdge2D),
  16                                  cvCreateMemStorage(0));
  17     Clear();
  18     SetPointCount(100);
  19
  20     m_win_size = cvSize(20,20);
  21     m_pyr_level = 3;
  22     m_criteria = cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.1);
  23 }
  24
  25
  26 bool CCvSubdivMorpher::GeneratePoints()
  27 {
  28 #if 1
  29     int x, y;
  30     int count = m_count - 4;
  31
  32     CvSize size = GetImageSize( m_left_gray_img );
  33
  34     int ny = cvRound( cvSqrt( ((float)count)*size.height/size.width ));
  35     int nx = cvFloor( ((float)count)/MAX(ny,1) );
  36
  37     float x_scale = ((float)size.width)/(nx + 1);
  38     float y_scale = ((float)size.height)/(ny + 1);
  39
  40     if( nx < 2 || ny < 2 ) return false;
  41
  42     m_realcount = nx*ny;
  43
  44     for( y = 0; y < ny; y++ )
  45     {
  46         for( x = 0; x < nx; x++ )
  47         {
  48             m_left_points[y*nx + x] =
  49                 cvPoint2D32f( (x + 0.5f)*x_scale, (y + 0.5f)*y_scale );
  50         }
  51     }
  52
  53 #else
  54     m_realcount = m_count;
  55
  56     cvGoodFeaturesToTrack( m_left_gray_img, m_left_pyr, m_right_pyr, m_left_points,
  57                            &m_realcount, 0.01, 5 );
  58 #endif
  59
  60     cvFindCornerSubPix( m_left_gray_img, m_left_points, m_realcount,
  61                         m_win_size, cvSize(-1,-1), m_criteria );
  62     return true;
  63
  64
  65 }
  66
  67 bool CCvSubdivMorpher::OnCalculateStereo()
  68 {
  69     CvSize size = GetImageSize( m_left_img );
  70     int i, k;
  71     bool result = false;
  72     char* status = new char[m_count];
  73
  74     m_left_gray_img = CreateIsometricImage( m_left_img, m_left_gray_img,
  75                                             m_left_img->depth, 1 );
  76     m_right_gray_img = CreateIsometricImage( m_right_img, m_right_gray_img,
  77                                              m_right_img->depth, 1 );
  78
  79     m_left_pyr = CreateIsometricImage( m_left_img, m_left_pyr,
  80                                        IPL_DEPTH_32F, 1 );
  81     m_right_pyr = CreateIsometricImage( m_right_img, m_right_pyr,
  82                                         IPL_DEPTH_32F, 1 );
  83
  84     cvCvtColor( m_left_img, m_left_gray_img, CV_BGR2GRAY );
  85     cvCvtColor( m_right_img, m_right_gray_img, CV_BGR2GRAY );
  86
  87     result = GeneratePoints();
  88     if( !result ) return result;
  89
  90     cvCalcOpticalFlowPyrLK( m_left_gray_img, m_right_gray_img,
  91                             m_left_pyr, m_right_pyr, m_left_points, m_right_points,
  92                             m_realcount, m_win_size, m_pyr_level, status, 0,
  93                             m_criteria,0);
  94
  95     m_right_gray_img->roi = m_left_gray_img->roi = m_left_img->roi;
  96
  97     for( i = 0, k = 0; i < m_realcount; i++ )
  98     {
  99         if( status[i] &&
 100             m_left_points[i].x > 1 && m_left_points[i].x < size.width - 2 &&
 101             m_left_points[i].y > 1 && m_left_points[i].y < size.height - 2  &&
 102             m_right_points[i].x > 1 && m_right_points[i].x < size.width - 2 &&
 103             m_right_points[i].y > 1 && m_right_points[i].y < size.height - 2 )
 104         {
 105             if( k < i )
 106             {
 107                 m_left_points[k] = m_left_points[i];
 108                 m_right_points[k] = m_right_points[i];
 109             }
 110             k++;
 111         }
 112     }
 113
 114     delete status;
 115
 116     m_realcount = k + 4;
 117
 118     m_right_points[k]   = m_left_points[k] = cvPoint2D32f( 1.f, 1.f );
 119     m_right_points[k+1] = m_left_points[k+1] = cvPoint2D32f( size.width - 2.f, 1.f );
 120     m_right_points[k+2] = m_left_points[k+2] = cvPoint2D32f( size.width - 2.f, size.height - 2.f );
 121     m_right_points[k+3] = m_left_points[k+3] = cvPoint2D32f( 1.f, size.height - 2.f );
 122
 123     cvInitSubdivDelaunay2D( m_subdiv, cvRect(0,0,size.width,size.height));
 124
 125     for( i = 0; i < m_realcount; i++ )
 126     {
 127         if( i == 77 )
 128         {
 129             printf("%d",i);
 130         }
 131         StereoSubdivPoint* point = (StereoSubdivPoint*)
 132             cvSubdivDelaunay2DInsert( m_subdiv, m_left_points[i] );
 133         point->another = m_right_points + i;
 134     }
 135
 136     return result;
 137 }
 138
 139
 140 bool CCvSubdivMorpher::OnCalculateVirtualImage()
 141 {
 142     CvSeqReader reader;
 143     int i, total = m_subdiv->edges->total;
 144
 145     cvZero( m_virtual_img );
 146
 147     cvStartReadSeq( (CvSeq*)(m_subdiv->edges), &reader, 0 );
 148
 149     for( i = 0; i < total; i++ )
 150     {
 151         CvQuadEdge2D* quadedge = (CvQuadEdge2D*)reader.ptr;
 152         CvSubdiv2DEdge edge = (CvSubdiv2DEdge)quadedge;
 153
 154         if( CV_IS_SET_ELEM_EXISTS( quadedge ))
 155         {
 156             if( !quadedge->pt[3] ) // left facet was not considered
 157             {
 158                 MorphFacet( edge );
 159             }
 160
 161             if( !quadedge->pt[1] ) // right facet was not considered
 162             {
 163                 MorphFacet( cvSubdiv2DSymEdge(edge));
 164             }
 165         }
 166
 167         CV_NEXT_SEQ_ELEM( sizeof(*quadedge), reader );
 168     }
 169
 170     cvSetSeqReaderPos( &reader, 0, 0 );
 171
 172     // clear "facet visited" flags
 173     for( i = 0; i < total; i++ )
 174     {
 175         CvQuadEdge2D* quadedge = (CvQuadEdge2D*)reader.ptr;
 176         quadedge->pt[1] = quadedge->pt[3] = 0;
 177         CV_NEXT_SEQ_ELEM( sizeof(*quadedge), reader );
 178     }
 179     //iplCopy( m_left_img, m_virtual_img );
 180
 181     cvDeleteMoire( m_virtual_img );
 182
 183     return true;
 184 }
 185
 186
 187 void CCvSubdivMorpher::MorphFacet( CvSubdiv2DEdge edge )
 188 {
 189 #define SHIFT      10
 190 #define DELTA      (1 << (SHIFT - 1))
 191 #define ASHIFT     8
 192 #define ADELTA     (1 << (ASHIFT - 1))
 193
 194     CvPoint2D32f left_points[3];
 195     CvPoint2D32f right_points[3];
 196     CvPoint2D32f middle_points[3];
 197     CvPoint left_c, right_c, middle_c;
 198
 199     CvSize size;
 200     int left_step = 0, right_step = 0, middle_step = 0;
 201     uchar* left_data = 0;
 202     uchar* right_data = 0;
 203     uchar* middle_data = 0;
 204     int min_len, max_len;
 205     int i, j, v;
 206     CvPoint2D32f left_pt_step, right_pt_step, middle_pt_step;
 207     float t;
 208     int alpha = cvRound(m_pan*(1 << ASHIFT));
 209     int flag = 0;
 210
 211     cvGetImageRawData( m_left_img, &left_data, &left_step, &size );
 212     cvGetImageRawData( m_right_img, &right_data, &right_step, 0 );
 213     cvGetImageRawData( m_virtual_img, &middle_data, &middle_step, 0 );
 214
 215     for( i = 0; i < 3; i++ )
 216     {
 217         StereoSubdivPoint* subdiv_pt = (StereoSubdivPoint*)cvSubdiv2DEdgeOrg( edge );
 218
 219         left_points[i] = subdiv_pt->pt;
 220         right_points[i] = subdiv_pt->another ? *subdiv_pt->another : left_points[i];
 221
 222         ((CvQuadEdge2D*)(edge & ~3))->pt[(edge + 3) & 3] = (CvSubdiv2DPoint*)1;
 223         edge = cvSubdiv2DGetEdge( edge, CV_NEXT_AROUND_LEFT );
 224
 225         middle_points[i].x = left_points[i].x + m_pan*(right_points[i].x - left_points[i].x);
 226         middle_points[i].y = left_points[i].y + m_pan*(right_points[i].y - left_points[i].y);
 227
 228         flag |= (unsigned)left_points[i].x >= (unsigned)size.width ||
 229                 (unsigned)left_points[i].y >= (unsigned)size.height;
 230     }
 231
 232     if( flag ) return;
 233
 234     // calculate min and max side length in the middle triangle
 235     v = cvRound(fabs(middle_points[0].x - middle_points[1].x) +
 236                 fabs(middle_points[0].y - middle_points[1].y));
 237
 238     min_len = max_len = v;
 239     i = 0;
 240
 241     v = cvRound(fabs(middle_points[1].x - middle_points[2].x) +
 242                 fabs(middle_points[1].y - middle_points[2].y));
 243
 244     if( v < min_len ) min_len = v, i = 1;
 245     if( v > max_len ) max_len = v;
 246
 247     v = cvRound(fabs(middle_points[0].x - middle_points[2].x) +
 248                 fabs(middle_points[0].y - middle_points[2].y));
 249
 250     if( v < min_len ) min_len = v, i = 2;
 251     if( v > max_len ) max_len = v;
 252
 253     if( i != 0 )
 254     {
 255         CvPoint2D32f t;
 256         CV_SWAP( left_points[2], left_points[i-1], t );
 257         CV_SWAP( right_points[2], right_points[i-1], t );
 258         CV_SWAP( middle_points[2], middle_points[i-1], t );
 259     }
 260
 261     left_c = cvPoint( cvRound(left_points[2].x*(1 << SHIFT)),
 262                       cvRound(left_points[2].y*(1 << SHIFT)));
 263     right_c = cvPoint( cvRound(right_points[2].x*(1 << SHIFT)),
 264                        cvRound(right_points[2].y*(1 << SHIFT)));
 265     middle_c = cvPoint( cvRound(middle_points[2].x*(1 << SHIFT)),
 266                         cvRound(middle_points[2].y*(1 << SHIFT)));
 267
 268     t = 1.f/min_len;
 269
 270     left_pt_step = cvPoint2D32f( (left_points[1].x - left_points[0].x)*t,
 271                                  (left_points[1].y - left_points[0].y)*t );
 272
 273     right_pt_step = cvPoint2D32f( (right_points[1].x - right_points[0].x)*t,
 274                                   (right_points[1].y - right_points[0].y)*t );
 275
 276     middle_pt_step = cvPoint2D32f( (middle_points[1].x - middle_points[0].x)*t,
 277                                    (middle_points[1].y - middle_points[0].y)*t );
 278
 279     t = 1.f/max_len;
 280
 281     for( i = 0; i < min_len; i++ )
 282     {
 283         CvPoint left_pt = cvPoint( cvRound(left_points[0].x*(1 << SHIFT)) + DELTA,
 284                                    cvRound(left_points[0].y*(1 << SHIFT)) + DELTA),
 285                 right_pt = cvPoint( cvRound(right_points[0].x*(1 << SHIFT)) + DELTA,
 286                                     cvRound(right_points[0].y*(1 << SHIFT)) + DELTA),
 287                 middle_pt = cvPoint( cvRound(middle_points[0].x*(1 << SHIFT)) + DELTA,
 288                                      cvRound(middle_points[0].y*(1 << SHIFT)) + DELTA);
 289
 290         CvPoint left_pt_step2 = cvPoint( cvRound((left_c.x - left_pt.x)*t),
 291                                          cvRound((left_c.y - left_pt.y)*t)),
 292                 right_pt_step2 = cvPoint( cvRound((right_c.x - right_pt.x)*t),
 293                                           cvRound((right_c.y - right_pt.y)*t)),
 294                 middle_pt_step2 = cvPoint( cvRound((middle_c.x - middle_pt.x)*t),
 295                                            cvRound((middle_c.y - middle_pt.y)*t));
 296
 297         for( j = 0; j < max_len; j++ )
 298         {
 299             uchar* left_ptr = left_data + (left_pt.y >> SHIFT)*left_step
 300                               + (left_pt.x >> SHIFT)*3;
 301             uchar* right_ptr = right_data + (right_pt.y >> SHIFT)*right_step
 302                                + (right_pt.x >> SHIFT)*3;
 303             uchar* middle_ptr = middle_data + (middle_pt.y >> SHIFT)*middle_step
 304                                 + (middle_pt.x >> SHIFT)*3;
 305
 306             int t0, t1, t2;
 307
 308             t0 = ((left_ptr[0] << ASHIFT) + alpha*(right_ptr[0] - left_ptr[0]) + ADELTA) >> ASHIFT;
 309             t1 = ((left_ptr[1] << ASHIFT) + alpha*(right_ptr[1] - left_ptr[1]) + ADELTA) >> ASHIFT;
 310             t2 = ((left_ptr[2] << ASHIFT) + alpha*(right_ptr[2] - left_ptr[2]) + ADELTA) >> ASHIFT;
 311
 312             middle_ptr[0] = (uchar)t0;
 313             middle_ptr[1] = (uchar)t1;
 314             middle_ptr[2] = (uchar)t2;
 315
 316             left_pt.x += left_pt_step2.x;
 317             left_pt.y += left_pt_step2.y;
 318             right_pt.x += right_pt_step2.x;
 319             right_pt.y += right_pt_step2.y;
 320             middle_pt.x += middle_pt_step2.x;
 321             middle_pt.y += middle_pt_step2.y;
 322         }
 323
 324         left_points[0].x += left_pt_step.x;
 325         left_points[0].y += left_pt_step.y;
 326
 327         right_points[0].x += right_pt_step.x;
 328         right_points[0].y += right_pt_step.y;
 329
 330         middle_points[0].x += middle_pt_step.x;
 331         middle_points[0].y += middle_pt_step.y;
 332     }
 333 }