如何用攝像頭來(lái)測(cè)距（opencv）
作者：郭世龍
最近一直忙著找工作，blog都長(zhǎng)草了，今天把以前作的一個(gè)東西放上來(lái)充充門(mén)面吧。記得在哪看到過(guò)老外做的這個(gè)東西，覺(jué)得很好玩，就自己也做了一個(gè)。在攝像頭下面固定一個(gè)激光筆，就構(gòu)成了這個(gè)簡(jiǎn)易的測(cè)距裝置。看一下圖吧。


原 理
假設(shè)激光束是與攝像頭的光軸完全平行，激光束的中心落點(diǎn)在在攝像頭的視域中是最亮的點(diǎn)。激光束照射到攝像頭視域中的跟蹤目標(biāo)上，那么攝像頭可以捕捉到這個(gè)點(diǎn)，通過(guò)簡(jiǎn)單的圖像處理的方法，可以在這偵圖像中找到激光束照射形成的最亮點(diǎn)，同時(shí)可以計(jì)算出Y軸上方向上從落點(diǎn)到圖像中心的象素的個(gè)數(shù)。這個(gè)落點(diǎn)越接近圖像的中心，被測(cè)物體距離機(jī)器人就越遠(yuǎn)。由下圖圖可以計(jì)算距離D：

（1）
等式中h是一個(gè)常量，是攝像頭與激光發(fā)射器之間的垂直距離，可以直接測(cè)量獲得。
θ可通過(guò)下式計(jì)算：
θ=Num*Rop+Offset （2）
其中：Num是從圖像中心到落點(diǎn)的像素個(gè)數(shù)
Rop是每個(gè)像素的弧度值
Offset是弧度誤差
合并以上等式可以得到：
（3）
Num可以從圖像上計(jì)算得到。Rop和Offset需要通過(guò)實(shí)驗(yàn)計(jì)算獲得。首先測(cè)量出D的準(zhǔn)確值，然后根據(jù)等式（1）可以計(jì)算出準(zhǔn)確的θ，根據(jù)等式（2）可到只含有參數(shù)Rop和Offset的方程。在不同的距離多次測(cè)量D的準(zhǔn)確值計(jì)算θ，求解方程組可以求出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 (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 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;
}
}
}
}
}
}

}
調(diào)用函數(shù)：
int CLaserVisionDlg::CaptureImage()
{
// CvCapture* capture = 0;

// capture = cvCaptureFromCAM(0); //0表示設(shè)備號(hào)
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 ); //創(chuàng)建一副圖像
imgOrign->origin = frame->origin;

}
cvCopy( frame, imgOrign, 0 ); //將圖frame復(fù)制到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;
}

本文轉(zhuǎn)自
http://blog.csdn.net/xylary/archive/2007/10/25/1843809.aspx

Trackback: http://tb.blog.csdn.net/TrackBack.aspx?PostId=1848557