障碍物点云分割算法python代码 障碍物识别算法有哪些

1.项目背景

使用两个型号和镜头一样的相机进行双目视觉算法开发，测量远距离下的物体距离

2.开发环境和使用设备

VS2019+opencv454 ，matlab2021a进行双目相机标定，相同型号的相机镜头2个，保证能够同时触发

3.详细设计方案

（1）双目设备安装要求及计算原理

使用的是平行式双目视觉测量方法，两个相机平行安装，安装距离间隔1米。

成像原理如下图所示：

建立空间直角坐标系，构建相似三角形，求视差：

图5-2 双目视觉测距原理

如图5-2所示，由相似三角形原理可知，PL1L2与PC1C2相似，建立方程求解公式如5-1所示：

化简后：

其中B为两个相机之间的距离，称为基线距离，f为相机焦距， 表示视差，用d表示。公式5-2中，基线距离B和焦距f已知，只需要求得视差d就可以得到物体距离。（2）使用matlab进行双目相机标定

1）标定前需要对两个相机的图像进行编号处理，保证一一对应

2）标定的图像数量大于40张，左右相机各20张左右。

3）标定板移动范围尽可能的覆盖需要测量的视野范围。

4）标定板不要太小，远距离情况下，标定板太小标出来不准，投影误差会比较大，对精度要求高的时候需要更换更大的标定板。

（3）标定结果如下，其中平均的投影误差在1.3个像素左右。

（4）标定验证

保证图像的y相同，便于后面进行立体匹配，也是检查标定的结果是否准确

图中立体校正后结果基本上能对上，误差可接受，需要迁移到c++工程中，保存两个相机的内参、畸变系数和共同的R、T矩阵。

（5）matlab保存标定结果

以下matlab代码参考某位博主的：

