自动驾驶AVM环视算法--任意角度的旋转算法实现_环视源码

测试环境 

opencv310

vs2022

参考：金书世界

算法源码获取：

链接：https://pan.baidu.com/s/1YLE-uM1JaHCMqDndPfU01Q 提取码：m7rc 

算法的主要实现

1、实现三个坐标轴轴的旋转。

2、不同相机之间的拼接融合。

测试的输出效果图像

算法的实现过程

根据当前的相机参数在保证算法的实际效果的前提下设置旋转角度的范围

	AVMData.A_rx = 114; //[72 114]
	AVMData.A_ry = 0;//[0 360]
	AVMData.A_rz = 0;//[-20 20]

在绕车辆周围旋转时，由于车辆是长方形的，不能在在车周围圆形的旋转吗，所以当前的额 算法是按照椭圆的方式旋转路径。

    //椭圆a=4000,b=3000
	float aa = 5000; float bb = 4000; 
	float qq = Data->A_ry * 3.1415927 / 180;
 
    Data->A.x = aa * sin(qq);
	Data->A.y = Data->CameraH_A;
	Data->A.z = -bb * cos(qq);
 
	Data->F.x = 0;
	Data->F.y = Data->CameraH_F;
	Data->F.z = (Data->FW + Data->BW) / 2;
 
	Data->B.x = 0;
	Data->B.y = Data->CameraH_B;
	Data->B.z = -(Data->FW + Data->BW) / 2;
 
 
	Data->L.x = -Data->LW;
	Data->L.y = Data->CameraH_B;
	Data->L.z = Data->F.z- Data->FW;
 
	Data->R.x = Data->RW;
	Data->R.y = Data->CameraH_B;
	Data->R.z = Data->F.z - Data->FW;

初始化相机的参数

js_initAngle(Data->data_TOP_B, Data->A_rx, -Data->A_ry,-Data->A_rz); js_initAngleT(Data->data_TOP_B, Data->data_TOP_T_B);
js_initAngle(Data->data_TOP_A, Data->A_rx, Data->A_ry,Data->A_rz);	js_initAngle(Data->data_0_A, 0, 0, 0); js_initAngleT(Data->data_TOP_A, Data->data_TOP_T_A);
js_initAngle(Data->data_0_F, 25, 0, 0);
js_initAngle(Data->data_0_B, 25, 180, 0);
js_initAngle(Data->data_0_L, 25, 90, 0); 
js_initAngle(Data->data_0_R, 25, -90, 0); 

计算车辆的拼接交点位置

    js_pointf Psrc;
	js_3Dpointf Pdst_LF, Pdst_RF, Pdst_LB, Pdst_RB;
	Pdst_LF.x = Data->L.x;
	Pdst_LF.y = Data->CameraH_A;
	Pdst_LF.z = Data->F.z;// -Data->A.z;
	js_World2Plane(Data->data_TOP_A, Data->Aa, Pdst_LF, &Data->LFsrc, Data->Af, Data->Ac, Data->zc);
 
	Pdst_RF.x = Data->R.x;
	Pdst_RF.y = Data->CameraH_A;
	Pdst_RF.z = Data->F.z;// -Data->A.z;
	js_World2Plane(Data->data_TOP_A, Data->Aa, Pdst_RF, &Data->RFsrc, Data->Af, Data->Ac, Data->zc);
 
	Pdst_LB.x = Data->L.x;
	Pdst_LB.y = Data->CameraH_A;
	Pdst_LB.z = Data->B.z; //Data->A.z - Data->B.z;
	js_World2Plane(Data->data_TOP_A, Data->Aa, Pdst_LB, &Data->LBsrc, Data->Af, Data->Ac, Data->zc);
 
	Pdst_RB.x = Data->R.x;
	Pdst_RB.y = Data->CameraH_A;
	Pdst_RB.z = Data->B.z; //Data->A.z - Data->B.z;
	js_World2Plane(Data->data_TOP_A, Data->Aa, Pdst_RB, &Data->RBsrc, Data->Af, Data->Ac, Data->zc);

