如何用摄像头来测距（opencv）

转自：http://blog.csdn.net/xylary/archive/2007/10/25/1843809.aspx

如何用摄像头来测距（opencv）

作者：郭世龙

      在摄像头下面固定一个激光笔，就构成了这个简易的测距装置。看一下图吧。

  原 理

 
假设激光束是与摄像头的光轴完全平行，激光束的中心落点在在摄像头的视域中是最亮的点。激光束照射到摄像头视域中的跟踪目标上，那么摄像头可以捕捉到这个
点，通过简单的图像处理的方法，可以在这侦图像中找到激光束照射形成的最亮点，同时可以计算出Y轴上方向上从落点到图像中心的象素的个数。这个落点越接近
图像的中心，被测物体距离机器人就越远。由下图图可以计算距离D：

（1）

  等式中h是一个常量，是摄像头与激光发射器之间的垂直距离，可以直接测量获得。

θ可通过下式计算：

θ=Num*Rop+Offset      （2）                          

其中：Num是从图像中心到落点的像素个数

Rop是每个像素的弧度值

Offset是弧度误差

合并以上等式可以得到：

（3）

Num
可以从图像上计算得到。Rop和Offset需要通过实验计算获得。首先测量出D的准确值，然后根据等式（1）可以计算出准确的θ，根据等式（2）可到只
含有参数Rop和Offset的方程。在不同的距离多次测量D的准确值计算θ，求解方程组可以求出Rop和Offset。这里
Rop＝0.0030354，Offset＝0.056514344。

程 序

头文件：

class LaserRange 

{

public:

struct RangeResult * GetRange(IplImage * imgRange,IplImage * imgDst);

 LaserRange();

 virtual ~LaserRange();

 

private:

    unsigned   int maxW;

   unsigned   int maxH;

    unsigned   int MaxPixel;

    RangeResult * strctResult;

   

 // Values used for calculating range from captured image data

 const double gain; // Gain Constant used for converting pixel offset to angle in radians

 const double offset; // Offset Constant

 const double h_cm;  // Distance between center of camera and laser

    unsigned int pixels_from_center; // Brightest pixel location from center

     

 void Preprocess(void * img,IplImage * imgTemp);

};

cpp文件：

LaserRange::LaserRange():gain(0.0030354),offset(0),h_cm(4.542)

{

  maxW=0;

  maxH=0;

  MaxPixel=0;

  

  pixels_from_center=0;    // Brightest pixel location from center

  strctResult=new RangeResult;

 

  strctResult->maxCol=0;

  strctResult->maxRow=0;

  strctResult->maxPixel=0;

  strctResult->Range=0.0;

 }

LaserRange::~LaserRange()

{

  if(NULL!=strctResult) delete strctResult;

}

struct RangeResult * LaserRange::GetRange(IplImage * imgRange,IplImage * imgDst)

{ 

    if(NULL==imgRange) return   strctResult;

       Preprocess(imgRange,imgDst);

      

    pixels_from_center = abs(120-maxH);

 // Calculate range in cm based on bright pixel location, and setup specific constants

     strctResult->Range= h_cm/tan(pixels_from_center * gain + offset);

      

    strctResult->PixfromCent=pixels_from_center;

       strctResult->maxCol=maxW;

       strctResult->maxRow=maxH;

       strctResult->maxPixel=MaxPixel;

       //strctResult->Range=0.0;

         return  strctResult;

}

void LaserRange::Preprocess(void *img, IplImage * imgTemp)