rowName = cell(1,10);
rowName{1,1} = '平移矩阵';
rowName{1,2} = '旋转矩阵';
rowName{1,3} = '相机1内参矩阵';
rowName{1,4} = '相机1径向畸变';
rowName{1,5} = '相机1切向畸变';
rowName{1,6} = '相机2内参矩阵';
rowName{1,7} = '相机2径向畸变';
rowName{1,8} = '相机2切向畸变';
rowName{1,9} = '相机1畸变向量';
rowName{1,10} = '相机2畸变向量';
xlswrite('out.xlsx',rowName(1,1),1,'A1');
xlswrite('out.xlsx',rowName(1,2),1,'A2');
xlswrite('out.xlsx',rowName(1,3),1,'A5');
xlswrite('out.xlsx',rowName(1,4),1,'A8');
xlswrite('out.xlsx',rowName(1,5),1,'A9');
xlswrite('out.xlsx',rowName(1,6),1,'A10');
xlswrite('out.xlsx',rowName(1,7),1,'A13');
xlswrite('out.xlsx',rowName(1,8),1,'A14');
xlswrite('out.xlsx',rowName(1,9),1,'A15');
xlswrite('out.xlsx',rowName(1,10),1,'A16');
xlswrite('out.xlsx',stereoParams.TranslationOfCamera2,1,'B1');  % 平移矩阵
xlswrite('out.xlsx',stereoParams.RotationOfCamera2.',1,'B2');  % 旋转矩阵
xlswrite('out.xlsx',stereoParams.CameraParameters1.IntrinsicMatrix.',1,'B5');  % 相机1内参矩阵
xlswrite('out.xlsx',stereoParams.CameraParameters1.RadialDistortion,1,'B8');  % 相机1径向畸变(1,2,5)
xlswrite('out.xlsx',stereoParams.CameraParameters1.TangentialDistortion,1,'B9');  % 相机1切向畸变(3,4)
xlswrite('out.xlsx',stereoParams.CameraParameters2.IntrinsicMatrix.',1,'B10');  % 相机2内参矩阵
xlswrite('out.xlsx',stereoParams.CameraParameters2.RadialDistortion,1,'B13');  % 相机2径向畸变(1,2,5)
xlswrite('out.xlsx',stereoParams.CameraParameters2.TangentialDistortion,1,'B14');  % 相机2切向畸变(3,4)
xlswrite('out.xlsx',[stereoParams.CameraParameters1.RadialDistortion(1:2), stereoParams.CameraParameters1.TangentialDistortion,...
    stereoParams.CameraParameters1.RadialDistortion(3)],1,'B15');  % 相机1畸变向量
xlswrite('out.xlsx',[stereoParams.CameraParameters2.RadialDistortion(1:2), stereoParams.CameraParameters2.TangentialDistortion,...
    stereoParams.CameraParameters2.RadialDistortion(3)],1,'B16');  % 相机2畸变向量

最后畸变那两行可能会报错，主要是标定的时候勾选的差异，选第一个就只输出2个，第三个参数是没有的，需要删掉matlab里面那一行。

保存的out.xls结果里面已经将参数进行转置了，可以直接在c++里面使用。（6）工程化应用

立体匹配和立体校正：

实际使用过程中，存在安装角度不平行等问题，因此需要进行立体校正。

//2.双目的立体标定和立体校正
void ObstacleRecongnition::stereo_check(Mat grayImageL, Mat grayImageR, Mat &rectifyImageL, Mat &rectifyImageR, Size imageSize, Mat &Q, Rect &validROIL, Rect &validROIR) {
	Mat mapLx, mapLy, mapRx, mapRy;     //映射表  
	Mat Rl, Rr, Pl, Pr;              //校正旋转矩阵R，投影矩阵P 重投影矩阵Q
	//左边相机的内参矩阵
	Mat cameraMatrixL = (Mat_<double>(3, 3) <<
		left_fx, 0, left_cx,
		0, left_fy,left_cy,
		0, 0, 1);
	//左边相机的畸变矩阵
	Mat distCoeffL = (Mat_<double>(5, 1) << left_k1,left_k2,left_p1,left_p2,left_k3);
	//右边相机的内参矩阵
	Mat cameraMatrixR = (Mat_<double>(3, 3) <<
		right_fx, 0, right_cx,
		0, right_fy, right_cy,
		0, 0, 1);
	//右边相机的畸变矩阵
	Mat distCoeffR = (Mat_<double>(5, 1) << right_k1,right_k2,right_p1,right_p2,right_k3);

	//两个相机标定后的平移矩阵
	Mat T = (Mat_<double>(3, 1) << double_py_x1,double_py_x2,double_py_x3);//T平移向量

	//两个相机标定后的旋转矩阵
	Mat rec = (Mat_<double>(3, 3) <<
		double_xz_x1, double_xz_x2, double_xz_x3,
		double_xz_x4, double_xz_x5, double_xz_x6,
		double_xz_x7, double_xz_x8, double_xz_x9);                //rec旋转向量

	Mat R;//R 旋转矩阵
	Rodrigues(rec, R); //Rodrigues变换，获得3*3的旋转矩阵

	//立体校正
	stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl, Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY,
		0, imageSize, &validROIL, &validROIR);
	//计算无畸变和修正转换关系
	initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pr, imageSize, CV_32FC1, mapLx, mapLy);//mapL——输出的X坐标重映射参数
	initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);//mapR——输出的Y坐标重映射参数

	//一幅图像中某位置的像素放置到另一个图片指定位置。
	//经过remap之后，左右相机的图像已经共面并且行对准了
	remap(grayImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);//最后一个参数是差值方式
	remap(grayImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);
	//输出的图像需和原图形一样的尺寸和类型

#if TEST_LOG
	//把校正结果显示出来
	Mat rgbRectifyImageL, rgbRectifyImageR;
	cv::cvtColor(rectifyImageL, rgbRectifyImageL, CV_GRAY2BGR);  //伪彩色图
	cv::cvtColor(rectifyImageR, rgbRectifyImageR, CV_GRAY2BGR);
	Mat canvas;
	double sf;
	int w, h;
	sf = 600. / MAX(rectifyImageL.cols, rectifyImageL.rows);
	w = cvRound(rectifyImageL.cols * sf);
	h = cvRound(rectifyImageL.rows * sf);
	canvas.create(h, w * 2, CV_8UC3);   //注意通道

	//左图像画到画布上
	Mat canvasPart = canvas(Rect(w * 0, 0, w, h));                                //得到画布的一部分  
	resize(rgbRectifyImageL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);     //把图像缩放到跟canvasPart一样大小 

	//右图像画到画布上
	canvasPart = canvas(Rect(w, 0, w, h));                                      //获得画布的另一部分  
	resize(rgbRectifyImageR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);

	//画上对应的线条
	for (int i = 0; i < canvas.rows; i += 16)
		line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);

	imwrite("D:\桌面文件\save\Rectifyimages.jpg", canvas);
	waitKey(0);
#endif
}

双视觉算法c++：