获取四个摄像头的图像数据（本测试使用的是opencv读取图像的方式实现的）

	IplImage* img_F = cvLoadImage("Front.png");
	IplImage* img_B = cvLoadImage("Back.png");
	IplImage* img_L = cvLoadImage("Left.png");
	IplImage* img_R = cvLoadImage("Right.png");

算法实现函数

js_getAVM_TOP(&AVMData, img_AVM->imageData, img_F->imageData, img_B->imageData, img_L->imageData, img_R->imageData, img_F->width, img_F->height, img_AVM->width, img_AVM->height, img_AVM->nChannels);

算法的图片的测试代码

// TODO: 在此添加控件通知处理程序代码
	IplImage* img_AVM = cvCreateImage(cvSize(1080, 1080), 8, 3);
	IplImage* img_AVM_resize = cvCreateImage(cvSize(404, 720), 8, 3);
	IplImage* img_F = cvLoadImage("Front.png");
	IplImage* img_B = cvLoadImage("Back.png");
	IplImage* img_L = cvLoadImage("Left.png");
	IplImage* img_R = cvLoadImage("Right.png");
	IplImage* det_TOP_F = cvCreateImage(cvSize(img_F->width, img_F->height), img_F->depth, img_F->nChannels);
	IplImage* det_TOP_B = cvCreateImage(cvSize(img_B->width, img_B->height), img_B->depth, img_B->nChannels);
	IplImage* det_TOP_L = cvCreateImage(cvSize(img_L->width, img_L->height), img_L->depth, img_L->nChannels);
	IplImage* det_TOP_R = cvCreateImage(cvSize(img_L->width, img_L->height), img_L->depth, img_L->nChannels);
 
	CvVideoWriter* writer = cvCreateVideoWriter("G:\\CSDN\\AVM\\Video\\AVM_Angle.avi", CV_FOURCC('X', 'V', 'I', 'D'), 25, cvSize(1080, 1080));
 
	js_AVM_obj AVMData;
	AVMData.A_rx = 114; //[72 114]
	AVMData.A_ry = 0;//[0 360]
	AVMData.A_rz = 0;//[-20 20]
	//AVMData.A_rx = 90; //[72 114]
	//AVMData.A_ry = 450;//[0 360]
	//AVMData.A_rz = -20;//[-20 20]
	int stept_index = 0;
	while (1)
	{
		js_init_avm(&AVMData, img_AVM->width, img_AVM->height);
		js_getAVM_TOP(&AVMData, img_AVM->imageData, img_F->imageData, img_B->imageData, img_L->imageData, img_R->imageData, img_F->width, img_F->height, img_AVM->width, img_AVM->height, img_AVM->nChannels);
 
		//cvCircle(img_AVM, cvPoint(AVMData.LFsrc.x, AVMData.LFsrc.y), 3, CV_RGB(255, 0, 0), 3);
		//cvCircle(img_AVM, cvPoint(AVMData.RFsrc.x, AVMData.RFsrc.y), 3, CV_RGB(255, 0, 0), 3);
		//cvCircle(img_AVM, cvPoint(AVMData.LBsrc.x, AVMData.LBsrc.y), 3, CV_RGB(255, 0, 0), 3);
		//cvCircle(img_AVM, cvPoint(AVMData.RBsrc.x, AVMData.RBsrc.y), 3, CV_RGB(255, 0, 0), 3);
 
		switch (stept_index)
		{ 
		case 0://x轴
			AVMData.A_rx = AVMData.A_rx - 1;
			if (AVMData.A_rx == 72)
			{
				stept_index = 1;
				AVMData.A_ry = 0;
			}
			break;
		case 1://y轴
			AVMData.A_ry = AVMData.A_ry + 1;
			if (AVMData.A_ry == 450)
			{
				stept_index = 2;
				AVMData.A_rx = 90;
				AVMData.A_rz = -20;
			}
			break;
		case 2://z轴
			AVMData.A_rz = AVMData.A_rz + 1;
			if (AVMData.A_rz == 20)
			{
				stept_index = 3;
			}
			break;
		default:
			break;
		}
		if (stept_index == 3)break;
 
		
		cvShowImage("img", img_AVM);
		
		cvWriteFrame(writer, img_AVM);
		cvWaitKey(10);
		free(AVMData.Ftable);
		free(AVMData.Btable);
		free(AVMData.Ltable);
		free(AVMData.Rtable);
 
	}
	
	cvReleaseVideoWriter(&writer);
	cvWaitKey(0);

