相机的标定(一)

写在前面

单位一个同事写的相机标定代码,个人作为学习与参考,感谢同事的帮助

CameraCalibrator2

// CameraCalibrator2.cpp : 定义控制台应用程序的入口点。
//

#include "stdafx.h"

#include "opencv2\opencv.hpp"
#include <string>

#pragma comment(lib, "opencv_world410.lib")//包含库文件
using namespace cv;
using namespace std;
#pragma warning(disable:4996)

#ifndef ModelInfoST
#define ModelInfoST
struct st_model
{
	int ID;//设备编号
	double pdCamera[9];//相机内参
	double pdCoefficients[5];//摄像机的5个畸变系数:k1,k2,p1,p2,k3仅支持径向,切向
	double pdRmat[9];//标定光平面旋转矩阵参数
	double pdTmat[3];//标定光平面平移矩阵参数
	st_model()
	{
		ID = -1;
		memset(pdCamera, 0, 9 * sizeof(double));
		memset(pdCoefficients, 0, 5 * sizeof(double));
		memset(pdRmat, 0, 9 * sizeof(double));
		memset(pdTmat, 0, 3 * sizeof(double));
	}
};
#endif

#ifndef Mat_ModelInfoST
#define Mat_ModelInfoST
struct st_Value_Info
{
	Mat mat_Carmera;
	Mat mat_Coefficients;
	Mat mat_InvH;
	Mat mat_Img;
	Point2f* pvecP;
	int nLen;
	st_Value_Info()
	{
		pvecP = NULL;
		nLen = 0;
	}
};
#endif

int ReadModel(const char* filePath, Mat& m_Carmera, Mat& m_Coefficients, Mat& m_InvH);//标定数据模型读取
void on_mouse(int event, int x, int y, int flags, void* ustc);
//图像畸变检测点==》世界坐标
void ComputeWorldPoints(Mat m_InvH, vector<Point2f> vecP, vector<Point2f>& vecPw);
void GetLaserWorldPoint(Mat Img, st_Value_Info m_Info, int offsetX, int offsetY);

int ReadModel(const char* filePath, Mat& m_Carmera, Mat& m_Coefficients, Mat& m_InvH)
{	
	//读取标定数据
	FILE* fp = fopen(filePath, "rb");
	if (fp == NULL)
	{
		printf("模型文件读取失败\n");
		return 0;
	}

	if (!feof(fp))
	{
		st_model model;
		fread(&model, sizeof(st_model), 1, fp);
		
		Mat cameraM(3, 3, CV_64FC1, model.pdCamera);//相机内参
		Mat Coefficients(1, 5, CV_64FC1, model.pdCoefficients);//相机畸变系数
		//Mat R(3, 3, CV_64FC1, model.pdRmat);//相机外参旋转矩阵
		//Mat T(3, 1, CV_64FC1, model.pdTmat);//相机外参平移矩阵
		Mat RT = Mat(3, 3, CV_64FC1, Scalar::all(0));//z平面为0简化
		RT.at<double>(0, 0) = model.pdRmat[0];
		RT.at<double>(0, 1) = model.pdRmat[1];
		RT.at<double>(0, 2) = model.pdTmat[0];
		RT.at<double>(1, 0) = model.pdRmat[3];
		RT.at<double>(1, 1) = model.pdRmat[4];
		RT.at<double>(1, 2) = model.pdTmat[1];
		RT.at<double>(2, 0) = model.pdRmat[6];
		RT.at<double>(2, 1) = model.pdRmat[7];
		RT.at<double>(2, 2) = model.pdTmat[2];
		Mat H = cameraM * RT;
		m_Carmera = cameraM.clone();
		m_Coefficients = Coefficients.clone();
		Mat invH = H.inv();
		m_InvH = invH.clone();
	}
	if (fp)
	{
		fclose(fp);
		fp = NULL;
	}

	return 1;
}

