激光平面标定求助（已知相机内参数和标定板的外参数）

asidi134 2019-08-12 07:15:08

求助：用OpenCV和C++写了一个激光平面标定的程序，目的是得到激光平面的三维方程，但方程结果不对，求大佬帮忙看一下问题出在哪里。
现在用OpenCV的相机标定函数已经获得了相机的内参数矩阵和每幅图像的外参数矩阵。再此基础上进行的激光平面标定

#include<iostream>

#include<opencv2/opencv.hpp>



using namespace std;

using namespace cv;



//全局变量：相机内参数矩阵，各标定板图像的旋转矩阵和平移矩阵

Mat instrinsic_Mat = (Mat_<double>(3, 3) << 1011.56213410783, 0, 339.4909058325158,0, 1032.8005369113, 251.8598465394016,0, 0, 1);

Mat instrinsic_Mat_invert = instrinsic_Mat.inv();

double fx = instrinsic_Mat.at<double>(0, 0);

double fy = instrinsic_Mat.at<double>(1, 1);

double cx = instrinsic_Mat.at<double>(0, 2);

double cy = instrinsic_Mat.at<double>(1, 2);



Mat ratationMat01 = (Mat_<double>(3, 1) << -2.214042276280793, -2.042125553204266, -0.1539188600609928);

Mat translationMat01 = (Mat_<double>(3, 1) << -2.924192514183808, -2.062099121158008, 28.91752066850143);



Mat ratationMat02 = (Mat_<double>(3, 1) << -0.22120867162204, -3.066096222327546, -0.6095364322919371);

Mat translationMat02 = (Mat_<double>(3, 1) << 2.069533497570583, -3.660400068141293, 26.17519894159816);



Mat ratationMat03 = (Mat_<double>(3, 1) << 1.676260593562374, 2.626057515467858, 0.03787192557694142);

Mat translationMat03 = (Mat_<double>(3, 1) << -3.729691927633717, -4.244904864126781, 28.52816373265996);



Mat ratationMat04 = (Mat_<double>(3, 1) << 1.428369712597115, 2.611472870951897, 0.5176479463955468);

Mat translationMat04 = (Mat_<double>(3, 1) << 3.414422553834327, 1.492396944583323, 37.72414534156961);



vector<Point> laserPoints_uv(Mat srcImage, string str);

Mat plane(Mat ratationMat, Mat translationMat);

vector<Point3d>  laserPoints_cam(vector<Point> laserPts_uv, Mat planeEquation);

Vec6f laserline_cam(string str, Mat ratationMat, Mat translationMat, vector<Point3f>&laser_cam);

Vec4d result(Vec6f laserEquation01, Vec6f laserEquation02);

Mat plane_fitting(vector<Point3f> input);

double error(vector<Point3f> input, Vec4d plane);



int main()

{

	//相机坐标系下激光条纹上点的集合

	vector<Point3f> laserpoint_cam01, laserpoint_cam02, laserpoint_cam03, laserpoint_cam04;

	//激光条纹的直线方程

	Vec6f laserEquation01 = laserline_cam("1.jpg", ratationMat01, translationMat01, laserpoint_cam01); 

	Vec6f laserEquation02 = laserline_cam("2.jpg", ratationMat02, translationMat02, laserpoint_cam02); 

	Vec6f laserEquation03 = laserline_cam("3.jpg", ratationMat03, translationMat03, laserpoint_cam03); 

	Vec6f laserEquation04 = laserline_cam("4.jpg", ratationMat04, translationMat04, laserpoint_cam04);

	//激光平面的方程

	Vec4d laserplane0102 = result(laserEquation01, laserEquation02);

	Vec4d laserplane0304 = result(laserEquation03, laserEquation04);

	//计算误差

	double error01 = error(laserpoint_cam03, laserplane0304);

	double error02 = error(laserpoint_cam04, laserplane0304);

	cout << " 误差1： "<< error01 << endl;

	cout << " 误差2： " << error02 << endl;



	cout << "求两个激光平面方程的比值，如果比值相同，则正确："

		<< laserplane0102[0] / laserplane0304[0] << " " 

		<< laserplane0102[1] / laserplane0304[1] << " " 

		<< laserplane0102[2] / laserplane0304[2] << " " 

		<< laserplane0102[3] / laserplane0304[3] << endl << endl;



	cout << endl << endl << endl;

	//将两条激光条纹的点集集合到一个集合中，用于直线拟合

	laserpoint_cam01.insert(laserpoint_cam01.end(), laserpoint_cam02.begin(), laserpoint_cam02.end());

	laserpoint_cam03.insert(laserpoint_cam03.end(), laserpoint_cam04.begin(), laserpoint_cam04.end());

	

	//用最小二乘法对激光平面进行拟合

	Mat plane0102 = plane_fitting(laserpoint_cam01);

	Mat plane0304 = plane_fitting(laserpoint_cam03);



	waitKey(0);

	system("pause");

	return 0;

}



//***********************************************

//  第二种方法：通过最小二乘法，通过像素坐标系下激光平面上的点来模拟激光平面方程

//***********************************************

Mat plane_fitting(vector<Point3f> input)