//双目视觉算法
void ObstacleRecongnition::binocular_vision(Mat rgbImageL, Mat rgbImageR, Mat &rectifyImageL1,Mat &rectifyImageR1) {
	Mat rectifyImageL,rectifyImageR;
	Mat grayImageL, grayImageR;
	const int imageWidth = rgbImageL.cols;                      
	const int imageHeight = rgbImageL.rows;
	Size imageSize = Size(imageWidth, imageHeight);
	
	//1.图像获取
	cv::cvtColor(rgbImageL, grayImageL, CV_BGR2GRAY);
	cv::cvtColor(rgbImageR, grayImageR, CV_BGR2GRAY);

	//2.立体校正--图像对齐了-在同一极线上检测左右摄像机图像的相同特征
	stereo_check(grayImageL, grayImageR, rectifyImageL, rectifyImageR, imageSize, Q, validROIL, validROIR);
	
	cv::cvtColor(rectifyImageL, rectifyImageL1, CV_GRAY2BGR);
	cv::cvtColor(rectifyImageR, rectifyImageR1, CV_GRAY2BGR);
}

4.通过立体匹配算法计算距离

双目视觉的核心就是通过视差计算距离：z = bf/d
其中b表示两个相机之间的距离，f表示相机焦距，d表示视差，d=xr-xl，即左右图像中的x坐标差，d单位是像素pixel。

这个也是目前主流的算法，通过立体匹配，然后生成视差图，最后计算距离，代码很简单，基本上都是opencv封装好的，只需要根据图像修改几个参数。
缺点：速度慢，好的视差图不容易得到。
实现：

void ObstacleRecongnition::stereo_match(Mat rectifyImageL, Mat rectifyImageR, Mat Q, Mat &xyz, Rect validROIL, Rect validROIR,Mat &SC_Map)
{
	Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, StereoSGBM_width, StereoSGBM_height);
	sgbm->setPreFilterCap(PreFilterCap);
	
	sgbm->setBlockSize(sgbmWinSize);
	int cn = rectifyImageL.channels();
	sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
	sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
	sgbm->setMinDisparity(0);
	sgbm->setNumDisparities(NumDisparities);
	sgbm->setUniquenessRatio(UniquenessRatio);
	sgbm->setSpeckleWindowSize(SpeckleWindowSize);
	sgbm->setSpeckleRange(SpeckleRange);
	sgbm->setDisp12MaxDiff(Disp12MaxDiff);
	sgbm->setMode(StereoSGBM::MODE_SGBM);
	Mat disp,disp8;
	sgbm->compute(rectifyImageL, rectifyImageR, disp);
	//去黑边
	//Mat img1p, img2p;
	//copyMakeBorder(rectifyImageL, img1p, 0, 0, NumDisparities, 0, IPL_BORDER_REPLICATE);
	//copyMakeBorder(rectifyImageR, img2p, 0, 0, NumDisparities, 0, IPL_BORDER_REPLICATE);
	//dispf = disp.colRange(NumDisparities, img2p.cols - NumDisparities);

	//视差图滤波算法
	cv::imwrite("D:\桌面文件\save\原始视差图.jpg", disp);

	disp.convertTo(disp8, CV_8U, 255 / (NumDisparities *16.));
	reprojectImageTo3D(disp, xyz, Q, true); //在实际求距离时，ReprojectTo3D出来的X / W, Y / W, Z / W都要乘以16(也就是W除以16)，才能得到正确的三维坐标信息。
	xyz = xyz * 16;
	Mat color(disp.size(), CV_8UC3);
	GenerateFalseMap(disp8, color);//转成彩图
	imwrite("D:\桌面文件\save\伪彩色视差图.jpg", color);

	waitKey(0);
}

获得视差图以后，可以通过对视差图进行滤波等操作，不做进一步研究，下面可以通过在视差图中随意点几个点，输出的距离是否和该位置的实际距离差不多进行验证。
通过视差图计算距离：

//4.通过视差图计算距离
float ObstacleRecongnition::compute_distance(Mat xyz, float up_x, float up_y, float down_x, float down_y) {
//在视差图中点几个点
	Point origin = Point(up_x, up_y);
	//通过双目参数将图像坐标进行转换
	Vec3f point3 = xyz.at<Vec3f>(origin);
	
	d = point3[0] * point3[0] + point3[1] * point3[1] + point3[2] * point3[2];
	d = sqrt(d);   //mm
	return d;
}

5.通过特征点匹配算法算距离（推荐）

1）使用yolov5对两个相机的图像进行识别，获取对应的目标

2）将定位后的图像进行特征点匹配，最后需要传入定位前原始图像的定位x坐标x_before1和x_before2来计算视差值d。

3）通过公式z=bf/d计算距离。
可参考ORB、SIFT、SURF特征提取算法实现特征点提取。
以SURF为例：
用到了基于GPU加速的opencv编程，要求opencv454是带cuda版本的，输入左右相机的图像，进行特征点匹配