void GetLaserWorldPoint(Mat Img, st_Value_Info m_Info, int offsetX, int offsetY)
{
	Mat mBw;	
	threshold(Img, mBw, 120, 255, THRESH_BINARY);
	Mat element = getStructuringElement(MORPH_RECT, Size(3, 3));
	erode(mBw, mBw, element);//腐蚀操作	
	Mat elementP = getStructuringElement(MORPH_RECT, Size(5, 5));
	dilate(mBw, mBw, elementP);//膨胀操作
		
	shared_ptr<Point2f> pvecP = shared_ptr<Point2f>(new Point2f[mBw.cols], default_delete<Point2f[]>());
	memset(pvecP.get(), 0, sizeof(Point2f) * mBw.cols);
	for (int j = 0; j < mBw.cols; j++)
	{
		double dgraytotal = 0;
		double dgrayY = 0;
		for (int i = 0; i < mBw.rows; i++)
		{
			unsigned char v = mBw.at<unsigned char>(i, j);
			dgraytotal += v;
			dgrayY += v * i;
		}
		if (dgraytotal > 255)
		{		
			Point2f pt = { (float)j, (float)(dgrayY / dgraytotal) };
			pvecP.get()[j] = pt;
		}
	}
	Mat mDraw;
	cvtColor(mBw, mDraw, COLOR_GRAY2BGR);
	for (int i = 0; i < mBw.cols; i++)
	{		
		circle(mDraw, pvecP.get()[i], 1, Scalar(0, 0, 255));
		//更新坐标
		pvecP.get()[i].x += offsetX;
		pvecP.get()[i].y += offsetY;
	}
	m_Info.nLen = mBw.cols;
	m_Info.pvecP = pvecP.get();
	m_Info.mat_Img = mDraw.clone();

	namedWindow("激光光斑检测");
	imshow("激光光斑检测", mDraw);	
	setMouseCallback("激光光斑检测", on_mouse, (void*)&m_Info);
	waitKey(0);
	destroyWindow("激光光斑检测");
}

void ComputeWorldPoints(Mat m_InvH, vector<Point2f> vecP, vector<Point2f>& vecPw)
{
	Point2f pt;
	for (int i = 0; i < vecP.size(); i++)
	{
		Mat P = Mat(3, 1, CV_64FC1, Scalar::all(0));
		P.at<double>(0, 0) = vecP[i].x;
		P.at<double>(1, 0) = vecP[i].y;
		P.at<double>(2, 0) = 1;//扩展齐次坐标
		Mat Pw = m_InvH * P;
		double dDenom = Pw.at<double>(2, 0);
		if (fabs(dDenom) < 0.0000001)
		{
			dDenom = 0.0000001;
		}
		pt.x = Pw.at<double>(0, 0) / dDenom;
		pt.y = Pw.at<double>(1, 0) / dDenom;
		vecPw.push_back(pt);
	}
}

void on_mouse(int event, int x, int y, int flags, void* ustc)
{
	st_Value_Info mInfo = *(st_Value_Info*)ustc;
	switch (event)
	{
	case EVENT_MOUSEMOVE:
	{
		if (mInfo.nLen > 0 && x > 0 && x < mInfo.nLen)
		{
			Mat draw = mInfo.mat_Img.clone();
			line(draw, Point2f(x, 0), Point2f(x, draw.cols - 1), Scalar(0, 0, 255), 1);
			imshow("激光光斑检测", draw);
		}		
	}
		break;
	case EVENT_LBUTTONDOWN: // 鼠标左键按下			
	{
		if (mInfo.nLen > 0 && x > 0 && x < mInfo.nLen)
		{			
			vector<Point2f> vecSrc, vecDst, vecPw;
			vecSrc.push_back(mInfo.pvecP[x]);
			undistortPoints(vecSrc, vecDst, mInfo.mat_Carmera, mInfo.mat_Coefficients, noArray(), mInfo.mat_Carmera);
			ComputeWorldPoints(mInfo.mat_InvH, vecDst, vecPw);
			for (int i = 0; i < vecDst.size(); i++)
			{
				printf("光斑中心点(x,y):(%.2f, %.2f)=======> 物理坐标(x,y):(%.2f, %.2f)\n", vecDst[i].x, vecDst[i].y, vecPw[i].x, vecPw[i].y);
			}
		}
	}
		break;
	case EVENT_LBUTTONUP: // 鼠标左键释放
		//cout << "buttonup" << endl;
		break;
	}
}

