摄像头测距原理与代码实现

外界3D影像投射到摄像头传感器的原理其实就是针孔成像原理。

针孔成像原理，如图所示：

      

f为摄像头的焦距，c为镜头光心

模型的主要依据公式为f/d=h/H，设物体所在平面与相机平面的距离为d，物体实际高度为H，在传感器上的高度为h
根据这个模型，我们就能求出目标物体与我们的摄像头平面的距离。

分两种情况，但是这两种情况的条件都是假设实际物体与摄像机所在平面平行。

一种是当物体主线段过光心的情况，这种情况是最容易计算的， 即 h=sqrt ((横坐标之差*Dx)^2+(纵坐标之差*Dy)^2),  Dx为每个像素的宽度，Dy为每个像素的高度，

关于相机传感的尺寸，参考 http://blog.csdn.net/u011572037/article/details/78236795

  另一种就是物体主线段不过光心的情况，一般情况下都属于这个情况。

不废话，直接上程序，焦距和图像中心坐标都是相机标定的出的结果，个人推荐matlab的标定程序，比较人性化，而且准确

int width=640,height=480;double focalLength=4.2;//焦距（单位：mm）///////////////////////////////////////////////////////////////////求相机到线段的距离,注意相机平面必须与线段平面平行//所用到的主要公式f/d=h/H/////////////////////////////////////////////////////////////////int  coordinate(Point First,Point Second,Point centre,Point2d Sensor_Size,int H){First=Point(1,1);Second=Point(1,300);//先求出主光轴到两点连线的垂足的距离（实际尺寸）double h=0;//h为传感器上的高度//求垂足的坐标int  Distance_To_Centre = 0;//返回值（实际距离）Point retVal;          double dx = abs(First.x - Second.x);          double dy = abs(First.y - Second.y);          double u = (centre.x - First.x)*(First.x - Second.x) +          (centre.y - First.y)*(First.y - Second.y);          u = u/((dx*dx)+(dy*dy));            retVal.x = abs(First.x + u*dx);          retVal.y = abs(First.y + u*dy);    double centreToretVal=0;//主轴到垂足在图像上的距离centreToretVal=sqrt((centre.x-retVal.x)*Sensor_Size.x/width*(centre.x-retVal.x)*Sensor_Size.x/width+\(centre.y-retVal.y)*Sensor_Size.y/height*(centre.y-retVal.y)*Sensor_Size.y/height);int d=0;//与线段所在平面的距离h=sqrt((First.x-Second.x)*Sensor_Size.x/width*(First.x-Second.x)*Sensor_Size.x/width+\(First.y-Second.y)*Sensor_Size.y/height*(First.y-Second.y)*Sensor_Size.y/height);d=focalLength*H/h;double arctan=0;//实际垂足与光心的连线与主光轴的倾斜夹角arctan=atan((centreToretVal/focalLength));if(acos(arctan)!=0)Distance_To_Centre=d/cos(arctan);//根据倾斜角度求出实际距离return Distance_To_Centre;}