求助：用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
uj5u.com熱心網友回復：
求助啊，別沉啊uj5u.com熱心網友回復：
看整個流程問題不大， plane函式是如何計算的？uj5u.com熱心網友回復：
解決了，是精度的問題uj5u.com熱心網友回復：
哪塊的精度問題啊，大佬你是怎么解決的
轉載請註明出處，本文鏈接：https://www.uj5u.com/houduan/90281.html
標籤：工具平臺和程序庫