void main(int argc, char** argv)
{	
	if (argc != 5)
	{
		printf("参数传入有误\n");
		return;
	}
	char* ImgFile = argv[1];
	char* ModelPath = argv[2];
	int offsetX = stoi(argv[3]);
	int offsetY = stoi(argv[4]);	

	st_Value_Info m_Info;
	int nRes = ReadModel(ModelPath, m_Info.mat_Carmera, m_Info.mat_Coefficients, m_Info.mat_InvH);
	if (nRes != 1)
	{
		printf("标定数据模型文件读取失败!\n");
		return;
	}

	Mat Img = imread(ImgFile, 0);
	GetLaserWorldPoint(Img, m_Info, offsetX, offsetY);
	getchar();//暂停
}


SingleCameraCalibrator

// SingleCameraCalibrator.cpp : 定义控制台应用程序的入口点。
//
#include "stdafx.h"
#include "opencv2\opencv.hpp"
#include <string>

#pragma comment(lib, "opencv_world410.lib")//包含库文件
using namespace cv;
using namespace std;
#pragma warning(disable:4996)

#ifndef ERR_STRUCT
#define ERR_STRUCT
struct ERR
{
	int index;
	double derr;
	Point2f pt_Img;
	Point3f pt_Pw;
	ERR(int ind, double dist, Point2f Img, Point3f Pw)
	{
		index = ind;
		derr = dist;
		pt_Img = Img;
		pt_Pw = Pw;
	}
	bool operator < (ERR item)
	{
		if (derr > item.derr)
			return true;
		else
			return false;
	};
};
#endif

#ifndef ModelInfoST
#define ModelInfoST
struct st_model
{
	int ID;//设备编号
	double pdCamera[9];//相机内参
	double pdCoefficients[5];//摄像机的5个畸变系数:k1,k2,p1,p2,k3仅支持径向,切向
	double pdRmat[9];//标定光平面旋转矩阵参数
	double pdTmat[3];//标定光平面平移矩阵参数
	st_model()
	{
		ID = -1;
		memset(pdCamera, 0, 9 * sizeof(double));
		memset(pdCoefficients, 0, 5 * sizeof(double));
		memset(pdRmat, 0, 9 * sizeof(double));
		memset(pdTmat, 0, 3 * sizeof(double));
	}
};
#endif

//读取相机内参数据
void ReadCameraParameters(char* strFile, Mat& cameraM, Mat& Coefficients, st_model& model);
void ReadFile_ini(char* strFile, int& ID, Rect& rect);
void Read_datalist(char* strFile, Rect roi_rect, double dx_offset, double dy_offset, vector<Point2f>& vecImg, vector<Point3f>& vecPw);
//相机参数标定,内参,外参
void SingleCameraCalibration(char* IntrinsicsFile, char* iniFile, char* dataListFile, char* modelFile, double dx_offset, double dy_offset);


void ReadCameraParameters(char* strFile, Mat& cameraM, Mat& Coefficients, st_model& model)
{
	//读取标定数据
	Mat cameraMatrix = Mat(3, 3, CV_64FC1, cv::Scalar::all(0));//内外参矩阵,H——单应性矩阵
	Mat distCoefficients = Mat(1, 5, CV_64FC1, cv::Scalar::all(0));//摄像机的5个畸变系数:k1,k2,p1,p2,k3
	FILE* fp = fopen(strFile, "r");
	double dMatrix[9] = { 0 };
	double dCoefficients[5] = { 0 };
	while (!feof(fp))
	{
		//获取内参
		for (int i = 0; i < 9; i++)
		{
			fscanf(fp, "%lf\t", &dMatrix[i]);
			model.pdCamera[i] = dMatrix[i];
		}
		//获取畸变系数
		for (int i = 0; i < 5; i++)
		{
			fscanf(fp, "%lf\t", &dCoefficients[i]);
			model.pdCoefficients[i] = dCoefficients[i];
		}
	}
	//赋值
	for (int i = 0; i < 3; i++)
	{
		double* pfRow = cameraMatrix.ptr<double>(i);
		for (int j = 0; j < 3; j++)
		{
			pfRow[j] = dMatrix[i * 3 + j];
		}
	}

	double* pfRow = distCoefficients.ptr<double>(0);
	for (int j = 0; j < 5; j++)
	{
		pfRow[j] = dCoefficients[j];
	}
	if (fp)
	{
		fclose(fp);
		fp = NULL;
	}
	cameraM = cameraMatrix.clone();
	Coefficients = distCoefficients.clone();
}