{

	Mat dst = Mat(3, 3, CV_32F, Scalar(0));

	Mat out = Mat(3, 1, CV_32F, Scalar(0));

	for (int i = 0; i < input.size(); i++)

	{

		dst.at<float>(0, 0) += pow(input[i].x, 2);

		dst.at<float>(0, 1) += input[i].x * input[i].y;

		dst.at<float>(0, 2) += input[i].x;

		dst.at<float>(1, 0) += input[i].x * input[i].y;

		dst.at<float>(1, 1) += pow(input[i].y, 2);

		dst.at<float>(1, 2) += input[i].y;

		dst.at<float>(2, 0) += input[i].x;;

		dst.at<float>(2, 1) += input[i].y;

		dst.at<float>(2, 2) = input.size();



		out.at<float>(0, 0) += (input[i].x*input[i].z);

		out.at<float>(1, 0) += (input[i].y*input[i].z);

		out.at<float>(2, 0) += input[i].z;

	}

	double determ = determinant(dst);

	if (abs(determ) < 0.001)

	{

		cout << "矩阵奇异" << endl;

		//return 0;

	}



	Mat dst_invert = dst.inv();

	Mat output = dst_invert * out;



	float a = output.at<float>(0, 0);

	float b = output.at<float>(1, 0);

	float c = output.at<float>(2, 0);

	float varsum = 0;

	for (int k = 0; k < input.size(); k++)

	{

		varsum += pow((a * input[k].x + b * input[k].y + c - input[k].z),2);	

	}

	cout << varsum / input.size() << endl;

	return output;

}



//***********************************************

// 第六步：检验误差

//***********************************************

double error(vector<Point3f> input, Vec4d plane)

{

	double a = -plane[0] / plane[2];

	double b = -plane[1] / plane[2];

	double c = -plane[3] / plane[2];

	float varsum = 0;

	for (int k = 0; k < input.size(); k++)

	{

		varsum += pow((a * input[k].x + b * input[k].y + c - input[k].z), 2);

	}

	return varsum / input.size();

}



//***********************************************

// 第五步：通过通过两个激光直线方程确定激光平面方程

//***********************************************

Vec4d result(Vec6f laserEquation01, Vec6f laserEquation02)

{

	double h1 = laserEquation01[0];		double k1 = laserEquation01[1];		double l1 = laserEquation01[2];

	double h2 = laserEquation02[0];		double k2 = laserEquation02[1];		double l2 = laserEquation02[2];



	double a = k1 * l2 - l1 * k2;

	double b = l1 * h2 - l2 * h1;

	double c = h1 * k2 - k1 * h2;

	double d = -(a*laserEquation02[3] + b * laserEquation02[4] + c * laserEquation02[5]);



	cout << "激光平面方程为：" << a << " " << b << " " << c << " " << d << endl << endl;



	Vec4d laserplane = Vec4d(a, b, c, d);

	return laserplane;



}



//***********************************************

// 第四步： 获取激光直线方程

//***********************************************

Vec6f laserline_cam(string str, Mat ratationMat, Mat translationMat,vector<Point3f>&laser_cam)

{

	Mat laserImage = imread(str, 0);

	Mat planes = plane(ratationMat, translationMat);

	vector<Point> laserpts = laserPoints_uv(laserImage, str);

	vector<Point3d> laser_camPoints = laserPoints_cam(laserpts, planes);

	laser_cam.assign(laser_camPoints.begin(),laser_camPoints.end());



	Vec6f laserEquation;

	fitLine(laser_camPoints, laserEquation, DIST_L1, 0, 0.01, 0.01);//图像1中，激光条纹在相机坐标系的直线方程

	cout << " 激光平面方程为："<< laserEquation << endl;

	return laserEquation;

}



//***********************************************

//第三步： 将激光条纹上的点从像素坐标系转换为相机坐标系

//***********************************************

vector<Point3d>  laserPoints_cam(vector<Point> laserPts_uv, Mat planeEquation)

{

	//平面方程的方向向量

	double a = planeEquation.at<double>(0, 0);

	double b = planeEquation.at<double>(0, 1);

	double c = planeEquation.at<double>(0, 2);

	double d = planeEquation.at<double>(0, 3);



	vector<Point3d> camPoints;

	for (int i = 0; i < laserPts_uv.size(); i++)

	{

		//激光条纹上的点在像素坐标系下为pixelPoint，（u,v,1）

		Mat pixelPoint = (Mat_<double>(3, 1) << laserPts_uv[i].x, laserPts_uv[i].y, 1);

		

		Mat imagePoint = instrinsic_Mat_invert * pixelPoint;//(u,v,1)乘相机的内参数矩阵的逆，就是激光条纹特征点在归一化平面的坐标，位于相机坐标系

		

		double x1 = imagePoint.at<double>(0, 0);

		double y1 = imagePoint.at<double>(1, 0);

		double z1 = imagePoint.at<double>(2, 0);

		double r2 = pow(x1, 2) + pow(y1, 2);



		double xc = (-d * x1) / (a*x1 + b * y1 + c);

		double yc = (-d * y1) / (a*x1 + b * y1 + c);

		double zc = -d / (a*x1 + b * y1 + c);

		

		Poi