最小二乘算法——自写函数

lsm(std:: vector<double> Av, std:: vector<double> Lv,const double initX )

{  

         double D = initX;//初值

         int vilidCount = Av.size();//A矩阵

 

          CvMat *A = cvCreateMat(vilidCount,1,CV_64F);  

          CvMat *AT = cvCreateMat(A->cols,A->rows,CV_64F);

          CvMat *ATA = cvCreateMat(AT->rows,A->cols,CV_64F);

          CvMat *ATA_inver = cvCreateMat(ATA->rows,ATA->cols,CV_64F);

          CvMat *X = cvCreateMat(A->cols,1,CV_64F);

　　　 CvMat *L = cvCreateMat(A->rows,X->cols,CV_64F);  CvMat *ATL = cvCreateMat(AT->rows,L->cols,CV_64F);

　　    CvMat *DX = cvCreateMat(X->rows,X->cols,CV_64F);

　　   cvZero(A);

　　   CV_MAT_ELEM(*X,double,0,0) = initX ; //初始化

 

        int iterCnt =0;

        while(1)  

       {   

           double D_ = CV_MAT_ELEM(*X,double,0,0);  

           for(int i =0 ; i< vilidCount; i++ )   

                 {

                         double d = Lv[i] ;//得出的视差函数    

                        double Distance = Av[i] ;//求每个像素的深度

                        CV_MAT_ELEM(*L,double,i,0) = -( Distance /D_ -d );  

                         CV_MAT_ELEM(*A,double,i,0) = -Distance / pow(D_,2);

                      }

　　　　　　  cvTranspose(A,AT);   cvMatMul(AT,A,ATA);   cvMatMul(AT,L,ATL);

 　　　　　　 cvInvert(ATA,ATA_inver);

 　　　　　　 cvMatMul(ATA_inver,ATL,DX);

　　　　　　  CV_MAT_ELEM(*X,double,0,0) = CV_MAT_ELEM(*DX,double,0,0) +CV_MAT_ELEM(*X,double,0,0);

　　　　　　  iterCnt ++;

 　　　　　　 if (CV_MAT_ELEM(*DX,double,0,0)<0.1    || iterCnt >10    ||vilidCount<3    /* ||1*/    )  

                  {

  　　　　　　　 break;  

                    }

                        }  

　　　　　　if (iterCnt>10)

                 {   

                    D = initX;

                      }  

                  else

                    {

                          D = CV_MAT_ELEM(*X,double,0,0);

                        }

 　　　　　　cvReleaseMat(&A);

                  cvReleaseMat(&AT);

                   cvReleaseMat(&ATA);

                   cvReleaseMat(&ATA_inver);

                   cvReleaseMat(&ATL);

                    cvReleaseMat(&L);

                    cvReleaseMat(&X);  

                    cvReleaseMat(&DX);

                   if (D<0)  

                   {   

                           D = 0;

                        return false;  

                          }

 

                           return D; }