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

2019-08-12 07:15:08 1.1k 阅读 AI 写同款 · GEO 优化AI 选题数据监测一键分发 ›

一文了解单线激光扫描系统的标定与成像原理单线激光扫描系统简介单线激光系统标定原理单线激光系统重建原理介绍单线激光扫描系统简介单线激光扫描系统由单线激光器和2d相机组成。激光器投射出一条激光线，在三维空间中构成一个激光平面，与待测物体的曲面相交，形成一条曲线，该曲线被相机拍摄到后，通过三角测量原理，可得这一曲线上每点的三维坐标。单线激光系统标定原理单线激光系统标定工作主要完成相机标定和激光平面方程的确定。相机标定是确定相机的像素坐标与世界坐标系之间的关系，即相机内参和外参。具体的操作过程准备一个尺寸已.. 阅读详情

求助：用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