matrix.cpp
Go to the documentation of this file.
00001 #include "ccd_panin.h"
00002 using namespace cv;
00003 cv::Mat img1;
00004 
00005 inline double ccd_det(uchar *ptr, int offset)
00006 {
00007   return ptr[offset+0]*(ptr[offset+4]*ptr[offset+8] - ptr[offset+5]*ptr[offset+7])
00008       *ptr[offset+1]*(ptr[offset+5]*ptr[offset+6] - ptr[offset+3]*ptr[offset+8])
00009       *ptr[offset+2]*(ptr[offset+3]*ptr[offset+7] - ptr[offset+4]*ptr[offset+6]);
00010 }
00011 
00012 int main (int argc, char * argv[]) 
00013 {
00014   // if (argc < 2)
00015   // {
00016   //   printf("Usage %s image.png \n", argv[0]);
00017   //   exit(0);
00018   // }
00019   // 
00020   // the count of points on curve, equidistant distributed
00021   const int resolution = 50;
00022   
00023   // the degree of B-Spline curve
00024   int t = 3;
00025   // load image from a specified file
00026   //img1= cvLoadImage(argv[1], 1);
00027   // img1 = imread(argv[1], 1);
00028   // cv::Mat img = imread(argv[1], 1);  
00029 
00030   img1 = imread("../data/ball.png", 1);
00031   cv::Mat img = imread("../data/ball.png", 1);
00032   // convert the image into Mat fortmat
00033   //cv::Mat img(img1);
00034 
00035   // store the control points trasformed in the shape space
00036   CvPoint2D64f pts_tmp;
00037 
00039   // mannaully initialize the control points
00041   cv::namedWindow("Original", 1);
00042   // cvSetMouseCallback( "Original", on_mouse, 0 );
00043   // cvShowImage("Original",img1);
00044   cv::imshow("Original", img1);
00045   char key ;
00046 
00047 
00048 
00049 
00050 
00051 
00052 
00053 
00054 
00055 
00056 
00057 
00058 
00059 
00060   // covariance matrix of model parameters
00061   // dimension: 6x6
00062   cv::Mat Sigma_Phi;
00063   Sigma_Phi = Mat::zeros(6,6, CV_64F);
00064 
00065   // srand(time(0));
00066   // for (int m = 0; m < 6; ++m)
00067   // {
00068   //   for (int n = 0; n < 6; ++n)
00069   //   {
00070   //     if(m == n)
00071   //       Sigma_Phi.at<double>(m,n) = double_rand(0.1, 3.0);
00072   //       // Sigma_Phi.at<double>(m,n) = 1.0;
00073   //     printf("%-5f ", Sigma_Phi.at<double>(m,n));
00074   //   }
00075   //   std::cout << std::endl;
00076   // }
00077 
00078   // mean value of vicinity regions of points on the curve
00079   // dimension: resolution x 6
00080   // the first 3 are r,g,b mean values outside the curve
00081   // the last 3 are r,g,b mean values inside the curve
00082   cv::Mat mean_vic(resolution, 6, CV_64F);
00083 
00084   // covariance matrix of vicinity regions of points on the curve
00085   // dimension: resolution x 18
00086   // the first 9 (3x3) are values outside the curve
00087   // the last 9 (3x3) are values in -n direction
00088   cv::Mat cov_vic(resolution, 18, CV_64F);
00089 
00090   //\nabla_E = \nabla_E_1 + \nabla_E_2
00091   //         = 2*(\Sigma_\Phi^*)^{-1}*\Phi
00092   //         - \sum_{k,l}J_{a_1} \hat{\Sigma_{k,l}^{-1}} (I_{k,l} - \hat{I}_{k,l}) 
00093   cv::Mat nabla_E(6,1, CV_64F);
00094   
00095   //\hessian_E = hessian_E_1 + hessian_E_2
00096   //           = (\Sigma_\Phi^*)^{-1} +
00097   //           \sum_{k,l} J_{a_1} \hat{\Sigma_{k,l}^{-1}} J_{a_1}
00098   cv::Mat hessian_E(6,6, CV_64F);
00099   
00100   // \hat{\Sigma_{k,l}}
00101   cv::Mat tmp_cov(3, 3, CV_64F);
00102   
00103   // \hat{\Sigma_{k,l}}^{-1}
00104   cv::Mat tmp_cov_inv(3,3, CV_64F);
00105   
00106   // J_{a_1} \hat{\Sigma_{k,l}}
00107   cv::Mat tmp_jacobian(6,3,CV_64F);
00108   cv::Mat tmp_pixel_diff(3,1,CV_64F);
00109 
00110   cv::Mat nv(resolution, 2, CV_64F);
00111 
00112   // temporary points used to store those points in the
00113   // normal direction as well as negative normal direction
00114   CvPoint tmp1, tmp2;
00115 
00116   // store the distance from a point in normal(negative norml) direction
00117   // to the point on the curve
00118   CvPoint2D64f tmp_dis1, tmp_dis2;
00119 
00120   // double *basic_ptr = bs.basic_mat_.ptr<double>(0);
00121   // for (size_t i = 0; i < pts.size(); ++i){
00122   //   std::cout << basic_ptr[i] << " ";
00123   // }
00124   // std::cout << std::endl;
00125 
00126   // h: search radius in the normal direction
00127   // delta_h: distance step in the normal direction
00128   int h = 40, delta_h = 2;
00129 
00130   // sigma_hat = gamma_3 * sigma
00131   //  double sigma_hat = max(h/sqrt(2*gamma_2), gamma_4);
00132   double sigma_hat = h/2.5;
00133 
00134   // some useful parameters, give in hanek's paper 
00135   double gamma_1 = 0.5, gamma_2 = 4, gamma_3 = 4;
00136 
00137   //double sigma_t = max(double(h/cvSqrt(2*gamma_2)), gamma_4);
00138   double sigma = sigma_hat/gamma_3;
00139   
00140   // locate covariance formula:
00141   // Sigma_v,s =  M_s^2(d^=) / omega_(d_v^=) - m_v,s * (m_v,s)^t + kappa*I
00142   // here, I is a identy matrix, to avoid signular
00143   double kappa = 0.5;
00144 
00145 
00146   
00147 
00148   // set the size of dimension2 for Mat::vic
00149   // 8 represents x,y, delta_Phi(distance to curve), dy(distance to curve)
00150 
00151   // count the points in normal direction(only one side)
00152   int normal_points_number = floor(h/delta_h);
00153 
00154 
00155   //vicinity matrix ,in cluding plenty amount of information
00156   // dimension-2: count(normal_points) * 10*2
00157   // col_1, col_2: coordinates of x and y
00158   // col_3, col_4: the distance between a normal points and the point on the curve d_v(x), d_v(y)
00159   // col_5: the probability P_v,1(x, m_phi, sigma_hat) = 0.5*erf(d_v(x)/(sqrt(2)*sigma_hat))
00160   // TODO: how to calculate sigma_v
00161   // col_6: the probability of pixel p to belong to the desired side s.
00162   //        W_s(p) = max(0, [a-gamm1)/(1-gamma1)]^4)
00163   // col_7, col_8 : evaluates the proximity of pixel p to the curve
00164   //        W_p(d_p, simga_p) = c*max[0, exp(-d_p^2/2*sigma_p'^2) - exp(-gamma_2))]
00165   //        sigma_p' = gamma_3*sigma_p + gamma_4
00166   //        W_sp = W_s * W_p
00167   // col_9:  access the distance |d_v= - d_p=| between pixel p and pixel v along the curve
00168   //       W' = 0.5*exp(-|d_v= - d_p=|/alpha)/alpha
00169   // col_10: the derivative of col_5: 1/(sqrt(2*PI)*sigma)*exp{-d_{k,l}^2/(2*sigma_hat*sigma_hat)}
00170   // so last omega_ps = W_s * W' 
00171   cv::Mat vic(resolution, 20*normal_points_number, CV_64F);
00172 
00173   // to save the normalized parameters of vic[i,8]
00174   // dimension: resolution x 2
00175   // the first column save the normalized coefficient outside the curve
00176   // the second column store the one inside the curve
00177   cv::Mat normalized_param(resolution, 2, CV_64F);
00178 
00179   // while (1)
00180   // {
00181   //   key = cvWaitKey(10);
00182   //   if (key == 27) break;
00183   // }
00185   std::vector<CvPoint2D64f> pts;  
00186   pts_tmp.x = 173, pts_tmp.y = 299;
00187   pts.push_back(pts_tmp);
00188   pts_tmp.x = 177, pts_tmp.y = 323;
00189   pts.push_back(pts_tmp);
00190   pts_tmp.x = 189, pts_tmp.y = 343;
00191   pts.push_back(pts_tmp);
00192   pts_tmp.x = 206, pts_tmp.y = 353;
00193   pts.push_back(pts_tmp);
00194   pts_tmp.x = 223, pts_tmp.y = 360;
00195   pts.push_back(pts_tmp);
00196   pts_tmp.x = 246, pts_tmp.y = 362;
00197   pts.push_back(pts_tmp);
00198   pts_tmp.x = 267, pts_tmp.y = 352;
00199   pts.push_back(pts_tmp);
00200   pts_tmp.x = 282, pts_tmp.y = 335;
00201   pts.push_back(pts_tmp);
00202   pts_tmp.x = 295, pts_tmp.y = 315;
00203   pts.push_back(pts_tmp);
00204   pts_tmp.x = 294, pts_tmp.y = 290;
00205   pts.push_back(pts_tmp);
00206   pts_tmp.x = 289, pts_tmp.y = 268;
00207   pts.push_back(pts_tmp);
00208   pts_tmp.x = 276, pts_tmp.y = 249;
00209   pts.push_back(pts_tmp);
00210   pts_tmp.x = 248, pts_tmp.y = 238;
00211   pts.push_back(pts_tmp);
00212   pts_tmp.x = 214, pts_tmp.y = 239;
00213   pts.push_back(pts_tmp);
00214   pts_tmp.x = 192, pts_tmp.y = 254;
00215   pts.push_back(pts_tmp);
00216   pts_tmp.x = 177, pts_tmp.y = 276;
00217   pts.push_back(pts_tmp);
00218   pts_tmp.x = 173, pts_tmp.y = 299;
00219   pts.push_back(pts_tmp);
00220   pts_tmp.x = 177, pts_tmp.y = 323;
00221   pts.push_back(pts_tmp);
00222   pts_tmp.x = 189, pts_tmp.y = 343;
00223   pts.push_back(pts_tmp);
00224 
00225   std::cout<<  "number of points: " << pts.size() << std::endl;
00226   // for closed curves, we have to append 3 more points
00227   // to the end, these 3 new points are the three one
00228   // located in the head of the array
00229   // if(pts.size() > 3)
00230   // {
00231   //   pts.push_back(pts[0]);
00232   //   pts.push_back(pts[1]);
00233   //   pts.push_back(pts[2]);
00234   // }
00235 
00236   // for debug
00237 #ifdef DEBUG
00238   for (size_t i = 0; i < pts.size(); ++i)
00239   {
00240     std::cout<< pts[i].x << " " << pts[i].y << std::endl;
00241   }
00242 #endif
00243 
00244   
00245 
00246   
00247   // model parameters, it is a 6x1 matrix
00248   cv::Mat Phi = Mat::zeros(6,1, CV_64F);
00249   // Phi.zeros(6, 1, CV_64F);
00250   // Phi.at<double>(3,0) = 0.25;
00251   // std::cout << " phi 0: " << Phi.at<double>(0,0) << " phi 1: " << Phi.at<double>(1,0) << std::endl;  
00252   // \delta_Phi: the difference of model parameters
00253   // between two iteration steps
00254   cv::Mat delta_Phi = Mat::zeros(6,1, CV_64F);
00255   // delta_Phi.zeros(6,1, CV_64F);
00256   // delta_Phi.at<double>(0,0) = -13.0;
00257   // delta_Phi.at<double>(1,0) = -15.0;
00258   // delta_Phi.at<double>(2,0) = 0.05;
00259   // delta_Phi.at<double>(3,0) = 0.05;
00260   // delta_Phi.at<double>(4,0) = -0.1;
00261   // delta_Phi.at<double>(5,0) = 0.2;
00262   // delta_Phi.at<double>(2,0) = -0.26;
00263   // delta_Phi.at<double>(5,0) = 0.22;
00264 
00265   // covariance matrix of model parameters
00266   // dimension: 6x6
00267   
00268     // update model parameters
00269   for (int i = 0; i < 6; ++i)
00270     Phi.at<double>(i,0) = Phi.at<double>(i,0) - delta_Phi.at<double>(i,0);
00271 
00272   for (size_t i = 0; i < pts.size(); ++i)
00273   {
00274     pts_tmp.x = Phi.at<double>(0,0) + (1+Phi.at<double>(2,0))*pts[i].x + Phi.at<double>(5,0)*pts[i].y;
00275     pts_tmp.y = Phi.at<double>(1,0) + (1+Phi.at<double>(3,0))*pts[i].y + Phi.at<double>(4,0)*pts[i].x;
00276 
00277     pts[i].x = round(pts_tmp.x);
00278     pts[i].y = round(pts_tmp.y);
00279   }
00280   
00281 //     BSpline bs(t , resolution, pts);
00282   
00283 //     for(int i=0;i < resolution;i++)
00284 //     {
00285     
00286 //       cv::circle( img1, bs[i], 2, CV_RGB(0,0, 255),1);
00287     
00288 // #ifdef DEBUG
00289 //       std::cout << bs[i].x  << " " << bs[i].y << std::endl;
00290 //       //ROS_DEBUG_STREAM(bs[i].x  << " " << bs[i].y);
00291 // #endif
00292 //     }
00293 
00295     // cvShowImage("Original",img1);
00296   
00297   
00298   
00299     for (size_t i = 0; i < pts.size(); ++i)
00300     {
00301       // C = W*\Phi + C_0
00302       //           1  0  x_0  0  0  y_0
00303       //     C = [                       ][\phi_0 \phi_1 \phi_2 \phi_3 \phi_4 \phi_5 ]^T + C_0
00304       //           0  1  0   y_0 x_0  0
00305       //
00306       pts_tmp.x = Phi.at<double>(0,0) + (1+Phi.at<double>(2,0))*pts[i].x + Phi.at<double>(5,0)*pts[i].y;
00307       pts_tmp.y = Phi.at<double>(1,0) + (1+Phi.at<double>(3,0))*pts[i].y + Phi.at<double>(4,0)*pts[i].x;
00308       pts[i].x = round(pts_tmp.x);
00309       pts[i].y = round(pts_tmp.y);
00310     }
00311   
00312     nv = Mat::zeros(resolution, 2, CV_64F);
00313     mean_vic = Mat::zeros(resolution, 6, CV_64F);
00314     cov_vic = Mat::zeros(resolution, 18, CV_64F);
00315     nabla_E = Mat::zeros(6,1, CV_64F);
00316     hessian_E = Mat::zeros(6,6, CV_64F);
00317 
00318     // Phi.zeros(6,1,CV_64F);
00319 
00320 
00321   
00322     // create a new B-spline curve: degree =2
00323     BSpline bs(t , resolution, pts);
00324 
00325 
00326 
00327 
00328     // converge condition
00329     // tol = \int (r - r_f)*n
00330 
00331   
00332     for(int i=0;i < resolution;i++)
00333     {
00334     
00335       cv::circle( img1, bs[i], 2, CV_RGB(0,0, 255),2);
00336     
00337 #ifdef DEBUG
00338       std::cout << bs[i].x  << " " << bs[i].y << std::endl;
00339       //ROS_DEBUG_STREAM(bs[i].x  << " " << bs[i].y);
00340 #endif
00341     
00342       // normal vector (n_x, n_y)
00343       // tagent vector (nv.at<double>(i,1), -n_x)
00344       nv.at<double>(i,0) = -bs.dt(i).y/cvSqrt(bs.dt(i).x*bs.dt(i).x + bs.dt(i).y*bs.dt(i).y);
00345       nv.at<double>(i,1) = bs.dt(i).x/cvSqrt(bs.dt(i).x*bs.dt(i).x + bs.dt(i).y*bs.dt(i).y);
00346 
00347       // save the old value of bspline
00348       // bs_old.at<double>(i,0) = bs[i].x;
00349       // bs_old.at<double>(i,1) = bs[i].y;
00350 
00351       // // save the old normal vector of bspline
00352       // bs_old.at<double>(i,2) = nv.at<double>(i,0);
00353       // bs_old.at<double>(i,3) = nv.at<double>(i,1);
00354     
00355 
00356       // std::cout << nv.at<double>(i,0) << " " << nv.at<double>(i,1) << std::endl;
00357       int k = 0;
00358       double alpha = 0.5;
00359       for (int j = delta_h; j <= h; j+=delta_h, k++)
00360       {
00362         // calculate in the direction +n: (n_x, n_y)
00364 
00365         // x_{k,l}
00366         tmp1.x = round(bs[i].x + j*nv.at<double>(i,0));
00367 
00368         // y_{k,l}
00369         tmp1.y = round(bs[i].y + j*nv.at<double>(i,1));
00370 
00371         // distance between x_{k,l} and x_{k,0} in the normal direction
00372         // appoximately it is l*h, l = {1,2,3,.....}
00373         tmp_dis1.x = (tmp1.x-bs[i].x)*nv.at<double>(i,0) + (tmp1.y-bs[i].y)*nv.at<double>(i,1);
00374 
00375         // distance between y_{k,l} and y_{k,0} along the curve
00376         // it approximates 0
00377         tmp_dis1.y = (tmp1.x-bs[i].x)*nv.at<double>(i,1) - (tmp1.y-bs[i].y)*nv.at<double>(i,0);
00378       
00379         vic.at<double>(i,10*k + 0) = tmp1.y;
00380         vic.at<double>(i,10*k + 1) = tmp1.x;
00381         vic.at<double>(i,10*k + 2) = tmp_dis1.x;
00382         vic.at<double>(i,10*k + 3) = tmp_dis1.y;
00383 
00384         // fuzzy assignment a(d_{k,l}) = 1/2*(erf(d_{kl})/\sqrt(2)*sigma) + 1/2
00385         vic.at<double>(i,10*k + 4) = 0.5*(erf((tmp_dis1.x)/(sqrt(2)*sigma)) + 1);
00386 
00387         // wp1 = (a_{d,l} - gamm_1) /(1-gamma_1)
00388         double wp1 = (vic.at<double>(i,10*k + 4) - gamma_1)/(1-gamma_1);
00389 
00390         // wp1^4, why? if a_{d,l} \approx 0.5, do not count the point
00391         vic.at<double>(i,10*k + 5) = wp1*wp1*wp1*wp1;
00392 
00393         // wp1 = (1-a_{d,l} - gamm_1) /(1-gamma_1)
00394         // double wp2 = (1-vic.at<double>(i,10*k + 4) - gamma_1)/(1-gamma_1);
00395         double wp2 = (1-vic.at<double>(i,10*k + 4) - 0.25);
00396         vic.at<double>(i,10*k + 6) = -64*wp2*wp2*wp2*wp2 + 0.25;
00397 
00398         // W_p(d_p, simga_p) = c*max[0, exp(-d_p^2/2*sigma_p'^2) - exp(-gamma_2))]
00399         vic.at<double>(i,10*k + 7) = max((exp(-0.5*tmp_dis1.x*tmp_dis1.x/(sigma_hat*sigma_hat)) - exp(-gamma_2)), 0.0);
00400 
00401         // W' = 0.5*exp(-|d_v= - d_p=|/alpha)/alpha
00402         vic.at<double>(i, 10*k + 8) = 0.5*exp(-abs(tmp_dis1.y)/alpha)/alpha;
00403       
00404         // the derivative of col_5: 1/(sqrt(2*PI)*sigma)*exp{-d_{k,l}^2/(2*sigma*sigma)}
00405         vic.at<double>(i, 10*k + 9) = exp(-tmp_dis1.x*tmp_dis1.x/(2*sigma*sigma))/(sqrt(2*CV_PI)*sigma);
00406       
00407         // calculate the normalization parameter c 
00408         normalized_param.at<double>(i, 0) += vic.at<double>(i, 10*k + 7);
00409 
00410 #ifdef DEBUG
00411         if(i == 0)
00412           std::cout << "tmp1 " << tmp1.x  << " " << tmp1.y << std::endl;
00413 #endif
00414       
00415         // cv::circle(img1, tmp1, 1, CV_RGB(0, 255, 255), 1, 8 , 0);
00416 
00418         // calculate in the direction -n: (-n_x, -n_y)
00420         tmp2.x = round(bs[i].x - j*nv.at<double>(i,0));
00421         tmp2.y = round(bs[i].y - j*nv.at<double>(i,1));
00422 
00423 #ifdef DEBUG
00424         if(i == 0)
00425           std::cout << "tmp2 " << tmp2.x  << " " << tmp2.y << std::endl;
00426 #endif
00427 
00428         // start compute the size in the direction of -(n_x, n_y)
00429         tmp_dis2.x = (tmp2.x-bs[i].x)*nv.at<double>(i,0) + (tmp2.y-bs[i].y)*nv.at<double>(i,1);
00430         tmp_dis2.y = (tmp2.x-bs[i].x)*nv.at<double>(i,1) - (tmp2.y-bs[i].y)*nv.at<double>(i,0);
00431         int negative_normal = k+normal_points_number;
00432         vic.at<double>(i,10*negative_normal + 0) = tmp2.y;
00433         vic.at<double>(i,10*negative_normal + 1) = tmp2.x;
00434         vic.at<double>(i,10*negative_normal + 2) = tmp_dis2.x;
00435         vic.at<double>(i,10*negative_normal + 3) = tmp_dis2.y;
00436         vic.at<double>(i,10*negative_normal + 4) = 0.5*(erf(tmp_dis2.x/(cvSqrt(2)*sigma)) + 1);
00437         wp1 = (vic.at<double>(i,10*negative_normal + 4) - 0.25);
00438         vic.at<double>(i,10*negative_normal + 5) = -64*wp1*wp1*wp1*wp1 + 0.25;
00439         wp2 = (1 - vic.at<double>(i,10*negative_normal + 4) - gamma_1)/(1-gamma_1);
00440         vic.at<double>(i,10*negative_normal + 6) = wp2*wp2*wp2*wp2;
00441         vic.at<double>(i,10*negative_normal + 7) = max((exp(-0.5*vic.at<double>(i,10*negative_normal + 2)*vic.at<double>(i,10*negative_normal + 2)/(sigma_hat*sigma_hat)) - exp(-gamma_2)), 0.0);
00442         vic.at<double>(i, 10*negative_normal + 8) = 0.5*exp(-abs(tmp_dis2.x)/alpha)/alpha;
00443         vic.at<double>(i, 10*negative_normal + 9) = exp(-tmp_dis2.x*tmp_dis2.x/(2*sigma*sigma))/(sqrt(2*CV_PI)*sigma);
00444         //      vic.at<double>(i, 10*k + 10) = ;
00445         normalized_param.at<double>(i, 1) += vic.at<double>(i, 10*negative_normal + 7);
00446         // cv::circle(img1, tmp2, 1, CV_RGB(0, 255, 255), 1, 8 , 0);
00447       }
00448     }
00449   
00450 
00451     //#ifdef DEBUG
00452     printf("%-5s  %-5s  %-5s  %-5s  %-5s  %-5s  %-5s  %-5s  %-5s  %-5s\n",
00453            "x", "y", "dist_x", "dist_y", "a", "w1^4", "w2^4", "prox", "edf", "erf'"
00454            );
00455     for (int  i = 0; i < 20*normal_points_number; ++i)
00456     {
00457       // std::cout << vic.at<double>(0,i) << "    ";
00458       printf("%-5f   ", vic.at<double>(0,i));
00459       if((i+1)%10 == 0)
00460         std::cout << std::endl;
00461     }
00462     //#endif
00463   
00465     // cvShowImage("Original",img1);
00466     cv::imshow("Original", img1);
00467   
00468   
00469     while (1)
00470     {
00471       key = cvWaitKey(10);
00472       if (key == 27) break;
00473     }
00475   
00476     // cv::Mat sigma_phi(6,6, CV_64F);
00477 
00478     // calculate the local statistics: mean and covariance
00479     // mean_vic = M_s(d_v=)/omega_s(d_v=)
00480     // cov_vic = M_s(d_v=)^2/omega_s(d_v=) - m_v,s * (m_v,s)' + kappa*I
00481     // a_vic[0] = c*Sigma_x(p_v,s(x, m_phi, sigma_phi)); a_vic[1] = 1-a_vic[0]
00482     // where:
00483     // omega_s(d_v=) = Simage(omega_p,s(d_v=))
00484     // M_s(d_v=) = Simga(omega_p,s(d_v=) * I_p), I_p is the pixel value in the point (x,y)
00485     // M_s(d_v=)^2 = Simga(omega_p,s(d_v=) * I_p*I_p'), I_p is the pixel value in the point (x,y)
00486     // here s = 1 or 2, where the 2rd dimesion is 3*3 and 10*3
00487     // we use last 3 or 10 elments to save the result
00488 
00489   
00490     for (int i = 0; i < resolution; ++i)
00491     {
00492       int k = 0;
00493       // w1 = \sum wp_1, w2 = \sum wp_2
00494       double w1 =0.0 , w2 = 0.0;
00495 
00496       // store mean value near the curve
00497       vector<double> m1(3,0.0), m2(3,0.0);
00498     
00499       // store the second mean value near the curve
00500       vector<double> m1_o2(9,0.0), m2_o2(9,0.0);
00501 
00503       // compute local statistics
00504       // ////////////////////////////////////////////////////////////////////
00505       // start search the points in the +n direction as well as -n direction
00506       for (int j = delta_h; j <= h; j+=delta_h, k++)
00507       {
00508         double wp1 = 0.0, wp2 = 0.0;
00509       
00510         int negative_normal = k + normal_points_number;
00511       
00512         // wp1 = w(a_{k,l})*w(d_{k,l})*w(d)
00513         wp1 = vic.at<double>(i, 10*k+ 5)*vic.at<double>(i, 10*k+ 7)*vic.at<double>(i, 10*k+ 8) / normalized_param.at<double>(i,0);
00514 
00515         // wp2 = w(a_{k,l})*w(d_{k,l})*w(d)
00516         wp2 = vic.at<double>(i, 10*k+ 6)*vic.at<double>(i, 10*k+ 7)*vic.at<double>(i, 10*k+ 8) / normalized_param.at<double>(i,1);
00517 
00518         //w1 = \sum{wp1}
00519         w1 += wp1;
00520 
00521         //w2 = \sum{wp2}
00522         w2 += wp2;
00523 
00524         // compute the mean value in the vicinity of a point
00525         // m_{ks} = I{k}^{s} = \sum_{l} w_{kls}{I_{kl}} : s = 1 or 2
00526         m1[0] += wp1*img.at<Vec3b>(vic.at<double>(i, 10*k + 0 ), vic.at<double>(i, 10*k + 1 ))[0];
00527         m1[1] += wp1*img.at<Vec3b>(vic.at<double>(i, 10*k + 0 ), vic.at<double>(i, 10*k + 1 ))[1];
00528         m1[2] += wp1*img.at<Vec3b>(vic.at<double>(i, 10*k + 0 ), vic.at<double>(i, 10*k + 1 ))[2];
00529         m2[0] += wp2*img.at<Vec3b>(vic.at<double>(i, 10*k + 0 ), vic.at<double>(i, 10*k + 1 ))[0];
00530         m2[1] += wp2*img.at<Vec3b>(vic.at<double>(i, 10*k + 0 ), vic.at<double>(i, 10*k + 1 ))[1];
00531         m2[2] += wp2*img.at<Vec3b>(vic.at<double>(i, 10*k + 0 ), vic.at<double>(i, 10*k + 1 ))[2];
00532 
00533         // compute second order local statistics
00534         // m_{k,s} = \sum_{l} w_{kls} I_{kl}*I_{kl}^T
00535 
00536         for (int m = 0; m < 3; ++m)
00537         {
00538           for (int n =0; n < 3; ++n)
00539           {
00540             m1_o2[m*3+n] += wp1*img.at<Vec3b>(vic.at<double>(i, 10*k + 0 ), vic.at<double>(i, 10*k + 1 ))[m]
00541                             *img.at<Vec3b>(vic.at<double>(i, 10*k + 0 ), vic.at<double>(i, 10*k + 1 ))[n];
00542             m2_o2[m*3+n] += wp2*img.at<Vec3b>(vic.at<double>(i, 10*k + 0 ), vic.at<double>(i, 10*k + 1 ))[m]
00543                             *img.at<Vec3b>(vic.at<double>(i, 10*k + 0 ), vic.at<double>(i, 10*k + 1 ))[n];
00544           }
00545         }
00546 
00547         wp2 = vic.at<double>(i, 10*negative_normal+ 5)*vic.at<double>(i, 10*negative_normal+ 7)*vic.at<double>(i, 10*negative_normal+ 8);
00548         wp1 = vic.at<double>(i, 10*negative_normal+ 6)*vic.at<double>(i, 10*negative_normal+ 7)*vic.at<double>(i, 10*negative_normal+ 8);
00549       
00550         w1 += wp1;
00551         w2 += wp2;
00552       
00553         m1[0] += wp1*img.at<Vec3b>(vic.at<double>(i, 10*negative_normal + 0 ), vic.at<double>(i, 10*negative_normal + 1 ))[0];
00554         m1[1] += wp1*img.at<Vec3b>(vic.at<double>(i, 10*negative_normal + 0 ), vic.at<double>(i, 10*negative_normal + 1 ))[1];
00555         m1[2] += wp1*img.at<Vec3b>(vic.at<double>(i, 10*negative_normal + 0 ), vic.at<double>(i, 10*negative_normal + 1 ))[2];
00556         m2[0] += wp2*img.at<Vec3b>(vic.at<double>(i, 10*negative_normal + 0 ), vic.at<double>(i, 10*negative_normal + 1 ))[0];
00557         m2[1] += wp2*img.at<Vec3b>(vic.at<double>(i, 10*negative_normal + 0 ), vic.at<double>(i, 10*negative_normal + 1 ))[1];
00558         m2[2] += wp2*img.at<Vec3b>(vic.at<double>(i, 10*negative_normal + 0 ), vic.at<double>(i, 10*negative_normal + 1 ))[2];
00559       
00560         for (int m = 0; m < 3; ++m)
00561         {
00562           for (int n =0; n < 3; ++n)
00563           {
00564             m1_o2[m*3+n] += wp1*img.at<Vec3b>(vic.at<double>(i, 10*negative_normal + 0 ), vic.at<double>(i, 10*negative_normal + 1 ))[m]
00565                             *img.at<Vec3b>(vic.at<double>(i, 10*negative_normal + 0 ), vic.at<double>(i, 10*negative_normal + 1 ))[n];
00566             m2_o2[m*3+n] += wp2*img.at<Vec3b>(vic.at<double>(i, 10*negative_normal + 0 ), vic.at<double>(i, 10*negative_normal + 1 ))[m]
00567                             *img.at<Vec3b>(vic.at<double>(i, 10*negative_normal + 0 ), vic.at<double>(i, 10*negative_normal + 1 ))[n];
00568           }
00569         }
00570       }
00571     
00572       mean_vic.at<double>(i, 0) = m1[0]/w1;
00573       mean_vic.at<double>(i, 1) = m1[1]/w1;
00574       mean_vic.at<double>(i, 2) = m1[2]/w1;
00575       mean_vic.at<double>(i, 3) = m2[0]/w2;
00576       mean_vic.at<double>(i, 4) = m2[1]/w2;
00577       mean_vic.at<double>(i, 5) = m2[2]/w2;
00578     
00579       for (int m = 0; m < 3; ++m)
00580       {
00581         for (int n = 0 ; n < 3; ++n)
00582         {
00583           cov_vic.at<double>(i, m*3+n) = m1_o2[m*3+n]/w1 -m1[m]*m1[n]/(w1*w1);
00584           cov_vic.at<double>(i, 9+m*3+n) = m2_o2[m*3+n]/w2 -m2[m]*m2[n]/(w2*w2);
00585           if(m == n)
00586           {
00587             cov_vic.at<double>(i, m*3+n) += kappa;
00588             cov_vic.at<double>(i, 9+m*3+n) += kappa;
00589           }
00590         }
00591       }
00592     }
00593 
00594     //debug
00595     //#ifdef DEBUG
00596     for (int i = 0; i < resolution; ++i)
00597     {
00598       std::cout << mean_vic.at<double>(i, 0) << " "
00599                 << mean_vic.at<double>(i, 1) << " "
00600                 << mean_vic.at<double>(i, 2) << " "
00601                 << mean_vic.at<double>(i, 3) << " "
00602                 << mean_vic.at<double>(i, 4) << " "
00603                 << mean_vic.at<double>(i, 5) << " "
00604                 << std::endl;
00605     }
00606     //#endif
00608 
00609     double cost1 = 0.0, cost2 = 0.0;
00610     for (int i = 0; i < resolution; ++i)
00611     {
00612       for (int j = 0; j < 2*normal_points_number; ++j)
00613       {
00614         tmp_cov = Mat::zeros(3,3,CV_64F);
00615         tmp_cov_inv = Mat::zeros(3,3,CV_64F);
00616       
00617         // \hat{}\Sigma_{kl}} = a_{kl}\Sigma_{k}^{1} + (1 - a_{kl})\Sigma_{k}^{2}
00618         for (int m = 0; m < 3; ++m)
00619         {
00620           for (int n = 0; n < 3; ++n)
00621           {
00622             tmp_cov.at<double>(m, n) = vic.at<double>(i,10*j+4) * cov_vic.at<double>(i,m*3+n)
00623                                        +(1-vic.at<double>(i,10*j+4))* cov_vic.at<double>(i,m*3+n+9);
00624           }
00625         }
00626 
00627         tmp_cov_inv = tmp_cov.inv(DECOMP_SVD);
00628                 tmp_pixel_diff = Mat::zeros(3, 1, CV_64F);
00629                 
00630         // std::cout << " pixel_diff: " ;
00631         //compute the difference between I_{kl} and \hat{I_{kl}}
00632         for (int m = 0; m < 3; ++m)
00633         {
00634           tmp_pixel_diff.at<double>(m,0) = img.at<Vec3b>(vic.at<double>(i,10*j+0), vic.at<double>(i,10*j+1))[m]- vic.at<double>(i,10*j+4) * mean_vic.at<double>(i,m)- (1-vic.at<double>(i,10*j+4))* mean_vic.at<double>(i,m+3);
00635         }
00636         // std::cout << "debug " << std::endl;
00637         cv::Mat tmp_res = Mat::zeros(1,1, CV_64F);
00638         tmp_res = tmp_pixel_diff.t()*tmp_cov_inv*tmp_pixel_diff;
00639         cost2 += exp(-0.5*tmp_res.at<double>(0,0))/cv::determinant(tmp_cov);
00640       }
00641     }
00642 
00643     std::cout << "cost 2 : " << cost2 << std::endl;
00644 
00645   Phi.release();
00646   delta_Phi.release();
00647   // img1.release();
00648   return 0;
00649 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


contracting_curve_density_algorithm
Author(s): Shulei Zhu, Dejan Pangercic
autogenerated on Thu May 23 2013 14:41:45