void ReadFile_ini(char* strFile, int& ID, Rect& rect)
{
	char temp[128] = { 0 };
	GetPrivateProfileString("SET", "ID", "-1", temp, 128, strFile);
	ID = atoi(temp);

	GetPrivateProfileString("SET", "OffsetX", "0", temp, 128, strFile);
	rect.x = atoi(temp);

	GetPrivateProfileString("SET", "OffsetY", "0", temp, 128, strFile);
	rect.y = atoi(temp);

	GetPrivateProfileString("SET", "Roi_Width", "0", temp, 128, strFile);
	rect.width = atoi(temp);

	GetPrivateProfileString("SET", "Roi_Height", "0", temp, 128, strFile);
	rect.height = atoi(temp);
}


void Read_datalist(char* strFile, Rect roi_rect, double dx_offset, double dy_offset, vector<Point2f>& vecImg, vector<Point3f>& vecPw)
{
	FILE* fp = fopen(strFile, "r");
	char filePath[512] = { 0 };
	while (!feof(fp))
	{
		fscanf(fp, "%s\n", filePath);
		FILE* fpi = fopen(filePath, "r");
		if (fpi)
		{	
			double pwx, pwy, Ix, Iy;
			int ncount = 0;
			while (!feof(fpi))
			{
				fscanf(fpi, "%lf %lf %lf %lf\n", &pwx, &pwy, &Ix, &Iy);
				Point2f pt = { (float)Ix + roi_rect.x, (float)Iy + roi_rect.y };
				vecImg.push_back(pt);
				Point3f ptw;
				ptw.x = float(pwx + dx_offset);
				ptw.y = (float)(pwy + dy_offset);
				ptw.z = 0;
				vecPw.push_back(ptw);
				ncount++;
			}

			fclose(fpi);
			printf("文件:%s\n", filePath);
			printf("数据总数:%d\n", ncount);
		}
		else
		{
			printf("%s 文件读取失败\n", filePath);
		}
	}
	if (fp)
	{
		fclose(fp);
	}
}

