写在前面
单位一个同事写的相机标定代码,个人作为学习与参考,感谢同事的帮助
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();//暂停
}