算法视频测试代码

// TODO: 在此添加控件通知处理程序代码
	IplImage* img_AVM_all = cvCreateImage(cvSize(1280*2+1080, 1440), 8, 3);
	IplImage* img_AVM = cvCreateImage(cvSize(1080, 1440), 8, 3);
	IplImage* img_AVM_resize = cvCreateImage(cvSize(1200, 480), 8, 3);
	IplImage* det_TOP_F = cvCreateImage(cvSize(1280, 720), 8, 3);
	IplImage* det_TOP_B = cvCreateImage(cvSize(1280, 720), 8, 3);
	IplImage* det_TOP_L = cvCreateImage(cvSize(1280, 720), 8, 3);
	IplImage* det_TOP_R = cvCreateImage(cvSize(1280, 720), 8, 3);
 
	CvVideoWriter* writer = cvCreateVideoWriter("G:\\CSDN\\AVM\\Video\\AVM_Result1.avi", CV_FOURCC('X', 'V', 'I', 'D'), 25, cvSize(1280 * 2 + 1080, 1440));
	cvNamedWindow("视频播放", CV_WINDOW_AUTOSIZE);
	CvCapture* C_img_F = cvCreateFileCapture("G:\\CSDN\\AVM\\Video\\Front.avi");
	CvCapture* C_img_B = cvCreateFileCapture("G:\\CSDN\\AVM\\Video\\Back.avi");
	CvCapture* C_img_L = cvCreateFileCapture("G:\\CSDN\\AVM\\Video\\Left.avi");
	CvCapture* C_img_R = cvCreateFileCapture("G:\\CSDN\\AVM\\Video\\Right.avi");
	IplImage* img_F, *img_B, *img_L, *img_R;
	js_AVM_obj AVMData;
	AVMData.A_rx = 80; //[72 114]
	AVMData.A_ry = 0;//[0 360]
	AVMData.A_rz = 0;//[-20 20]
	js_init_avm(&AVMData, img_AVM->width, img_AVM->height);
	while (img_F = cvQueryFrame(C_img_F))
	{
		img_B = cvQueryFrame(C_img_B);
		img_L = cvQueryFrame(C_img_L);
		img_R = cvQueryFrame(C_img_R);
		js_getAVM_TOP(&AVMData, img_AVM->imageData, img_F->imageData, img_B->imageData, img_L->imageData, img_R->imageData, img_F->width, img_F->height, img_AVM->width, img_AVM->height, img_AVM->nChannels);
 
 
		cvSetImageROI(img_AVM_all, cvRect(0, 0, 1280, 720));
		cvCopy(img_F, img_AVM_all);
		cvResetImageROI(img_AVM_all);
		
		cvSetImageROI(img_AVM_all, cvRect(1280, 0, 1280, 720));
		cvCopy(img_B, img_AVM_all);
		cvResetImageROI(img_AVM_all);
 
		cvSetImageROI(img_AVM_all, cvRect(0, 720, 1280, 720));
		cvCopy(img_L, img_AVM_all);
		cvResetImageROI(img_AVM_all);
 
		cvSetImageROI(img_AVM_all, cvRect(1280, 720, 1280, 720));
		cvCopy(img_R, img_AVM_all);
		cvResetImageROI(img_AVM_all);
 
		cvSetImageROI(img_AVM_all, cvRect(1280*2, 0, 1080, 1440));
		cvCopy(img_AVM, img_AVM_all);
		cvResetImageROI(img_AVM_all);
 
		cvWriteFrame(writer, img_AVM_all);
		cvResize(img_AVM_all, img_AVM_resize);
		cvShowImage("视频播放", img_AVM_resize);
		char c = cvWaitKey(1);
		if (c == 27)break;
	}
	cvReleaseVideoWriter(&writer);
	cvReleaseCapture(&C_img_F);
	cvReleaseCapture(&C_img_B);
	cvReleaseCapture(&C_img_L);
	cvReleaseCapture(&C_img_R);
	cvDestroyWindow("视频播放");

测试效果的视频