//相机参数标定,内参,外参
void SingleCameraCalibration(char* IntrinsicsFile, char* iniFile, char* dataListFile, char* modelFile, double dx_offset, double dy_offset)
{
	st_model model;	
	//读取内参矩阵数据	
	Mat cameraM, distCoefficients;
	ReadCameraParameters(IntrinsicsFile, cameraM, distCoefficients, model);
	//显示值测试
	printf("相机内参矩阵:\n");
	for (int i = 0; i < cameraM.rows; i++)
	{
		for (int j = 0; j < cameraM.cols; j++)
		{
			printf("%lf\t", cameraM.at<double>(i, j));			
		}
		printf("\n");
	}
	printf("\n");
	printf("镜头畸变系数:\n");
	for (int i = 0; i < distCoefficients.rows; i++)
	{
		for (int j = 0; j < distCoefficients.cols; j++)
		{
			printf("%lf\t", distCoefficients.at<double>(i, j));
		}
		printf("\n");
	}
	printf("\n");
	//读取配置文件	
	int ID;
	Rect roi_rect;
	ReadFile_ini(iniFile, ID, roi_rect);
	if (ID < 0 || roi_rect.width < 100 || roi_rect.height < 100)
	{
		printf("配置文件数据有误\n");	
		return;
	}
	model.ID = ID;

	printf("设备编号:%d, 图像ROI区域:offsetX = %d, offsetY = %d, Width = %d, Height = %d\n",
		ID, roi_rect.x, roi_rect.y, roi_rect.width, roi_rect.height);
	printf("\n");
	//读取标定图像数据文件	
	vector<Point2f> vecImgTotal;
	vector<Point3f> vecPwTotal;	
	Read_datalist(dataListFile, roi_rect, dx_offset, dy_offset, vecImgTotal, vecPwTotal);
	
	vector<Point2f> vecImg;
	vector<Point3f> vecPw;
	for (int i = 0; i < vecImgTotal.size(); i++)
	{
		vecImg.push_back(vecImgTotal[i]);
		vecPw.push_back(vecPwTotal[i]);
	}
	printf("*********************结构光平面标定*********************\n");
	int Flag = 0;//标定标志,0标定中,1标定完成
	while (vecImg.size() > 10 && vecPw.size() > 10)
	{
		//由相机内参获取某一平面的旋转向量,平移向量
		Mat rvec, tvec;
		bool b = solvePnPRansac(vecPw, vecImg, cameraM, distCoefficients, rvec, tvec);

		//由旋转向量获取旋转矩阵
		Mat rotation_Matrix = cv::Mat(3, 3, CV_32FC1, cv::Scalar::all(0));//旋转矩阵
		Rodrigues(rvec, rotation_Matrix);

		printf("\n");
		printf("旋转矩阵:\n");
		int Ind = 0;
		for (int i = 0; i < rotation_Matrix.rows; i++)
		{
			for (int j = 0; j < rotation_Matrix.cols; j++)
			{
				printf("%f\t", rotation_Matrix.at<double>(i, j));
				model.pdRmat[Ind] = rotation_Matrix.at<double>(i, j);
				Ind++;
			}
			printf("\n");
		}

		printf("\n");
		printf("平移矩阵:\n");
		Ind = 0;
		for (int i = 0; i < tvec.rows; i++)
		{
			for (int j = 0; j < tvec.cols; j++)
			{
				printf("%f\t", tvec.at<double>(i, j));
				model.pdTmat[Ind] = tvec.at<double>(i, j);
				Ind++;
			}
			printf("\n");
		}

		//验证标定结果		
		Mat M = Mat(3, 3, CV_64FC1, Scalar::all(0));
		M.at<double>(0, 0) = rotation_Matrix.at<double>(0, 0);
		M.at<double>(0, 1) = rotation_Matrix.at<double>(0, 1);
		M.at<double>(0, 2) = tvec.at<double>(0, 0);
		M.at<double>(1, 0) = rotation_Matrix.at<double>(1, 0);
		M.at<double>(1, 1) = rotation_Matrix.at<double>(1, 1);
		M.at<double>(1, 2) = tvec.at<double>(1, 0);
		M.at<double>(2, 0) = rotation_Matrix.at<double>(2, 0);
		M.at<double>(2, 1) = rotation_Matrix.at<double>(2, 1);
		M.at<double>(2, 2) = tvec.at<double>(2, 0);

		Mat H = cameraM * M;
		Mat invH = H.inv();//单应性可逆矩阵		
	
		//畸变校正
		vector<Point2f> vecDst;
		//undistortPoints(vecImg, vecDst, cameraM, distCoefficients);//校正后点为相机坐标
		undistortPoints(vecImg, vecDst, cameraM, distCoefficients, noArray(), cameraM);//校正后点为同为像素坐标

		//图像畸变校正点==》世界坐标
		Point2f pt;
		vector<Point2f> vecdstPw;
		for (int i = 0; i < vecDst.size(); i++)
		{
			Mat P = Mat(3, 1, CV_64FC1, Scalar::all(0));
			P.at<double>(0, 0) = vecDst[i].x;
			P.at<double>(1, 0) = vecDst[i].y;
			P.at<double>(2, 0) = 1;//扩展齐次坐标
			Mat Pw = invH * P;
			double dDenom = Pw.at<double>(2, 0);
			if (fabs(dDenom) < 0.0000001)
			{
				dDenom = 0.0000001;
			}
			pt.x = Pw.at<double>(0, 0) / dDenom;
			pt.y = Pw.at<double>(1, 0) / dDenom;
			vecdstPw.push_back(pt);
		}

		//统计分析结果
		double davgx = 0;
		double davgy = 0;
		vector<ERR> vecErr;
		for (int i = 0; i < vecdstPw.size(); i++)
		{
			davgx += fabs(vecdstPw[i].x - vecPw[i].x);
			davgy += fabs(vecdstPw[i].y - vecPw[i].y);
			double dist = sqrt((vecdstPw[i].x - vecPw[i].x) * (vecdstPw[i].x - vecPw[i].x) + (vecdstPw[i].y - vecPw[i].y) *(vecdstPw[i].y - vecPw[i].y));
			ERR pt(i, dist, vecImg[i], vecPw[i]);
			vecErr.push_back(pt);
		}
		davgx = davgx / vecdstPw.size();
		davgy = davgy / vecdstPw.size();
		
		printf("\n***********************标靶点验证***********************\n");
		sort(vecErr.begin(), vecErr.end());
		printf("\n误差较大前10:\n");
		for (int i = 0; i < 10; i++)
		{
			int ind = vecErr[i].index;
			printf("标靶:(%.2lf,%.2lf), 计算:(%.2lf,%.2lf)  误差:(%.2lf,%.2lf)\n",
				vecErr[i].pt_Pw.x, vecErr[i].pt_Pw.y, vecdstPw[ind].x, vecdstPw[ind].y, fabs(vecErr[i].pt_Pw.x - vecdstPw[ind].x), fabs(vecErr[i].pt_Pw.y - vecdstPw[ind].y));
		}

		printf("\n标靶点总数:%d\nx方向平均误差:%lf, y方向平均误差:%lf\n", vecdstPw.size(), davgx, davgy);
		printf("\n标定结果是否达标(Y/N)?\n");	
		while (1)
		{
			char c = getchar();
			if (c == 'Y' || c == 'y' )
			{
				Flag = 1;
				break;
			}
			else if (c == 'N' || c == 'n')
			{
				printf("**********************重新标定**********************\n");
				break;
			}
			else if (c != '\n')
			{
				printf("请重新确认\n");				
			}			
		}
		if (Flag == 1)
		{
			printf("**********************标定已完成**********************\n");
			//写入模型文件			
			FILE* fpb = fopen(modelFile, "wb");
			fwrite(&model, sizeof(st_model), 1, fpb);
			if (fpb)
			{
				fclose(fpb);
				fpb = NULL;
			}
			break;
		}
		else
		{
			printf("\n请输入N[1~9]去除误差较大前N个重新标定\n");
			while (1)
			{
				char c = getchar();
				if (c > '0' && c <= '9')
				{	
					int count = (int)(c - '0');//字符转数字
					printf("count: %d\n",count);				
					vecImg.clear();
					vecPw.clear();
					for (int i = 0; i < vecErr.size(); i++)
					{						
						if (i > count - 1)
						{
							vecImg.push_back(vecErr[i].pt_Img);
							vecPw.push_back(vecErr[i].pt_Pw);
						}
					}				
					break;
				}				
				else if (c != '\n')
				{
					printf("请重新输入N\n");
				}
			}
		}
	}	
}

void main(int argc, char** argv)
{	
	if (argc != 7)
	{
		printf("参数传入有误\n");
		return;
	}	
	char* IntrinsicsFile = argv[1];
	char* iniFile = argv[2];
	char* dataListFile = argv[3];
	char* modelFile = argv[4];
	double dx_offset = stod(argv[5]);
	double dy_offset = stod(argv[6]);	
	//相机参数标定,内参,外参
	SingleCameraCalibration(IntrinsicsFile, iniFile, dataListFile, modelFile, dx_offset, dy_offset);
	getchar();//暂停
}


评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值