//surf特征匹配算法
float ObstacleRecongnition::surf_demo(Mat rectifyImageL,Mat rectifyImageR,int x_before1,int x_before2) {
	cv::cvtColor(rectifyImageL, rectifyImageL, CV_BGR2GRAY);
	cv::cvtColor(rectifyImageR, rectifyImageR, CV_BGR2GRAY);
	cv::cuda::printShortCudaDeviceInfo(cv::cuda::getDevice());
	cuda::SURF_CUDA surf(points_alg_surf);
	// detecting keypoints & computing descriptors
	cuda::GpuMat keypoints1GPU, keypoints2GPU;
	cuda::GpuMat descriptors1GPU, descriptors2GPU;

	cuda::GpuMat imageL, imageR;
	imageL.upload(rectifyImageL);
	imageR.upload(rectifyImageR);
	float dis;
	try {
		surf(imageL, cuda::GpuMat(), keypoints1GPU, descriptors1GPU);
		surf(imageR, cuda::GpuMat(), keypoints2GPU, descriptors2GPU);
		// matching descriptors
		Ptr<cv::cuda::DescriptorMatcher> matcher = cv::cuda::DescriptorMatcher::createBFMatcher(surf.defaultNorm());
		vector<DMatch> matches;
		matcher->match(descriptors1GPU, descriptors2GPU, matches);

		// downloading results
		vector<KeyPoint> keypoints1, keypoints2;
		vector<float> descriptors1, descriptors2;
		surf.downloadKeypoints(keypoints1GPU, keypoints1);
		surf.downloadKeypoints(keypoints2GPU, keypoints2);
		surf.downloadDescriptors(descriptors1GPU, descriptors1);
		surf.downloadDescriptors(descriptors2GPU, descriptors2);


		//筛选距离，不同特征点存在匹配错误的情况，可以根据实际情况调整
		//calculate the max and min distance between key points
		double maxdist = 0; double mindist = 100;
		for (int i = 0; i < descriptors1GPU.rows; i++)
		{
			double dist = matches[i].distance;
			if (dist < mindist)mindist = dist;
			if (dist > maxdist)maxdist = dist;
		}
		//cout << "Matching quantity:" << matches.size() << endl;

		//select the good match points
		vector<DMatch>goodMatches;
		for (int i = 0; i < descriptors1GPU.rows; i++)
		{
			if (matches[i].distance < 2 * mindist)
			{
				goodMatches.push_back(matches[i]);
			}
		}
		//cout << "Good matching quantity:" << goodMatches.size() << endl;

		//calculate parallax
		vector<parallax>para;
		for (int i = 0; i < goodMatches.size(); i++)
		{
			parallax temp;

			temp.leftX = keypoints1[goodMatches[i].queryIdx].pt.x + x_before1;
			temp.rightX = keypoints2[goodMatches[i].queryIdx].pt.x + x_before2;
			temp.paraValue = temp.leftX - temp.rightX;
			para.push_back(temp);
			cout << "No." << i + 1 << ":\t l_X ";
			cout << para[i].leftX << "\t r_X " << para[i].rightX;
			cout << "\t surf parallax " << para[i].paraValue << endl;
		}
		//sort(para.begin(), para.end(), ascendPara);
		int idxMedian = int(para.size() / 2);
		double paraMedian = para[idxMedian].paraValue;
		vector<parallax>::iterator it;
		for (it = para.begin(); it != para.end(); )
		{
			if (it->paraValue<((1 - errorRange)*paraMedian) || it->paraValue>((1 + errorRange)*paraMedian))
				it = para.erase(it);
			else
				it++;
		}
		cout << "Final data..." << endl;
		double paraSum = 0;
		double paraMean;
		for (int i = 0; i < para.size(); i++)
		{
			paraSum = paraSum + para[i].paraValue;
			cout << "No." << i << "\t" << para[i].paraValue << endl;

		}
		paraMean = paraSum / para.size();
		cout << "surf Parallax is " << paraMean << " pixel." << endl;
		//z = bf/d
		dis = (cameradis * camera_f) / (paraMean);

		// drawing the results
		Mat img_matches;
		drawMatches(Mat(imageL), keypoints1, Mat(imageR), keypoints2, matches, img_matches);
		cv::imwrite("D:\桌面文件\save\surf_img.jpg", img_matches);
	}
	catch (...) {
		dis = 100;
	}
	//cout << "now dis：" << dis << endl;
	return dis;
}

6.通过中点算距离

直接对校正后的图像进行识别，然后使用目标检测框的中点位置作为最终的结果，再得到视差，计算距离。