C++/opencv实现DLT（直接线性变换法）标定相机

背景

老师布置的作业是利用已知点的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。

代码仓库地址：https://github.com/Lguanghui/openCV_DLP