ccd.cpp
Go to the documentation of this file.
00001 // Software License Agreement (BSD License)
00002 // 
00003 //   Copyright (c) 2011, Shulei Zhu <schuleichu@gmail.com>
00004 //   All rights reserved.
00005 // 
00006 //   Redistribution and use in source and binary forms, with or without
00007 //   modification, are permitted provided that the following conditions
00008 //   are met:
00009 // 
00010 //    * Redistributions of source code must retain the above copyright
00011 //      notice, this list of conditions and the following disclaimer.
00012 //    * Redistributions in binary form must reproduce the above
00013 //      copyright notice, this list of conditions and the following
00014 //      disclaimer in the documentation and/or other materials provided
00015 //      with the distribution.
00016 //    * Neither the name of Shulei Zhu nor the names of its
00017 //      contributors may be used to endorse or promote products derived
00018 //      from this software without specific prior written permission.
00019 // 
00020 //   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 //   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 //   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 //   FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 //   COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 //   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 //   BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 //   LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 //   CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 //   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 //   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 //   POSSIBILITY OF SUCH DAMAGE.
00032 // 
00033 // 
00034 // ccd.cpp --- 
00035 // File            : ccd.cpp
00036 // Created: Sa Jun 18 14:03:47 2011 (+0200)
00037 // Author: Shulei Zhu
00038 
00039 // Code:
00040 
00041 #include "opencv/cv.h"
00042 #include "opencv/highgui.h"
00043 #include <vector>
00044 #include <iostream>
00045 #include <algorithm>
00046 #include "ccd/auto_init.h"
00047 #include "ccd/bspline.h"
00048 #include "ccd/ccd.h"
00049 cv::Mat canvas_tmp;
00050 
00051 inline double logistic(double x)
00052 {
00053   return 1.0/(1.0+exp(-x));
00054 }
00055 
00056 inline double probit(double x)
00057 {
00058   return 0.5*(1+1/sqrt(2)*erf(x));
00059 }
00060 
00061 inline cv::Scalar random_color(CvRNG* rng)
00062 {
00063   int color = cvRandInt(rng);
00064   return CV_RGB(color&255, (color>>8)&255, (color>>16)&255);
00065 }
00066 
00067 void CCD::read_params( const std::string& filename)
00068 {
00069   cv::FileStorage fs(filename, cv::FileStorage::READ);
00070   params_.gamma_1 = double(fs["gamma_1"]);      
00071   params_.gamma_2 = double(fs["gamma_2"]);      
00072   params_.gamma_3 = double(fs["gamma_3"]);      
00073   params_.gamma_4 = double(fs["gamma_4"]);      
00074   params_.alpha   = double(fs["alpha"]);
00075   params_.beta   = double(fs["beta"]);
00076   params_.kappa   = double(fs["kappa"]);        
00077   params_.c       = double(fs["c"]);            
00078   params_.h       = int(fs["h"]);            
00079   params_.delta_h = int(fs["delta_h"]);      
00080   params_.resolution = int(fs["resolution"]);   
00081   params_.degree  = int(fs["degree"]);
00082   params_.phi_dim  = int(fs["phi_dim"]);
00083   // std::cerr<< params_.gamma_1<< " ";
00084   // std::cerr<< params_.gamma_2<< " ";
00085   // std::cerr<< params_.gamma_3<< " ";
00086   // std::cerr<< params_.gamma_4<< " ";
00087   // std::cerr<< params_.alpha<< " ";
00088   // std::cerr<< params_.kappa<< " ";
00089   // std::cerr<< params_.c<< " ";
00090   // std::cerr<< params_.delta_h<< " ";
00091   // std::cerr<< params_.resolution<< " ";
00092   // std::cerr<< params_.degree<< " ";
00093   // std::cerr<< params_.phi_dim<< std::endl;
00094 }
00095 
00096 void CCD::init_mat()
00097 {
00098   Phi = cv::Mat::zeros(params_.phi_dim,1, CV_64F);
00099   Sigma_Phi = cv::Mat::zeros(params_.phi_dim,params_.phi_dim, CV_64F);
00100   delta_Phi = cv::Mat::zeros(params_.phi_dim,1, CV_64F);
00101 }
00102 
00103 void CCD::init_cov(BSpline &bs, int degree)
00104 {
00105   int n_dim = (int)pts.size() - 3;
00106   cv::Mat W = cv::Mat::zeros(2*n_dim, params_.phi_dim, CV_64F);
00107   cv::Mat U = cv::Mat::zeros(2*n_dim, 2*n_dim, CV_64F);
00108   for (int i = 0; i < n_dim; ++i)
00109   {
00110     double *W_ptr = W.ptr<double>(i);
00111     W_ptr[0] = 1;
00112     W_ptr[1] = 0;
00113     W_ptr[2] = pts[i].x;
00114     W_ptr[3] = 0;
00115     W_ptr[4] = 0;
00116     W_ptr[5] = pts[i].y;
00117     if(params_.phi_dim == 8)
00118     {
00119       W_ptr[6] = pts[i].z;
00120       W_ptr[7] = 0;
00121     }
00122     W_ptr = W.ptr<double>(i+params_.resolution);
00123     W_ptr[0] = 0;
00124     W_ptr[1] = 1;
00125     W_ptr[2] = 0;
00126     W_ptr[3] = pts[i].y;
00127     W_ptr[4] = pts[i].x;
00128     W_ptr[5] = 0;
00129     if(params_.phi_dim == 8)
00130     {
00131       W_ptr[6] = 0;
00132       W_ptr[7] = pts[i].z;
00133     }
00134   }
00135 
00136   cv::Mat tmp_mat = cv::Mat::zeros(n_dim, n_dim, CV_64F);
00137   int interval = params_.resolution/(pts.size() - degree);
00138 
00139   for (int i = 0; i < n_dim; ++i)
00140   {
00141     double *basic_mat_ptr = bs.basic_mat_.ptr<double>(i*interval);
00142     for (int m = 0; m < n_dim; ++m)
00143     {
00144       double *tmp_mat_ptr = tmp_mat.ptr<double>(m);
00145       for (int n = 0; n < n_dim; ++n)
00146         tmp_mat_ptr[n] += basic_mat_ptr[m]*basic_mat_ptr[n];
00147     }
00148   }
00149   
00150   for (int i = 0; i < n_dim; ++i)
00151   {
00152     double *tmp_mat_ptr = tmp_mat.ptr<double>(i);
00153     double *U_ptr = U.ptr<double>(i);
00154     for (int j = 0; j < n_dim; ++j)
00155     {
00156       U_ptr[j] = tmp_mat_ptr[j]/n_dim;
00157       U_ptr = U.ptr<double>(i+n_dim);
00158       U_ptr[j+n_dim] = tmp_mat_ptr[j]/n_dim;
00159     }
00160   }
00161 
00162   cv::Mat tmp_cov;
00163   cv::gemm(W, U, params_.beta,  cv::Mat(), 0, tmp_cov, cv::GEMM_1_T);
00164   cv::gemm(tmp_cov, W, 1,  cv::Mat(), 0, Sigma_Phi, 0);
00165 }
00166 
00167 
00168 void CCD::clear()
00169 {
00170   vic.release();
00171   mean_vic.release();
00172   cov_vic.release();
00173   nv.release();
00174   Phi.release();
00175   Sigma_Phi.release();
00176   delta_Phi.release();
00177   bs_old.release();
00178   nabla_E.release();
00179   hessian_E.release();
00180   image.release();
00181   canvas.release();
00182   if(!tpl.empty()) tpl.release();
00183 }
00184 
00185 
00186 void CCD::local_statistics(BSpline &bs)
00187 {
00188   cv::Mat_<cv::Vec3b>& img = (cv::Mat_<cv::Vec3b>&)image;
00189   // std::cout << params_.gamma_1 << " " << params_.gamma_2 << " " << params_.gamma_3 << " " << params_.gamma_4 << std::endl;
00190   double sigma = params_.h/(params_.alpha*params_.gamma_3);
00191   // sigma_hat = gamma_3 * sigma
00192   
00193   //  double sigma_hat = max(h/sqrt(2*gamma_2), gamma_4);
00194   double sigma_hat = params_.gamma_3*sigma + params_.gamma_4;
00195 
00196   // to save the normalized parameters of vic[i,8]
00197   // dimension: resolution x 2
00198   // the first column save the normalized coefficient outside the curve
00199   // the second column store the one inside the curve
00200   cv::Mat normalized_param = cv::Mat::zeros(params_.resolution, 2, CV_64F);
00201   
00202   vic = cv::Mat::zeros(params_.resolution, 20*floor(params_.h/params_.delta_h), CV_64F);
00203 
00204   // temporary points used to store those points in the
00205   // normal direction as well as negative normal direction
00206   cv::Point3d tmp1, tmp2;
00207 
00208   // store the distance from a point in normal(negative norml) direction
00209   // to the point on the curve
00210   cv::Point3d tmp_dis1, tmp_dis2;
00211 
00212   CvRNG rng;
00213   cv::Scalar color = random_color(&rng);
00214   for(int i=0; i < params_.resolution;i++)
00215   {
00216     // cv::circle(canvas, cv::Point2d(bs[i].x, bs[i].y), 1,color, 1);
00217     double *nv_ptr = nv.ptr<double>(i);
00218     // normal vector (n_x, n_y)
00219     // tagent vector (nv.at<double>(i,1), -n_x)
00220     nv_ptr[0] = -bs.dt(i).y/cvSqrt(bs.dt(i).x*bs.dt(i).x + bs.dt(i).y*bs.dt(i).y);
00221     nv_ptr[1] = bs.dt(i).x/cvSqrt(bs.dt(i).x*bs.dt(i).x + bs.dt(i).y*bs.dt(i).y);
00222 
00223     // dimension = 4
00224     // 0 - x value
00225     // 1 - y value
00226     // 2 - normal vector x
00227     // 3 - normal vector y
00228     bs_old = cv::Mat::zeros(params_.resolution, 4, CV_64F);
00229     double *bs_old_ptr = bs_old.ptr<double>(i);
00230 
00231     // save the old value of bspline
00232     bs_old_ptr[0] = bs[i].x;
00233     bs_old_ptr[1] = bs[i].y;
00234 
00235     // save the old normal vector of bspline
00236     bs_old_ptr[2] = nv_ptr[0];
00237     bs_old_ptr[3] = nv_ptr[1];
00238     
00239 
00240     // std::cout << nv_ptr[0] << " " << nv_ptr[1] << std::endl;
00241     int k = 0;
00242     double alpha = 0.5;
00243     double *vic_ptr = vic.ptr<double>(i);
00244     for (int j = params_.delta_h; j <= params_.h; j += params_.delta_h, k++)
00245     {
00247       // calculate in the direction +n: (n_x, n_y)
00249 
00250       // x_{k,l}
00251       tmp1.x = round(bs[i].x + j*nv_ptr[0]);
00252 
00253       // y_{k,l}
00254       tmp1.y = round(bs[i].y + j*nv_ptr[1]);
00255 
00256       // cv::circle(canvas_tmp, cv::Point2d(tmp1.x, tmp1.y), 1, CV_RGB(255,0,0), 1);
00257 
00258       // distance between x_{k,l} and x_{k,0} in the normal direction
00259       // appoximately it is l*h, l = {1,2,3,.....}
00260       tmp_dis1.x = (tmp1.x-bs[i].x)*nv_ptr[0] + (tmp1.y-bs[i].y)*nv_ptr[1];
00261 
00262       // distance between y_{k,l} and y_{k,0} along the curve
00263       // it approximates 0
00264       tmp_dis1.y = (tmp1.x-bs[i].x)*nv_ptr[1] - (tmp1.y-bs[i].y)*nv_ptr[0];
00265       
00266       vic_ptr[10*k + 0] = tmp1.y;
00267       vic_ptr[10*k + 1] = tmp1.x;
00268       vic_ptr[10*k + 2] = tmp_dis1.x;
00269       vic_ptr[10*k + 3] = tmp_dis1.y;
00270 
00271       // fuzzy assignment a(d_{k,l}) = 1/2*(erf(d_{kl})/\sqrt(2)*sigma) + 1/2
00272       // vic_ptr[10*k + 4] = 0.5*(erf((tmp_dis1.x)/(sqrt(2)*sigma)) + 1);
00273       vic_ptr[10*k + 4] = logistic(tmp_dis1.x/(sqrt(2)*sigma));
00274 
00275       // wp1 = (a_{d,l} - gamm_1) /(1-gamma_1)
00276       double wp1 = (vic_ptr[10*k + 4] - params_.gamma_1)/(1-params_.gamma_1);
00277 
00278       // wp1^4, why? if a_{d,l} \approx 0.5, do not count the point
00279       vic_ptr[10*k + 5] = wp1*wp1*wp1*wp1;
00280 
00281       // wp1 = (1-a_{d,l} - gamm_1) /(1-gamma_1)
00282       // double wp2 = (1-vic_ptr[10*k + 4] - gamma_1)/(1-gamma_1);
00283       double wp2 = (1-vic_ptr[10*k + 4] - 0.25);
00284       vic_ptr[10*k + 6] = -64*wp2*wp2*wp2*wp2 + 0.25;
00285 
00286       // W_p(d_p, simga_p) = c*max[0, exp(-d_p^2/2*sigma_p'^2) - exp(-gamma_2))]
00287       vic_ptr[10*k + 7] = std::max((exp(-0.5*tmp_dis1.x*tmp_dis1.x/(sigma_hat*sigma_hat)) - exp(-params_.gamma_2)), 0.0);
00288 
00289       // W' = 0.5*exp(-|d_v= - d_p=|/alpha)/alpha
00290       vic_ptr[ 10*k + 8] = 0.5*exp(-abs(tmp_dis1.y)/alpha)/alpha;
00291       
00292       // the derivative of col_5: 1/(sqrt(2*PI)*sigma)*exp{-d_{k,l}^2/(2*sigma*sigma)}
00293       vic_ptr[ 10*k + 9] = exp(-tmp_dis1.x*tmp_dis1.x/(2*sigma*sigma))/(sqrt(2*CV_PI)*sigma);
00294 
00295       // m1_debug[0] += img(tmp1.y, tmp1.x)[0];
00296       // m1_debug[1] += img(tmp1.y, tmp1.x)[1];
00297       // m1_debug[2] += img(tmp1.y, tmp1.x)[2];
00298       // calculate the normalization parameter c 
00299       normalized_param.at<double>(i, 0) += vic_ptr[ 10*k + 7];
00300 
00301         
00302 #ifdef DEBUG
00303       if(i == 0)
00304         std::cout << "tmp1 " << tmp1.x  << " " << tmp1.y << std::endl;
00305 #endif
00306       
00307       // cv::circle(img1, tmp1, 1, CV_RGB(0, 255, 255), 1, 8 , 0);
00308 
00309       // for (int m = 0; m < 3; ++m)
00310       // {
00311       //   vic_pixels.at<Vec3b>(i, k)[m] = img(tmp1.y, tmp1.x)[m];          
00312       // }
00313 
00315       // calculate in the direction -n: (-n_x, -n_y)
00317       tmp2.x = round(bs[i].x - j*nv_ptr[0]);
00318       tmp2.y = round(bs[i].y - j*nv_ptr[1]);
00319       // cv::circle(canvas_tmp, cv::Point2d(tmp2.x, tmp2.y), 1, CV_RGB(255,0,0), 1);
00320 #ifdef DEBUG
00321       if(i == 0)
00322         std::cout << "tmp2 " << tmp2.x  << " " << tmp2.y << std::endl;
00323 #endif
00324 
00325       // start compute the size in the direction of -(n_x, n_y)
00326       tmp_dis2.x = (tmp2.x-bs[i].x)*nv_ptr[0] + (tmp2.y-bs[i].y)*nv_ptr[1];
00327       tmp_dis2.y = (tmp2.x-bs[i].x)*nv_ptr[1] - (tmp2.y-bs[i].y)*nv_ptr[0];
00328       int negative_normal = k + (int)floor(params_.h/params_.delta_h);
00329       vic_ptr[10*negative_normal + 0] = tmp2.y;
00330       vic_ptr[10*negative_normal + 1] = tmp2.x;
00331       vic_ptr[10*negative_normal + 2] = tmp_dis2.x;
00332       vic_ptr[10*negative_normal + 3] = tmp_dis2.y;
00333       // vic_ptr[10*negative_normal + 4] = 0.5*(erf(tmp_dis2.x/(cvSqrt(2)*sigma)) + 1);
00334       vic_ptr[10*negative_normal + 4] = logistic(tmp_dis2.x/(sqrt(2)*sigma));
00335       // vic_ptr[10*negative_normal + 4] = 0.5;
00336       wp1 = (vic_ptr[10*negative_normal + 4] - 0.25);
00337       vic_ptr[10*negative_normal + 5] = -64*wp1*wp1*wp1*wp1 + 0.25;
00338       wp2 = (1 - vic_ptr[10*negative_normal + 4] - params_.gamma_1)/(1-params_.gamma_1);
00339       vic_ptr[10*negative_normal + 6] = wp2*wp2*wp2*wp2;
00340       vic_ptr[10*negative_normal + 7] = std::max((exp(-0.5*tmp_dis2.x*tmp_dis2.x/(sigma_hat*sigma_hat)) - exp(-params_.gamma_2)), 0.0);
00341       vic_ptr[ 10*negative_normal + 8] = 0.5*exp(-abs(tmp_dis2.x)/alpha)/alpha;
00342       vic_ptr[ 10*negative_normal + 9] = exp(-tmp_dis2.x*tmp_dis2.x/(2*sigma*sigma))/(sqrt(2*CV_PI)*sigma);
00343       normalized_param.at<double>(i, 1) += vic_ptr[ 10*negative_normal + 7];
00344     }
00345   }
00346 
00347 
00348 #ifdef DEBUG
00349   printf("%-5s  %-5s  %-5s  %-5s  %-5s  %-5s  %-5s  %-5s  %-5s  %-5s\n",
00350          "x", "y", "dist_x", "dist_y", "a", "w1^4", "w2^4", "prox", "edf", "erf'"
00351          );
00352   for (int  i = 0; i < 20*floor(params_.h/params_.delta_h); ++i)
00353   {
00354     // std::cout << vic.at<double>(0,i) << "    ";
00355     printf("%-5f   ", vic.at<double>(0,i));
00356     if((i+1)%10 == 0)
00357       std::cout << std::endl;
00358   }
00359 #endif
00360 
00361   for (int i = 0; i < params_.resolution; ++i)
00362   {
00363     int k = 0;
00364     // w1 = \sum wp_1, w2 = \sum wp_2
00365     double w1 =0.0 , w2 = 0.0;
00366 
00367     // store mean value near the curve
00368     std::vector<double> m1(3,0.0), m2(3,0.0);
00369     
00370     // store the second mean value near the curve
00371     std::vector<double> m1_o2(9,0.0), m2_o2(9,0.0);
00372 
00374     // compute local statistics
00375     // ////////////////////////////////////////////////////////////////////
00376     // start search the points in the +n direction as well as -n direction
00377     double wp1 = 0.0, wp2 = 0.0;
00378 
00379     double *vic_ptr = vic.ptr<double>(i);
00380     double *mean_vic_ptr = mean_vic.ptr<double>(i);
00381     double *cov_vic_ptr = cov_vic.ptr<double>(i);
00382     for (int j = params_.delta_h; j <= params_.h; j += params_.delta_h, k++)
00383     {
00384       wp1 = 0.0, wp2 = 0.0;
00385       int negative_normal = k + (int)floor(params_.h/params_.delta_h);
00386       
00387       // wp1 = w(a_{k,l})*w(d_{k,l})*w(d)
00388       wp1 = vic_ptr[ 10*k+ 5]*vic_ptr[ 10*k+ 7]/normalized_param.at<double>(i,0);
00389 
00390       // wp2 = w(a_{k,l})*w(d_{k,l})*w(d)
00391       wp2 = vic_ptr[ 10*k+ 6]*vic_ptr[ 10*k+ 7]/normalized_param.at<double>(i,1);;
00392 
00393       //w1 = \sum{wp1}
00394       w1 += wp1;
00395 
00396       //w2 = \sum{wp2}
00397       w2 += wp2;
00398 
00399       // compute the mean value in the vicinity of a point
00400       // m_{ks} = I{k}^{s} = \sum_{l} w_{kls}{I_{kl}} : s = 1 or 2
00401 
00402       m1[0] += wp1*img(vic_ptr[ 10*k + 0 ], vic_ptr[ 10*k + 1 ])[0];
00403       m1[1] += wp1*img(vic_ptr[ 10*k + 0 ], vic_ptr[ 10*k + 1 ])[1];
00404       m1[2] += wp1*img(vic_ptr[ 10*k + 0 ], vic_ptr[ 10*k + 1 ])[2];
00405       m2[0] += wp2*img(vic_ptr[ 10*k + 0 ], vic_ptr[ 10*k + 1 ])[0];
00406       m2[1] += wp2*img(vic_ptr[ 10*k + 0 ], vic_ptr[ 10*k + 1 ])[1];
00407       m2[2] += wp2*img(vic_ptr[ 10*k + 0 ], vic_ptr[ 10*k + 1 ])[2];
00408 
00409       // compute second order local statistics
00410       // m_{k,s} = \sum_{l} w_{kls} I_{kl}*I_{kl}^T
00411 
00412       for (int m = 0; m < 3; ++m)
00413       {
00414         for (int n =0; n < 3; ++n)
00415         {
00416           m1_o2[m*3+n] += wp1*img(vic_ptr[ 10*k + 0 ], vic_ptr[ 10*k + 1 ])[m]
00417                           *img(vic_ptr[ 10*k + 0 ], vic_ptr[ 10*k + 1 ])[n];
00418           m2_o2[m*3+n] += wp2*img(vic_ptr[ 10*k + 0 ], vic_ptr[ 10*k + 1 ])[m]
00419                           *img(vic_ptr[ 10*k + 0 ], vic_ptr[ 10*k + 1 ])[n];
00420         }
00421       }
00422         
00423       wp1 = vic_ptr[ 10*negative_normal+ 5]*vic_ptr[ 10*negative_normal+ 7]/normalized_param.at<double>(i,0);
00424       wp2 = vic_ptr[ 10*negative_normal+ 6]*vic_ptr[ 10*negative_normal+ 7]/normalized_param.at<double>(i,1);
00425       
00426       w1 += wp1;
00427       w2 += wp2;
00428       
00429       m1[0] += wp1*img(vic_ptr[10*negative_normal+0], vic_ptr[10*negative_normal+1])[0];
00430       m1[1] += wp1*img(vic_ptr[10*negative_normal+0], vic_ptr[10*negative_normal+1])[1];
00431       m1[2] += wp1*img(vic_ptr[10*negative_normal+0], vic_ptr[10*negative_normal+1])[2];
00432       m2[0] += wp2*img(vic_ptr[10*negative_normal+0], vic_ptr[10*negative_normal+1])[0];
00433       m2[1] += wp2*img(vic_ptr[10*negative_normal+0], vic_ptr[10*negative_normal+1])[1];
00434       m2[2] += wp2*img(vic_ptr[10*negative_normal+0], vic_ptr[10*negative_normal+1])[2];
00435       
00436       for (int m = 0; m < 3; ++m)
00437       {
00438         for (int n =0; n < 3; ++n)
00439         {
00440           m1_o2[m*3+n] += wp1*img(vic_ptr[ 10*negative_normal + 0 ], vic_ptr[ 10*negative_normal + 1 ])[m]
00441                           *img(vic_ptr[ 10*negative_normal + 0 ], vic_ptr[ 10*negative_normal + 1 ])[n];
00442           m2_o2[m*3+n] += wp2*img(vic_ptr[ 10*negative_normal + 0 ], vic_ptr[ 10*negative_normal + 1 ])[m]
00443                           *img(vic_ptr[ 10*negative_normal + 0 ], vic_ptr[ 10*negative_normal + 1 ])[n];
00444         }
00445       }
00446     }
00447     // std::cout << "w1: " << "              w2:" << std::endl;
00448     // std::cout << "w1 == " << w1 << "  w2== " << w2 << std::endl;
00449     mean_vic_ptr[0] = m1[0]/w1;
00450     mean_vic_ptr[1] = m1[1]/w1;
00451     mean_vic_ptr[2] = m1[2]/w1;
00452     mean_vic_ptr[3] = m2[0]/w2;
00453     mean_vic_ptr[4] = m2[1]/w2;
00454     mean_vic_ptr[5] = m2[2]/w2;
00455     
00456     for (int m = 0; m < 3; ++m)
00457     {
00458       for (int n = 0 ; n < 3; ++n)
00459       {
00460         cov_vic_ptr[ m*3+n] = m1_o2[m*3+n]/w1 -m1[m]*m1[n]/(w1*w1);
00461         cov_vic_ptr[ 9+m*3+n] = m2_o2[m*3+n]/w2 -m2[m]*m2[n]/(w2*w2);
00462         if(m == n)
00463         {
00464           cov_vic_ptr[ m*3+n] += params_.kappa;
00465           cov_vic_ptr[ 9+m*3+n] += params_.kappa;
00466         }
00467       }
00468     }
00469   }
00470   normalized_param.release();
00471 }
00472 
00473 
00474 void CCD::refine_parameters(BSpline &bs)
00475 {
00476   cv::Mat_<cv::Vec3b>& img = (cv::Mat_<cv::Vec3b>&)image;
00477   cv::Mat tmp_cov = cv::Mat::zeros(3,3,CV_64F);
00478   cv::Mat tmp_cov_inv = cv::Mat::zeros(3,3,CV_64F);
00479   cv::Mat tmp_jacobian = cv::Mat::zeros(params_.phi_dim,3,CV_64F);
00480   cv::Mat tmp_pixel_diff = cv::Mat::zeros(3, 1, CV_64F);
00481 
00482   // std::cout << "dimension: " << Sigma_Phi.cols << " " << Sigma_Phi.rows << std::endl;
00483   
00484   for (int i = 0; i < params_.resolution; ++i)
00485   {
00486     double *vic_ptr = vic.ptr<double>(i);
00487     double *nv_ptr = nv.ptr<double>(i);
00488     double *mean_vic_ptr = mean_vic.ptr<double>(i);
00489     double *cov_vic_ptr = cov_vic.ptr<double>(i);
00490     double normal_points_number = floor(params_.h/params_.delta_h);
00491     for (int j = 0; j < 2*normal_points_number; ++j)
00492     {
00493       tmp_cov = cv::Mat::zeros(3,3,CV_64F);
00494       tmp_cov_inv = cv::Mat::zeros(3,3,CV_64F);
00495       
00496       for (int m = 0; m < 3; ++m)
00497       {
00498         double *tmp_cov_ptr = tmp_cov.ptr<double>(m);
00499         for (int n = 0; n < 3; ++n)
00500         {
00501           tmp_cov_ptr[n] = vic_ptr[10*j+4] * cov_vic_ptr[m*3+n] +(1-vic_ptr[10*j+4])* cov_vic_ptr[m*3+n+9];
00502         }
00503       }
00504       
00505       tmp_cov_inv = tmp_cov.inv(cv::DECOMP_SVD);
00506  
00507       tmp_pixel_diff = cv::Mat::zeros(3, 1, CV_64F);
00508 
00509       //compute the difference between I_{kl} and \hat{I_{kl}}
00510       for (int m = 0; m < 3; ++m)
00511       {
00512         tmp_pixel_diff.at<double>(m,0) = img(vic_ptr[10*j+0], vic_ptr[10*j+1])[m]- vic_ptr[10*j+4] * mean_vic_ptr[m]- (1-vic_ptr[10*j+4])* mean_vic_ptr[m+3];
00513       }
00514       
00515       //compute jacobian matrix        
00516       tmp_jacobian = cv::Mat::zeros(params_.phi_dim,3,CV_64F);
00517  
00518       for (int n = 0; n < 3; ++n)
00519       {
00520         tmp_jacobian.at<double>(0,n) = vic_ptr[10*j + 9]*(mean_vic_ptr[n] - mean_vic_ptr[n+3])*nv_ptr[0];
00521         tmp_jacobian.at<double>(1,n) = vic_ptr[10*j + 9]*(mean_vic_ptr[n] - mean_vic_ptr[n+3])*nv_ptr[1];
00522         tmp_jacobian.at<double>(2,n) = vic_ptr[10*j + 9]*(mean_vic_ptr[n] - mean_vic_ptr[n+3])*nv_ptr[0]*bs[i].x;
00523         tmp_jacobian.at<double>(3,n) = vic_ptr[10*j + 9]*(mean_vic_ptr[n] - mean_vic_ptr[n+3])*nv_ptr[1]*bs[i].y;
00524         tmp_jacobian.at<double>(4,n) = vic_ptr[10*j + 9]*(mean_vic_ptr[n] - mean_vic_ptr[n+3])*nv_ptr[1]*bs[i].x;
00525         tmp_jacobian.at<double>(5,n) = vic_ptr[10*j + 9]*(mean_vic_ptr[n] - mean_vic_ptr[n+3])*nv_ptr[0]*bs[i].y;
00526         if(params_.phi_dim == 8)
00527         {
00528           tmp_jacobian.at<double>(6,n) = vic_ptr[10*j + 9]*(mean_vic_ptr[n] - mean_vic_ptr[n+3])*nv_ptr[0]*bs[i].z;
00529           tmp_jacobian.at<double>(7,n) = vic_ptr[10*j + 9]*(mean_vic_ptr[n] - mean_vic_ptr[n+3])*nv_ptr[1]*bs[i].z;
00530         }
00531         // std::cout << mean_vic_ptr[n] << " " << mean_vic_ptr[n+3] << std::endl;
00532       }
00533       
00534       //\nabla{E_2} = \sum J * \Sigma_{kl}^{-1} * (I_{kl} - \hat{I_{kl}})
00535       nabla_E += tmp_jacobian*tmp_cov_inv*tmp_pixel_diff;
00536       //Hessian{E_2} = \sum J * \Sigma_{kl}^{-1} * J
00537       hessian_E += tmp_jacobian*tmp_cov_inv*tmp_jacobian.t();
00538     }
00539   }
00540 
00541   cv::Mat Sigma_Phi_inv = Sigma_Phi.inv(cv::DECOMP_SVD);
00542   hessian_E += Sigma_Phi_inv;
00543   nabla_E += 2*Sigma_Phi_inv*Phi;
00544 
00545   cv::Mat hessian_E_inv = hessian_E.inv(cv::DECOMP_SVD);
00546   delta_Phi = hessian_E_inv*nabla_E;
00547   // #ifdef DEBUG
00548   // std::cout << delta_Phi.at<double>(0,0) << " "
00549   //           << delta_Phi.at<double>(1,0) << " " 
00550   //           << delta_Phi.at<double>(2,0) << " " 
00551   //           << delta_Phi.at<double>(3,0) << " " 
00552   //           << delta_Phi.at<double>(4,0) << " " 
00553   //           << delta_Phi.at<double>(5,0) << " ";
00554   // if(params_.phi_dim == 8)
00555   // {
00556   //   std::cout<< delta_Phi.at<double>(6,0) << " " 
00557   //            << delta_Phi.at<double>(7,0) << " ";
00558   // }
00559   // std::cout<< std::endl;
00560   // #endif
00561   // cv::norm(delta_Phi);
00562   
00563   Phi -= delta_Phi;
00564   Sigma_Phi = params_.c*Sigma_Phi + 2*(1-params_.c)*hessian_E_inv;
00565 
00566   Sigma_Phi_inv.release();
00567   hessian_E_inv.release();
00568   tmp_cov.release();
00569   tmp_cov_inv.release();
00570   tmp_jacobian.release();
00571   tmp_pixel_diff.release();
00572 }
00573 
00574 void CCD::run_ccd()
00575 {
00576   // store the control points trasformed in the shape space
00577   int iter = 0;
00578   double tol = 0.0;
00579   double tol_old = 0.0;
00580   bool convergence = false;
00581   double norm = 0.0;
00582 
00583   do{
00584     // update model parameters
00585     // for (int i = 0; i < 6; ++i)
00586     //   Phi.at<double>(i,0) = Phi.at<double>(i,0) - delta_Phi.at<double>(i,0);
00587 
00588     // update the control points in terms of the change of
00589     // model parameters    
00590     for (size_t i = 0; i < pts.size(); ++i)
00591     {
00592       // C = W*\Phi + C_0
00593       //           1  0  x_0  0  0  y_0
00594       //     C = [                       ][\phi_0 \phi_1 \phi_2 \phi_3 \phi_4 \phi_5 ]^T + C_0
00595       //           0  1  0   y_0 x_0  0
00596       //
00597       pts[i].x = Phi.at<double>(0,0) + (1+Phi.at<double>(2,0))*pts[i].x + Phi.at<double>(5,0)*pts[i].y;
00598       if(params_.phi_dim == 8)
00599         pts[i].x +=  Phi.at<double>(6,0)*pts[i].z;
00600       pts[i].y = Phi.at<double>(1,0) + (1+Phi.at<double>(3,0))*pts[i].y + Phi.at<double>(4,0)*pts[i].x;
00601       if(params_.phi_dim == 8)
00602         pts[i].y += Phi.at<double>(7,0)*pts[i].z;
00603       pts[i].z = pts[i].z;
00604     }
00605 
00606     
00607     nv = cv::Mat::zeros(params_.resolution, 2, CV_64F);
00608     mean_vic = cv::Mat::zeros(params_.resolution, 6, CV_64F);
00609     cov_vic = cv::Mat::zeros(params_.resolution, 18, CV_64F);
00610     nabla_E = cv::Mat::zeros(params_.phi_dim,1, CV_64F);
00611     hessian_E = cv::Mat::zeros(params_.phi_dim,params_.phi_dim, CV_64F);
00612 
00613     // create a new B-spline curve
00614     BSpline bs(params_.degree , params_.resolution, pts);
00615     image.copyTo(canvas_tmp);
00616 
00617     for (int i = 0; i < params_.resolution; ++i)
00618     {
00619         int j = (i+1)%params_.resolution;
00620         cv::line(canvas_tmp, cv::Point2d(bs[i].x, bs[i].y),cv::Point2d(bs[j].x, bs[j].y),CV_RGB(255, 0, 0 ),2,8,0);
00621         // cv::line(canvas, cv::Point2d(bs[i].x, bs[i].y),cv::Point2d(bs[j].x, bs[j].y),CV_RGB( 0, 0, 255 ),2,8,0);
00622     }
00623 
00624     // converge condition
00625     // tol = \int (r - r_f)*n
00626     tol = 0.0;
00627     if(iter > 0)
00628     {
00629       for (int k = 0; k < params_.resolution; ++k)
00630       {
00631         tol += pow((bs[k].x - bs_old.at<double>(k, 0))*bs_old.at<double>(k, 2) +
00632                    (bs[k].y - bs_old.at<double>(k, 1))*bs_old.at<double>(k, 3), 2);
00633       }
00634       // if(iter > 1)
00635       // params_.h = std::max(params_.h/log(tol_old/tol),10.0);
00636       tol_old = tol;
00637     }
00638     local_statistics(bs);
00639     
00640     refine_parameters(bs);
00641 
00642     norm =  0.0;
00643     for (int i = 0; i < params_.phi_dim; ++i){
00644       if(i == 0 || i == 1)
00645         norm += delta_Phi.at<double>(i, 0)*delta_Phi.at<double>(i, 0)/10000;
00646       else
00647         norm += delta_Phi.at<double>(i, 0)*delta_Phi.at<double>(i, 0);
00648     }
00649     norm = cv::sqrt(norm);
00650     std::cerr << "iter: " << iter << "   tol: " << tol  << " norm: " << cv::norm(delta_Phi, cv::NORM_L2)  << " norm_tmp:" << norm << " params.h: " << params_.h << std::endl;
00651     // if(iter == 19)
00652     //   for (int i = 0; i < params_.resolution; ++i){
00653     //     int j = (i+1)%params_.resolution;
00654     //     cv::line(canvas, cv::Point2d(bs[i].x, bs[i].y),cv::Point2d(bs[j].x, bs[j].y),CV_RGB( 0, 0, 255 ),2,8,0);        
00655     //   }
00656 
00657     std::stringstream name;
00658     name << iter;
00659     cv::imwrite(name.str() + ".png", canvas_tmp);
00660     canvas_tmp.release();
00661     // cv::imwrite(name.str() + ".png", canvas);
00662 
00663     // cv::imshow("CCD", canvas);    
00664     // cv::waitKey(200);
00665 
00666     if(iter >=1 && tol < 1 && params_.h > 10)
00667     {
00668       params_.h = params_.h/sqrt(2);
00669     }
00670     
00671     if(iter >= 1 && tol < 0.001)
00672     {
00673       for (int i = 0; i < params_.resolution; ++i)
00674       {
00675         // std::cout << bs[i].x << " " << bs[i].y << " " <<bs[i].z << std::endl;
00676         int j = (i+1)%params_.resolution;
00677         // cv::line(canvas_tmp, cv::Point2d(bs[i].x, bs[i].y),cv::Point2d(bs[j].x, bs[j].y),CV_RGB( 255, 0, 0 ),2,8,0);
00678         cv::line(canvas, cv::Point2d(bs[i].x, bs[i].y),cv::Point2d(bs[j].x, bs[j].y),CV_RGB( 255, 0, 0 ),2,8,0);
00679         // cv::circle(canvas_tmp, cv::Point2d(bs[i].x, bs[i].y), 2 ,CV_RGB(255,0,0), 2); 
00680         // cv::circle(canvas, cv::Point2d(bs[i].x, bs[i].y), 1 ,CV_RGB(0,255,0), 1); 
00681       }
00682       convergence = true;
00683       init_cov(bs, params_.degree);
00684     }
00685     iter += 1;
00686     // bs.release();
00687   }while(!convergence);
00688 }
 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