单目相机测距

1. 三角测距
   
   

void getCoord(cv::Point &point, float coord[2]){
	float y;
	float x;
	if (point.y > IMG_HEIGHT / 2) {
		y = h / tan(theta + atan((point.y - IMG_HEIGHT / 2) * 2 * tan(beta) / IMG_HEIGHT));
	}
	else {
		float alpha = theta - atan((IMG_HEIGHT / 2 - point.y) * 2 * tan(beta) / IMG_HEIGHT);
		if (alpha < 0) {
			y = 10;
		}
		else {
			y = h / tan(alpha);
		}
	}
	//x方向畸变 桶形畸变，暂未考虑枕形畸变，用sqrt平方根来近似
	float rate = 1.0 - (point.y - IMG_HEIGHT / 2) * 2 / IMG_HEIGHT;
	float gama0 = gama1 + sqrt(rate) * (gama - gama1);
	x = sqrt(h *h + y * y) * (point.x - IMG_WIDTH / 2) * tan(gama0) * 2 / IMG_WIDTH;
	coord[0] = x;
	coord[1] = y;
	return;
}


void getDistance(cv::Point &point, float &dis) {
	float coord[2];
	getCoord(point, coord);
	dis = sqrt(pow(coord[0], 2) + pow(coord[1], 2));
	return;
}

2. PnP测距
   参考：【RoboMaster】我是这样搞定第一次单目相机测距的
   原理（PnP函数求解）：

bool cv::solvePnP(
  cv::InputArray  objectPoints,     //三维点坐标矩阵,至少四个（世界坐标系） 必填
  cv::InputArray  imagePoints,      //该四个点在图像中的像素坐标	必填
  cv::InputArray  cameraMatrix,     //相机内参矩阵（3*3）	选填
  cv::InputArray  distCoeffs,       //相机畸变系数（5*1）或（4*1） 选填
  cv::OutputArray rvec,             //输出旋转矩阵（3*1） 
  cv::OutputArray tvec,             //输出平移矩阵 （3*1） ***这就可以实现测距目的***
  bool        useExtrinsicGuess = false,  
  int        flags = cv::SOLVEPNP_ITERATIVE
);

应用：

vector<Point3f> obj = vector<Point3f>{
        cv::Point3f(-HALF_LENGTH, -HALF_LENGTH, 0),  //tl
        cv::Point3f(HALF_LENGTH, -HALF_LENGTH, 0),  //tr
        cv::Point3f(HALF_LENGTH, HALF_LENGTH, 0),  //br
        cv::Point3f(-HALF_LENGTH, HALF_LENGTH, 0)  //bl
    };   //自定义二维码四个点坐标
    cv::Mat rVec = cv::Mat::zeros(3, 1, CV_64FC1);//init rvec 
    cv::Mat tVec = cv::Mat::zeros(3, 1, CV_64FC1);//init tvec
    cv::solvePnP(obj, pnts, cameraMatrix, distCoeff, rVec, tVec, false, SOLVEPNP_ITERATIVE);

不建议使用原因如下：
2.1 需要知晓目标物的尺寸，比如上面代码里的HALF_LENGTH
2.2 虽然相机内外参是选填的，但如果内外参准确的话会大大提高测距精度
2.3 目标形状变化太大或部分遮挡或特征点不明显等均会影响测距精度
3. 是