目录
- 背景
- C++实现
-
- 头文件和全局变量
- 读取txt文件里的坐标信息
- main函数
-
- 选取标定点
- 构造A矩阵
- 构造U矩阵
- 计算L矩阵
- 求解内参数
- 求解外参数
- 计算重投影误差
- 收尾
- 运行结果
背景
老师布置的作业是利用已知点的3维坐标信息和成像信息，标定相机的内外参数，并计算误差。已知点的信息如下：
其中，前三列是XYZ世界坐标，最后两列是成像平面的u v坐标。
建议先吃透理论知识，理论知识可以参考：
- 经典：传统相机标定方法解析：直接线性法和Tsai两步标定法（推荐）
- DLT method（需要科学上网）（推荐）
- 《利用二维 DLT 进行数字摄像机标定》（汤想明）(知网资源）
C++实现
头文件和全局变量
#include <stdio.h>
#include <iostream>
#include<opencv2/opencv.hpp>
#include<opencv2/core/core.hpp>
#include<fstream>
#include <string>
#include <sstream>
using namespace std;
using namespace cv;
float **Points=nullptr;//全局指针，存放坐标数组
int Points_count=0;
读取txt文件里的坐标信息
//读取txt文件
void readfile(string filename)
{
ifstream file("/Users/yunyi/Desktop/Calibration_Points.txt");
string Line;
string buf;
int i=0,j=0;
//动态申请二维数组存放坐标
Points=(float**)malloc(sizeof(float*)*30);
for(int k=0;k<30;k++){
*(Points+k)=(float*)malloc(sizeof(float)*5);
}
//打开文件，把坐标存入数组
if(file)
{
while (getline(file, Line)) {//按行读取
stringstream split(Line);//按空格分割
while(split>>buf){
Points[i][j]=stof(buf);
j++;
}
i++;
j=0;//不能忘了清零列标！
Points_count++;
}
}
file.close();
}
main函数
选取标定点
由理论知识可知，6个不共面的点即可完成对相机的标定。剩余的点可以用来测试标定结果。
//从文件中读取坐标点
string filename="/Users/yunyi/Desktop/Calibration_Points.txt";
readfile(filename);
//显示28个坐标点
cout<<"全部坐标点："<<endl;
for(int l=0;l<Points_count;l++)
{
for (int m=0; m<5; m++) {
cout<<Points[l][m]<<" ";
}
cout<<endl;
}
//选取6个标定点
float Points_cali[6][5];
//int index[6]={0,3,5,8,15,27};
int index[6]={0,8,14,19,23,27};
int ii=0;
for (int i=0; i<6; i++) {
for (int j=0; j<5; j++) {
Points_cali[i][j]=Points[index[ii]][j];
}
ii++;
}
//其余的点
float Points_remain[22][5];
int index_remain[22]={1,2,3,4,5,6,7,9,10,11,12,13,15,16,17,18,20,21,22,24,25,26};
ii=0;
for(int i=0;i<22;i++){
for (int j=0; j<5; j++) {
Points_remain[i][j]=Points[index_remain[ii]][j];
}
ii++;
}
构造A矩阵
A和U矩阵是求解L矩阵的关键。A矩阵即是：
//构造A矩阵
ii=0;
float a[12][11]={0};
for (int i=0; i<12; i++) {
if(i%2==0)
{
a[i][0]=Points_cali[ii][0];
a[i][1]=Points_cali[ii][1];
a[i][2]=Points_cali[ii][2];
a[i][3]=1;
a[i][8]=-Points_cali[ii][0]*Points_cali[ii][3];
a[i][9]=-Points_cali[ii][1]*Points_cali[ii][3];
a[i][10]=-Points_cali[ii][2]*Points_cali[ii][3];
}
else{
a[i][4]=Points_cali[ii][0];
a[i][5]=Points_cali[ii][1];
a[i][6]=Points_cali[ii][2];
a[i][7]=1;
a[i][8]=-Points_cali[ii][0]*Points_cali[ii][4];
a[i][9]=-Points_cali[ii][1]*Points_cali[ii][4];
a[i][10]=-Points_cali[ii][2]*Points_cali[ii][4];
ii++;
}
}
cout<<endl<<"A矩阵:"<<endl;
Mat A(12, 11, CV_32F,a);//存放A矩阵
cout<<A<<endl;
构造U矩阵
U矩阵：
其中，P34=1，因此U矩阵其实就是标定点的u和v坐标堆在一起。
//构造U矩阵
float u[12]={0};
ii=0;
for(int i=0;i<12;i=i+2){
u[i]=Points_cali[ii][3];
u[i+1]=Points_cali[ii][4];
ii++;
}
Mat U(12,1,CV_32F,u);
cout<<endl<<"U矩阵："<<endl;
cout<<U<<endl;
计算L矩阵
根据上面L和U、A矩阵的关系，计算L矩阵。
L矩阵把内外参数综合在了一起。
//计算得到L矩阵
Mat L=Mat::zeros(11, 1, CV_32F);
L=(((A.t())*A).inv())*(A.t())*U;
cout<<endl<<"L矩阵："<<endl<<L<<endl;
求解内参数
公式我就不贴了，可以参考文章开头的参考链接。
//计算u0 v0
float u0=((L.at<float>(0,0))*(L.at<float>(8,0))+(L.at<float>(1,0))*(L.at<float>(9,0))+(L.at<float>(2,0))*(L.at<float>(10,0)))/(pow(L.at<float>(8,0), 2)+pow(L.at<float>(9,0), 2)+pow(L.at<float>(10,0), 2));
float v0=((L.at<float>(4,0))*(L.at<float>(8,0))+(L.at<float>(5,0))*(L.at<float>(9,0))+(L.at<float>(6,0))*(L.at<float>(10,0)))/(pow(L.at<float>(8,0), 2)+pow(L.at<float>(9,0), 2)+pow(L.at<float>(10,0), 2));
//计算fu fv
//一种计算方式
//float fu=sqrt(((pow(L.at<float>(0,0),2))+(pow(L.at<float>(0,1),2))+(pow(L.at<float>(0,2),2)))/(pow(L.at<float>(0,8), 2)+pow(L.at<float>(0,9), 2)+pow(L.at<float>(0,10), 2))-pow(u0, 2));
//float fv=sqrt(((pow(L.at<float>(0,4),2))+(pow(L.at<float>(0,5),2))+(pow(L.at<float>(0,6),2)))/(pow(L.at<float>(0,8), 2)+pow(L.at<float>(0,9), 2)+pow(L.at<float>(0,10), 2))-pow(v0, 2));
//另一种计算方式
float fu=sqrt((pow(u0*L.at<float>(8,0)-L.at<float>(0,0), 2)+pow(u0*L.at<float>(9,0)-L.at<float>(1,0), 2)+pow(u0*L.at<float>(10,0)-L.at<float>(2,0), 2))/(pow(L.at<float>(0,8), 2)+pow(L.at<float>(0,9), 2)+pow(L.at<float>(0,10), 2)));
float fv=sqrt((pow(v0*L.at<float>(8,0)-L.at<float>(4,0), 2)+pow(v0*L.at<float>(9,0)-L.at<float>(5,0), 2)+pow(v0*L.at<float>(10,0)-L.at<float>(6,0), 2))/(pow(L.at<float>(0,8), 2)+pow(L.at<float>(0,9), 2)+pow(L.at<float>(0,10), 2)));
Mat K=(Mat_<float>(3, 3)<<fu,0,u0,0,fv,v0,0,0,1);//内参数矩阵
cout<<endl<<"内参数矩阵："<<endl<<K<<endl;
计算得到的数值应该都是非负数。
求解外参数
//把L矩阵转化成3*4形式，最后一个l12取1
Mat L_34=(Mat_<float>(3, 4)<<L.at<float>(0,0),L.at<float>(1,0),L.at<float>(2,0),L.at<float>(3,0),L.at<float>(4,0),L.at<float>(5,0),L.at<float>(6,0),L.at<float>(7,0),L.at<float>(8,0),L.at<float>(9,0),L.at<float>(10,0),1);//L矩阵的3*4形式
cout<<endl<<"外参数矩阵："<<endl<<(K.inv())*L_34/sqrt(pow(L.at<float>(0,8), 2)+pow(L.at<float>(0,9), 2)+pow(L.at<float>(0,10), 2))<<endl;
计算重投影误差
//计算重投影误差
//计算用于标定的坐标点重投影误差
cout<<endl<<"标定点的误差计算：";
float err_caliPoints[6]={0};
float err_caliPoints_mean=0;
float err_relative=0;
float u_restruct,v_restruct;
for (int i=0; i<6; i++) {
u_restruct=(Points_cali[i][0]*L.at<float>(0,0)+Points_cali[i][1]*L.at<float>(1,0)+Points_cali[i][2]*L.at<float>(2,0)+L.at<float>(3,0))/(Points_cali[i][0]*L.at<float>(8,0)+Points_cali[i][1]*L.at<float>(9,0)+Points_cali[i][2]*L.at<float>(10,0)+1);
v_restruct=(Points_cali[i][0]*L.at<float>(4,0)+Points_cali[i][1]*L.at<float>(5,0)+Points_cali[i][2]*L.at<float>(6,0)+L.at<float>(7,0))/(Points_cali[i][0]*L.at<float>(8,0)+Points_cali[i][1]*L.at<float>(9,0)+Points_cali[i][2]*L.at<float>(10,0)+1);
err_caliPoints[i]=sqrt(pow(u_restruct-Points_cali[i][3], 2)+pow(v_restruct-Points_cali[i][4], 2));
cout<<endl<<"标定点"<<i+1<<"的重投影误差："<<err_caliPoints[i]<<" 相对误差："<<abs(err_caliPoints[i])/sqrt(pow(Points_cali[i][3],2)+pow(Points_cali[i][4], 2))<<endl;
err_caliPoints_mean=err_caliPoints_mean+err_caliPoints[i];
err_relative=err_relative+abs(err_caliPoints[i])/sqrt(pow(Points_cali[i][3],2)+pow(Points_cali[i][4], 2));
}
cout<<"均值："<<err_caliPoints_mean/6<<" "<<err_relative/6<<endl;
其余点的重投影误差计算步骤和上述代码大同小异。
收尾
由于用了一个动态二维数组存储坐标点，因此程序结尾要释放空间。
//释放空间
for(int i = 0; i < 30;i++){
free(*(Points+i));
}
运行结果
其中，外参数矩阵是下图的M矩阵（即L矩阵，下图是老师的ppt，采用的是M矩阵的叫法）中m34未归一化的结果。上面的步骤中，L矩阵的最后一个值L12取1就是把右下角的m34提取出来的结果。但其实m34=1/｜m3｜，并不是1。