#include "cv.h"  

#include "highgui.h" 

#include "cxcore.h"

#include "cvaux.h"

#include "math.h"

#ifdef _DEBUG

#pragma comment(lib,"cv200d.lib")

#pragma comment(lib,"cvaux200d.lib")

#pragma comment(lib,"cxcore200d.lib")

#pragma comment(lib,"highgui200d.lib")

#else

#pragma comment(lib,"cv200.lib")

#pragma comment(lib,"cvaux200.lib")

#pragma comment(lib,"cxcore200.lib")

#pragma comment(lib,"highgui200.lib")

#endif

CvMat * cumsum(CvMat *src,int rc)

{

 CvMat *Imdst = cvCloneMat(src);

 if (rc==1)

 {

  for(int row=1;row<src->rows;row++)

  {

   for(int col=0;col<src->cols;col++)

   {

    cvSetReal2D(Imdst,row,col,cvGetReal2D(Imdst,row-1,col)+cvGetReal2D(Imdst,row,col));

   }

  }

 }

 if (rc==2)

 {

  for(int row=0;row<src->rows;row++)

  {

   for(int col=1;col<src->cols;col++)

   {

    cvSetReal2D(Imdst,row,col,cvGetReal2D(Imdst,row,col-1)+cvGetReal2D(Imdst,row,col));

   }

  }

 }

 return Imdst;

}

CvMat * boxFilter(CvMat *src,int r)

{

 CvMat *Imdst = cvCloneMat(src);

 //imCum = cumsum(imSrc, 1);

 CvMat *imCum = cumsum(Imdst,1);

 //imDst(1:r+1, :) = imCum(1+r:2*r+1, :);

 CvMat *subMat = cvCreateMat(r+1,Imdst->cols,CV_32FC1);

 cvGetRows(imCum,subMat,r,2*r+1);//前闭后开的区间

 for (int row = 0;row<r+1;row++)

 {

  for(int col = 0;col<Imdst->cols;col++)

  {

   cvSetReal2D(Imdst,row,col,cvGetReal2D(subMat,row,col));

  }

 }

 cvReleaseMat(&subMat);

 //imDst(r+2:hei-r, :) = imCum(2*r+2:hei, :) - imCum(1:hei-2*r-1, :);

 subMat = cvCreateMat(Imdst->rows-2*r,Imdst->cols,CV_32FC1);

 cvGetRows(imCum,subMat,2*r+1,Imdst->rows);//这里是不对的第rows行没有被提取

 CvMat *subMat2 = cvCreateMat(Imdst->rows-2*r,Imdst->cols,CV_32FC1);

 cvGetRows(imCum,subMat2,0,Imdst->rows-2*r-1); 

 cvSub(subMat,subMat2,subMat2);

 for (int row = r+1;row<Imdst->rows-r;row++)

 {

  for(int col = 0;col<Imdst->cols;col++)

  {

   cvSetReal2D(Imdst,row,col,cvGetReal2D(subMat2,row-r-1,col));

  }

 }

 cvReleaseMat(&subMat);

 cvReleaseMat(&subMat2);

 //imDst(hei-r+1:hei, :) = repmat(imCum(hei, :), [r, 1]) - imCum(hei-2*r:hei-r-1, :);

 subMat = cvCreateMat(r,Imdst->cols,CV_32FC1);

 cvGetRows(imCum,subMat,r,2*r);

 CvMat *subMatOne = cvCreateMat(1,Imdst->cols,CV_32FC1);

 cvRepeat(cvGetRow(imCum,subMatOne,Imdst->rows-1),subMat);

 subMat2 = cvCreateMat(r+1,Imdst->cols,CV_32FC1);

 cvGetRows(imCum,subMat2,Imdst->rows-2*r-1,Imdst->rows-r-1);

 cvSub(subMat,subMat2,subMat2);


 for (int row = Imdst->rows-r;row<Imdst->rows;row++)

 {

  for(int col = 0;col<Imdst->cols;col++)

  {

   cvSetReal2D(Imdst,row,col,cvGetReal2D(subMat2,row+r-Imdst->rows,col));

  }

 }

 cvReleaseMat(&subMat);

 cvReleaseMat(&subMat2);


 CvMat *Imdst2= cvCloneMat(Imdst);

 //imCum = cumsum(imDst, 2);

 imCum = cumsum(Imdst2,2);

 //imDst(:, 1:r+1) = imCum(:, 1+r:2*r+1);

 subMat = cvCreateMat(Imdst2->rows,r+1,CV_32FC1);

 cvGetCols(imCum,subMat,r,2*r+1);

 for(int row=0;row<Imdst2->rows;row++)

 {

  for(int col=0;col<r+1;col++)

  {  

   cvSetReal2D(Imdst2,row,col,cvGetReal2D(subMat,row,col));

  }

 }

 cvReleaseMat(&subMat);

 //imDst(:, r+2:wid-r) = imCum(:, 2*r+2:wid) - imCum(:, 1:wid-2*r-1);

 subMat = cvCreateMat(Imdst2->rows,Imdst2->cols-2*r-1,CV_32FC1);

 subMat2 = cvCreateMat(Imdst2->rows,Imdst2->cols-2*r-1,CV_32FC1);

 cvGetCols(imCum,subMat,2*r+1,imCum->cols);

 cvGetCols(imCum,subMat2,0,imCum->cols-2*r-1);

 cvSub(subMat,subMat2,subMat2);

 for(int row=0;row<Imdst2->rows;row++)

 {

  for(int col=r+1;col<Imdst->cols-r;col++)

  {

   cvSetReal2D(Imdst2,row,col,cvGetReal2D(subMat2,row,col-r-1));

  }

 }

 cvReleaseMat(&subMat);

 cvReleaseMat(&subMat2);


 //imDst(:, wid-r+1:wid) = repmat(imCum(:, wid), [1, r]) - imCum(:, wid-2*r:wid-r-1);

 subMat = cvCreateMat(Imdst2->rows,r,CV_32FC1);

 cvGetCols(imCum,subMat,r,2*r);

 subMatOne = cvCreateMat(Imdst2->rows,1,CV_32FC1);

 cvRepeat(cvGetCol(imCum,subMatOne,Imdst->cols-1),subMat);

 subMat2 = cvCreateMat(Imdst2->rows,r,CV_32FC1);

 cvGetCols(imCum,subMat2,imCum->cols-2*r-1,imCum->cols-r-1);

 cvSub(subMat,subMat2,subMat2);

 for(int row=0;row<Imdst2->rows;row++)

 {

  for(int col=Imdst2->cols-r;col<Imdst->cols;col++)

  {

   cvSetReal2D(Imdst2,row,col,cvGetReal2D(subMat2,row,col+r-Imdst2->cols));

  }

 }

 cvReleaseMat(&subMat);

 cvReleaseMat(&subMat2);

 cvReleaseMat(&subMatOne);

 return Imdst2;


}