{

    MaxPixel=0;                //处理下一帧前 最大像素值清零；

   IplImage* image = reinterpret_cast<IplImage*>(img);

  

       cvCvtPixToPlane( image,0 ,0 ,imgTemp , 0);

 

      for( int j=((imgTemp->width-60)/2-1); j<(imgTemp->width-40)/2+59; j++) 

  {

    for(int i=5; i<imgTemp->height-5; i++)

    {

  

    if((imgTemp->imageData[i*imgTemp->widthStep+j])>MaxPixel)

    {

    
if(
((imgTemp->imageData[(i-1)*imgTemp->widthStep+j])>MaxPixel)
&&
((imgTemp->imageData[(i-1)*imgTemp->widthStep+j+1])>MaxPixel)
&&((imgTemp->imageData[(i-1)*imgTemp->widthStep+j-1])>MaxPixel)  
)

     {

      if(
((imgTemp->imageData[(i+1)*imgTemp->widthStep+j])>MaxPixel)
&&
((imgTemp->imageData[(i+1)*imgTemp->widthStep+j+1])>MaxPixel)
&&((imgTemp->imageData[(i+1)*imgTemp->widthStep+j-1])>MaxPixel)  
)

       {

       if((imgTemp->imageData[i*(imgTemp->widthStep)+j+1])>MaxPixel)

       {

       if((imgTemp->imageData[i*(imgTemp->widthStep)+j-1])>MaxPixel)

       {

         MaxPixel=imgTemp->imageData[i*imgTemp->widthStep+j] ;

         maxW=j;

         maxH=i;

       }

       }

       }

     }

    }

  }

     

}

调用函数：

int CLaserVisionDlg::CaptureImage()

{

   // CvCapture* capture = 0;

 

   // capture = cvCaptureFromCAM(0);  //0表示设备号

    if( !capture )

    {

        fprintf(stderr,"Could not initialize capturing.../n");

        return -1;

    }

 

   // cvNamedWindow( "LaserRangeImage", 1 );

  // cvvNamedWindow( "image", 1);

   cvvNamedWindow( "Dimage", 1);

  

   for(;;)

    {

        IplImage* frame = 0;

      

  if(isStop) break;

        frame = cvQueryFrame( capture ); //从摄像头抓取一副图像框架

        if( !frame )

            break;

       if( !imgOrign )

        {

            //allocate all the buffers

            imgOrign = cvCreateImage( cvGetSize(frame), 8, 3 );    //创建一副图像

            imgOrign->origin = frame->origin;

    

  }

        cvCopy( frame, imgOrign, 0 );  //将图frame复制到image

  //cvShowImage("LaserRangeImage",imgOrign); 

    

 

       if(!imgDest)

    {

   imgDest=cvCreateImage( cvSize( imgOrign->width,imgOrign->height),8,1);

      cvZero( imgDest );

    }

        struct RangeResult * temp= laservsion.GetRange(imgOrign,imgDest);

   

       
cvLine( imgOrign,cvPoint(temp->maxCol,0),
cvPoint(temp->maxCol,imgOrign->height),cvScalar(100,100,255,0),1,8,0);

        cvLine( imgOrign,cvPoint(0,temp->maxRow), cvPoint(imgOrign->width,temp->maxRow),cvScalar(100,100,255,0),1,8,0);

   

       // cvvShowImage( "image", imgOrign);

  cvSaveImage("image.bmp", imgOrign);

         

        cvvShowImage( "Dimage", imgDest);

      

        //在PictureBox上显示图片

  CDC* pDC = GetDlgItem(IDC_Picture)->GetDC();

        CDC dcmemory;

        BITMAP bm;

        dcmemory.CreateCompatibleDC(pDC);

        CBitmap* pBmp;

        CString szFileName = "image.bmp";

        HBITMAP hBk = (HBITMAP)::LoadImage(NULL,szFileName,IMAGE_BITMAP,0,0,LR_LOADFROMFILE);  

       if(NULL!=hBk)  

    {

        pBmp=CBitmap::FromHandle(hBk);

        pBmp->GetObject(sizeof(BITMAP), &bm);

        dcmemory.SelectObject(pBmp);

        pDC->BitBlt(0, 0, bm.bmWidth, bm.bmHeight, &dcmemory, 0, 0, SRCCOPY);

    }

      

      

       char str[80];         // To print message

    CDC *pDCp= GetDC();

       char str2[80];

     

    // Display frame coordinates as well as calculated range

      
sprintf(str, "Pix Max Value=%d  At x= %u, y= %u, PixfromCent=
%d",temp->maxPixel,temp->maxCol, temp->maxRow,
temp->PixfromCent);

       sprintf(str2, "Range= %f cm ",temp->Range);

       pDCp->TextOut(30, 33, str);

       pDCp->TextOut(50, 50, str2);

    ReleaseDC(pDCp);

    

      int  c = cvWaitKey(10);

      //  if( c == 'q' )

       //     break;

     

 }

//cvReleaseCapture( &capture );

//cvDestroyWindow("LaserRangeImage");

// cvDestroyWindow( "image");

 cvDestroyWindow( "Dimage");

return 0;

}