CV_IMPL void

cvSplitssss( const CvMat * srcarr, CvMat* dstarr0, CvMat* dstarr1, CvMat* dstarr2, CvMat* dstarr3 )

{

 for(int y=0;y<srcarr->rows;y++)

 {

  for(int x=0;x<srcarr->cols;x++)

  {

   cvSetReal2D(dstarr0,y,x,cvGet2D(srcarr,y,x).val[0]/255.00);

   if(dstarr1!=NULL&&dstarr2!=NULL)

   {

    cvSetReal2D(dstarr1,y,x,cvGet2D(srcarr,y,x).val[1]/255.00);

    cvSetReal2D(dstarr2,y,x,cvGet2D(srcarr,y,x).val[2]/255.00);

   }


  }

 }

}

CvMat * GuidedFilter_Color(CvMat * I,CvMat *pp,int r, float eps)

{

 int height = pp->rows;

 int weight = pp->cols;


 CvMat *p = cvCreateMat(height,weight,CV_32FC1);

 cvSplitssss(pp,p,NULL,NULL,NULL);

 CvMat *ones = cvCreateMat(height,weight,CV_32FC1);

 cvSet(ones,cvRealScalar(1));

 CvMat * N = boxFilter(ones,r);

 CvMat * I_b = cvCreateMat(height,weight,CV_32FC1);

 CvMat * I_g = cvCreateMat(height,weight,CV_32FC1);

 CvMat * I_r = cvCreateMat(height,weight,CV_32FC1);

 cvZero(I_r);

 cvSplitssss(I,I_r,I_g,I_b,NULL);


 CvMat * mean_I_r = cvCreateMat(height,weight,CV_32FC1);

 cvDiv(boxFilter(I_r,r),N,mean_I_r);

 CvMat * mean_I_g = cvCreateMat(height,weight,CV_32FC1);

 cvDiv(boxFilter(I_g,r),N,mean_I_g);

 CvMat * mean_I_b = cvCreateMat(height,weight,CV_32FC1);

 cvDiv(boxFilter(I_b,r),N,mean_I_b); 

 CvMat * mean_p = cvCreateMat(height,weight,CV_32FC1);

 cvDiv(boxFilter(p,r),N,mean_p);

 CvMat * pr = cvCreateMat(height,weight,CV_32FC1);

 cvMul(I_r,p,pr);

 CvMat * mean_Ip_r = cvCreateMat(height,weight,CV_32FC1);

 cvDiv(boxFilter(pr,r),N,mean_Ip_r);

 CvMat * pg = cvCreateMat(height,weight,CV_32FC1);

 cvMul(I_g,p,pg);

 CvMat * mean_Ip_g = cvCreateMat(height,weight,CV_32FC1);

 cvDiv(boxFilter(pg,r),N,mean_Ip_g);

 CvMat * pb = cvCreateMat(height,weight,CV_32FC1);

 cvMul(I_b,p,pb);

 CvMat * mean_Ip_b = cvCreateMat(height,weight,CV_32FC1);

 cvDiv(boxFilter(pb,r),N,mean_Ip_b); 

 cvMul(mean_I_r,mean_p,pr);

 cvMul(mean_I_g,mean_p,pg);

 cvMul(mean_I_b,mean_p,pb);

 CvMat * cov_Ip_r = cvCreateMat(height,weight,CV_32FC1);

 cvSub(mean_Ip_r,pr,cov_Ip_r);

 CvMat * cov_Ip_g = cvCreateMat(height,weight,CV_32FC1);

 cvSub(mean_Ip_g,pg,cov_Ip_g);

 CvMat * cov_Ip_b = cvCreateMat(height,weight,CV_32FC1);

 cvSub(mean_Ip_b,pb,cov_Ip_b);


 CvMat * var_I_rr  = cvCreateMat(height,weight,CV_32FC1);

 cvMul(I_r,I_r,pr);

 cvDiv(boxFilter(pr,r),N,var_I_rr);

 cvMul(mean_I_r,mean_I_r,pr);

 cvSub(var_I_rr,pr,var_I_rr);

 CvMat * var_I_rg  = cvCreateMat(height,weight,CV_32FC1);

 cvMul(I_r,I_g,pr);

 cvDiv(boxFilter(pr,r),N,var_I_rg);

 cvMul(mean_I_r,mean_I_g,pr);

 cvSub(var_I_rg,pr,var_I_rg);

 CvMat * var_I_rb  = cvCreateMat(height,weight,CV_32FC1);

 cvMul(I_r,I_b,pr);

 cvDiv(boxFilter(pr,r),N,var_I_rb);

 cvMul(mean_I_r,mean_I_b,pr);

 cvSub(var_I_rb,pr,var_I_rb);

 CvMat * var_I_gg  = cvCreateMat(height,weight,CV_32FC1);

 cvMul(I_g,I_g,pr);

 cvDiv(boxFilter(pr,r),N,var_I_gg);

 cvMul(mean_I_g,mean_I_g,pr);

 cvSub(var_I_gg,pr,var_I_gg);

 CvMat * var_I_gb  = cvCreateMat(height,weight,CV_32FC1);

 cvMul(I_g,I_b,pr);

 cvDiv(boxFilter(pr,r),N,var_I_gb);

 cvMul(mean_I_g,mean_I_b,pr);

 cvSub(var_I_gb,pr,var_I_gb);

 CvMat * var_I_bb  = cvCreateMat(height,weight,CV_32FC1);

 cvMul(I_b,I_b,pr);

 cvDiv(boxFilter(pr,r),N,var_I_bb);

 cvMul(mean_I_b,mean_I_b,pr);

 cvSub(var_I_bb,pr,var_I_bb);


 CvMat * Sigma = cvCreateMat(3,3,CV_32FC1);

 CvMat * cov_Ip = cvCreateMat(1,3,CV_32FC1);

 CvMat * cov_Ipo = cvCreateMat(1,3,CV_32FC1);

 CvMat * SigmaInv = cvCreateMat(3,3,CV_32FC1);

 CvMat * a_b = cvCreateMat(height,weight,CV_32FC1);

 CvMat * a_g = cvCreateMat(height,weight,CV_32FC1);

 CvMat * a_r = cvCreateMat(height,weight,CV_32FC1);

 cvZero(SigmaInv);

 for(int i=0;i<p->rows;i++)

 {

  for (int j=0;j<p->cols;j++)

  {

   cvSetReal2D(Sigma,0,0,cvGetReal2D(var_I_rr,i,j)+2*eps);

   cvSetReal2D(Sigma,0,1,cvGetReal2D(var_I_rg,i,j));

   cvSetReal2D(Sigma,0,2,cvGetReal2D(var_I_rb,i,j));

   cvSetReal2D(Sigma,1,0,cvGetReal2D(var_I_rg,i,j));

   cvSetReal2D(Sigma,1,1,cvGetReal2D(var_I_gg,i,j)+2*eps);

   cvSetReal2D(Sigma,1,2,cvGetReal2D(var_I_gb,i,j));

   cvSetReal2D(Sigma,2,0,cvGetReal2D(var_I_rb,i,j));

   cvSetReal2D(Sigma,2,1,cvGetReal2D(var_I_gb,i,j));

   cvSetReal2D(Sigma,2,2,cvGetReal2D(var_I_bb,i,j)+2*eps);


   cvSetReal2D(cov_Ip,0,0,cvGetReal2D(cov_Ip_r,i,j));

   cvSetReal2D(cov_Ip,0,1,cvGetReal2D(cov_Ip_g,i,j));

   cvSetReal2D(cov_Ip,0,2,cvGetReal2D(cov_Ip_b,i,j));

   cvInvert(Sigma,SigmaInv);

   cvMatMulAdd(cov_Ip,SigmaInv,0,cov_Ip);

   cvSetReal2D(a_r,i,j,cvGetReal2D(cov_Ip,0,0));

   cvSetReal2D(a_g,i,j,cvGetReal2D(cov_Ip,0,1));

   cvSetReal2D(a_b,i,j,cvGetReal2D(cov_Ip,0,2));

  }

 }

 cvMul(a_r,mean_I_r,pr);

 cvMul(a_g,mean_I_g,pg);

 cvMul(a_b,mean_I_b,pb);

 cvSub(mean_p,pr,mean_p);

 cvSub(mean_p,pg,mean_p);

 cvSub(mean_p,pb,mean_p);


 cvMul(boxFilter(a_r,r),I_r,I_r);

 cvMul(boxFilter(a_g,r),I_g,I_g);

 cvMul(boxFilter(a_b,r),I_b,I_b);

 cvAdd(I_r,I_g,I_r);

 cvAdd(I_r,I_b,I_r);

 cvAdd(I_r,boxFilter(mean_p,r),I_r);

 cvDiv(I_r,N,I_r);

 cvReleaseMat(&a_b);

 cvReleaseMat(&a_g);

 cvReleaseMat(&a_r);

 cvReleaseMat(&SigmaInv);

 cvReleaseMat(&cov_Ip);

 cvReleaseMat(&Sigma);

 cvReleaseMat(&var_I_bb);

 cvReleaseMat(&var_I_gb);

 cvReleaseMat(&var_I_gg);

 cvReleaseMat(&var_I_rb);

 cvReleaseMat(&var_I_rg);

 cvReleaseMat(&var_I_rr);

 cvReleaseMat(&cov_Ip_r);

 cvReleaseMat(&cov_Ip_g);

 cvReleaseMat(&cov_Ip_b);

 cvReleaseMat(&pr);

 cvReleaseMat(&pg);

 cvReleaseMat(&pb);

 cvReleaseMat(&mean_Ip_r);

 cvReleaseMat(&mean_Ip_g);

 cvReleaseMat(&mean_Ip_b);

 cvReleaseMat(&I_g);

 cvReleaseMat(&I_b);

 cvReleaseMat(&ones);

 return I_r;

}

附加比较完整的opecv guidefiltercolor