北邮鲁鹏老师三维重建课程之相机标定

2023-11-19 11:41:28

在看北邮鲁鹏老师的三维重建的课程过程中，去官网找到有三个作业。现将三个作业里面的第一个作业相机标定完成。总体来说，可以分为三个部分，即图像坐标点和世界坐标点的获取；映射矩阵的生成，相机内外参的求解三个部分。现总结如下：

图像坐标点的获取

上鲁鹏老师作业里边的标定图，如下图所示：

通过该图可以看到世界坐标系建立在立体标版的原点，一个方形格子代表单位1，三个互相垂直的平面上各取了四个点，并用框标明了。因此，我们第一步就是要获得上述12个点的图像坐标。

在这里，我用halcon对图像进行处理，获取到框内外的边缘，并对角点像素坐标估算，求内外角点平均值得到图像坐标点。（这里本来想直接求角点的，最后估算了一下）

有更好的方法可以交流。

得到对应的点的关系如下：

(0,8,7)->(363.5,1143) (0,4,7)->(320,958) (0,8,3)->(616,1112) (0,4,3)->(542,943)
(8,0,9)->(212,404) (6,0,9)->(201,523) (8,0,1)->(667,459) (6,0,1)->(642,559)
(4,1,0)->(684,679) (5,1,0)->(695,635) (5,9,0)->(913,896) (4,9,0)->(889,946)

read_image (Jietu20200301091513, 'E:/文件文档/鲁鹏-三维重建资料/Total3DExercises-main/MVGlab01_camera-calibration-master/images/Jietu20200301-091513.jpg')
get_image_size (Jietu20200301091513, Width, Height)
dev_clear_window ()
dev_close_window ()
dev_open_window (0, 0, Width/2, Height/2, 'black', WindowHandle)
dev_display (Jietu20200301091513)
rgb1_to_gray (Jietu20200301091513, GrayImage)
*寻找第一个和第二个矩形区域
threshold (GrayImage, Region, 75, 90)
connection (Region, ConnectedRegions)
fill_up (ConnectedRegions, RegionFillUp)
select_shape (RegionFillUp, SelectedRegions1, ['rectangularity','area'], 'and', [0.7,17500], [1,50000])
*寻找第三个矩形区域
dev_clear_window ()
dev_display (Jietu20200301091513)
threshold (GrayImage, Region1, 128, 140)
connection (Region1, ConnectedRegions1)
fill_up (ConnectedRegions1, RegionFillUp1)
select_shape (RegionFillUp1, SelectedRegions2, ['rectangularity','area'], 'and', [0.7,40000], [1,80000])
*将上述三个区域合并
concat_obj (SelectedRegions1, SelectedRegions2, ObjectsConcat)
count_obj (ObjectsConcat, Number)
dev_display (Jietu20200301091513)
dev_display (ObjectsConcat)
stop ()
*分别对三个区域进行形态学处理，并获得矩形边角的角点图像坐标
for Index := 1 to Number by 1
    select_obj (ObjectsConcat, ObjectSelected, Index)
    *最大的框
    dilation_rectangle1 (ObjectSelected, RegionDilation, 31, 31)
    *将该区域像素全部置为255
    paint_region (RegionDilation, GrayImage, ImageResult, 255, 'fill')
    *膨胀操作,腐蚀操作，做差裁剪ROI
    dilation_rectangle1 (ObjectSelected, RegionDilation1, 5, 5)
    erosion_rectangle1 (RegionDilation1, RegionErosion, 21, 21)
    difference (RegionDilation1, RegionErosion, RegionDifference)
    paint_region (RegionDifference, ImageResult, ImageResult2, 0, 'fill')
    reduce_domain (ImageResult2, RegionDilation, ImageReduced)
    invert_image (ImageReduced, ImageInvert)
    edges_sub_pix (ImageInvert, Edges1, 'canny', 1, 20, 40)
    select_contours_xld (Edges1, SelectedContours, 'contour_length', 100, 30000, 100, 30000)
    dev_display (Jietu20200301091513)
    dev_display (SelectedContours)
    
endfor

提取出边框的内外轮廓，根据内外轮廓估计角点位置。

生成图像坐标点到世界坐标点的映射矩阵

首先通过鲁鹏老师的课件进行说明：

也就是，基于图像和世界坐标点对应的关系，构建P矩阵，这里我用C++ OPENCV进行构建：

#include "stdafx.h"
#include <stdio.h>
#include <cmath>
#include<io.h>
#include <fstream>
#include <iostream>
#include <direct.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <map>
#include <algorithm>

using namespace std;
using namespace cv;

int main()
{
    //读取标定图像文件
    Mat src = imread(".\\images\\Jietu20200301-091513.jpg",4);
    //找到图像点对应标版坐标系世界点的坐标关系
    //根据imagewatch可以看到相关的关系
    //先找到图像中对应四边形的角点
    //(0,8,7)->(363.5,1143) (0,4,7)->(320,958)  (0,8,3)->(616,1112)  (0,4,3)->(542,943)
    //(8,0,9)->(212,404)    (6,0,9)->(201,523)  (8,0,1)->(667,459)  (6,0,1)->(642,559)
    //(4,1,0)->(684,679)    (5,1,0)->(695,635)  (5,9,0)->(913,896)  (4,9,0)->(889,946)
    //根据上面的点开始构建矩阵关系
    //声明两个动态数组用来存储点集
    vector<Point3d> vec_worldpoints;
    vector<Point2d> vec_picpoints;
    vec_worldpoints.push_back(Point3f(0, 8, 7));
    vec_worldpoints.push_back(Point3f(0, 4, 7));
    vec_worldpoints.push_back(Point3f(0, 8, 3));
    vec_worldpoints.push_back(Point3f(0, 4, 3));
    vec_picpoints.push_back(Point2f(363.5, 1143));
    vec_picpoints.push_back(Point2f(320, 958));
    vec_picpoints.push_back(Point2f(616, 1112));
    vec_picpoints.push_back(Point2f(542, 943));

    vec_worldpoints.push_back(Point3f(8, 0, 9));
    vec_worldpoints.push_back(Point3f(6, 0, 9));
    vec_worldpoints.push_back(Point3f(8, 0, 1));
    vec_worldpoints.push_back(Point3f(6, 0, 1));
    vec_picpoints.push_back(Point2f(212, 404));
    vec_picpoints.push_back(Point2f(201, 523));
    vec_picpoints.push_back(Point2f(667, 459));
    vec_picpoints.push_back(Point2f(642, 559));

    vec_worldpoints.push_back(Point3f(4, 1, 0));
    vec_worldpoints.push_back(Point3f(5, 1, 0));
    vec_worldpoints.push_back(Point3f(5, 9, 0));
    vec_worldpoints.push_back(Point3f(4, 9, 0));
    vec_picpoints.push_back(Point2f(684, 679));
    vec_picpoints.push_back(Point2f(695, 635));
    vec_picpoints.push_back(Point2f(913, 896));
    vec_picpoints.push_back(Point2f(889, 946));
    //将上述点用来构建矩阵
    Mat P = (Mat_<double>(24, 12));
    for (int i = 0; i < 12; i++)
    {
        //第一行（奇数行）赋值
        P.at<double>(2 * i, 0) = vec_worldpoints[i].x;
        P.at<double>(2 * i, 1) = vec_worldpoints[i].y;
        P.at<double>(2 * i, 2) = vec_worldpoints[i].z;
        P.at<double>(2 * i, 3) = 1;
        P.at<double>(2 * i, 4) = 0;
        P.at<double>(2 * i, 5) = 0;
        P.at<double>(2 * i, 6) = 0;
        P.at<double>(2 * i, 7) = 0;
        P.at<double>(2 * i, 8) = -(vec_picpoints[i].x)*(vec_worldpoints[i].x);
        P.at<double>(2 * i, 9) = -(vec_picpoints[i].x)*(vec_worldpoints[i].y);
        P.at<double>(2 * i, 10) = -(vec_picpoints[i].x)*(vec_worldpoints[i].z);
        P.at<double>(2 * i, 11) = -vec_picpoints[i].x;
        //第二行（偶数行）赋值
        P.at<double>(2 * i + 1, 0) = 0;
        P.at<double>(2 * i + 1, 1) = 0;
        P.at<double>(2 * i + 1, 2) = 0;
        P.at<double>(2 * i + 1, 3) = 0;
        P.at<double>(2 * i + 1, 4) = vec_worldpoints[i].x;
        P.at<double>(2 * i + 1, 5) = vec_worldpoints[i].y;
        P.at<double>(2 * i + 1, 6) = vec_worldpoints[i].z;
        P.at<double>(2 * i + 1, 7) = 1;
        P.at<double>(2 * i + 1, 8) = -(vec_picpoints[i].y)*(vec_worldpoints[i].x);
        P.at<double>(2 * i + 1, 9) = -(vec_picpoints[i].y)*(vec_worldpoints[i].y);
        P.at<double>(2 * i + 1, 10) = -(vec_picpoints[i].y)*(vec_worldpoints[i].z);
        P.at<double>(2 * i + 1, 11) = -vec_picpoints[i].y;
    }
    return 0;
}

这里推荐VS的插件imagewatch，可以直接将opencv里边的mat类型变量可视化，构建后的矩阵如下图所示：

这里本来是一个24行的矩阵，只截取出部分行。接着就是对该矩阵进行奇异值分解

    //对矩阵P进行奇异值分解，并分别求右矩阵的转置和逆
	//因为右矩阵为正交矩阵，所以他的逆和装置相等
	Mat W, U, Vt;
	SVD::compute(P, W, U, Vt, 4);
	Mat Vtt = Vt.t();
	Mat Vtinv = Vt.inv();
	//根据奇异值分解得到的右矩阵，得到透视投影矩阵M
	//p=MP  p为图像坐标点，P为世界坐标点
	//下面就是矩阵的赋值
	Mat M(Mat_<double>(3, 4));
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			switch (i)
			{
			case 0:
				M.at<double>(i, j) = Vtt.at<double>(j, 11);
				break;
			case 1:
				M.at<double>(i, j) = Vtt.at<double>(j + 4, 11);
				break;
			case 2:
				M.at<double>(i, j) = Vtt.at<double>(j + 8, 11);
				break;
			default:
				break;
			}
		}
	}
	//来个点进行测试，将上述一个世界坐标系点齐次坐标形式(0,8,7,1)与M相乘
	//得到放缩系数
	//检验出来图像坐标为（363.13，1142.6）与对应的图像坐标(363.5,1143)误差在一个像素以内
	Mat picPoint, worldPoint, uvPoint;
	worldPoint = (Mat_<double>(4, 1) << 0.0, 8.0, 7.0, 1.0);
	picPoint = M*worldPoint;
	double scalFactor = 1 / picPoint.at<double>(2, 0);
	uvPoint = scalFactor*picPoint;

分解得到右向量的最后一个列向量，即最小奇异值对应的列向量，将该向量转为3*4的矩阵形式，其矩阵如下图所示：（如果对上面代码有问题，想去看看鲁鹏老师课程，对照课件就明了了）

到这里为校验下M矩阵的精度，对世界坐标点(0,8,7,1)进行重投影，即p=MP，将世界坐标点代入投影矩阵，得到图像坐标uo，v0，与原值图像坐标(363.5,1143)进行对比。

下图为将上述一个世界坐标系点齐次坐标形式(0,8,7,1)与M相乘，经过放缩得到的u0,v0

检验出来图像坐标为（363.13，1142.6）与对应的图像坐标(363.5,1143)误差在一个像素以内。

相机内外参数的求取

那么接下来就是根据M矩阵求解相机的内外参数，还是那句话，对过程有疑问，去复习视频和课件，只要脑海中有那个过程，代码很好理解。

所有参数的求解，都是基于上面图片的这些公式。

    Mat finalM = M;
	//我们知道p=finalM*P，其中p为图像坐标系的齐次坐标，P为世界坐标系的齐次坐标
	//finalM为确定了放缩系数以后的透视投影矩阵
	//p=finalM*P=K*[R T]*P
	//K*[R T]=finalM=[A b]
	//同时我们知道R T外参矩阵对应旋转平移，旋转为3*3，平移为3*1
	//因此我们得到矩阵A为finalM的左边三列，矩阵b为finalM的最右一列
	//先分离出A b矩阵
	Mat A = (Mat_<double>(3, 3));
	Mat b = (Mat_<double>(3, 1));
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			A.at<double>(i, j) = finalM.at<double>(i, j);
		}
	}
	for (size_t i = 0; i < 3; i++)
	{
		b.at<double>(i, 0) = finalM.at<double>(i, 3);
	}
	//求A的转置
	Mat At = A.t();
	//分解出a1,a2,a3，之前先转置
	//声明三个列向量
	Mat a1 = (Mat_<double>(3, 1));
	Mat a2 = (Mat_<double>(3, 1));
	Mat a3 = (Mat_<double>(3, 1));
	for (int i = 0; i < 3; i++)
	{
		a1.at<double>(i, 0) = At.at<double>(i, 0);
		a2.at<double>(i, 0) = At.at<double>(i, 1);
		a3.at<double>(i, 0) = At.at<double>(i, 2);
	}
	//先求放缩系数
	double a31 = a3.at<double>(0, 0);
	double a32 = a3.at<double>(1, 0);
	double a33 = a3.at<double>(2, 0);
	double factor = 1.0 / sqrt(pow(a31, 2) + pow(a32, 2) + pow(a33, 2));
	//开始求内参，先求中心点u0 和v0

	Mat u0 = pow(factor, 2)* a1.t()*a3;
	Mat v0 = pow(factor, 2)* a3.t()*a2;

到这里，我们求得了相机内参的中心点，uo，v0（634.93，736.16）

中心点偏差好像有点大，但不算特别离谱，因为这里是默认理想透视模型，忽略了畸变。

    //接着求内参theta角
    //cos(theta)=-[(a1×a3)·(a2×a3)]/[|a1×a3|·|a2×a3|]
    //先写一个向量叉乘的函数
    //上面将a1，a2，a3向量，以矩阵形式表示的，这里用opencv的Vec3f表示
    Vec3d A1, A2, A3;
    for (int i = 0; i < 3; i++)
    {
        A1[i] = At.at<double>(i, 0);
        A2[i] = At.at<double>(i, 1);
        A3[i] = At.at<double>(i, 2);
    }
    double factor1 = 1.0 / sqrt(pow(A3[0], 2) + pow(A3[1], 2) + pow(A3[2], 2));
    double U0 = pow(factor1, 2)* A1.dot(A3);
    double V0 = pow(factor1, 2)* A3.dot(A2);
    //定义costheta
    //自定义一个函数计算，两个向量叉乘的模
    double costhea = -(A1.cross(A3)).dot(A2.cross(A3)) / (ModulusofCrossProduct(A1, A3)*ModulusofCrossProduct(A2, A3));
    double sinthea = sqrt(1 - pow(costhea, 2));
    //接着求解水平、垂直方向的焦距alpha、beta
    double alpha = pow(factor1, 2)*ModulusofCrossProduct(A1, A3)*sinthea;
    double beta = pow(factor1, 2)*ModulusofCrossProduct(A2, A3)*sinthea;
    //至此，相机的内部参数求解完毕
    //开始求解相机外参
    //r1=(a2×a3)/|a2×a3|  r3=±a3/|a3|   r2=r1×r3
    Vec3d r1 = A2.cross(A3) / ModulusofCrossProduct(A2, A3);
    Vec3d r3 = A3 / sqrt(pow(A3[0], 2) + pow(A3[1], 2) + pow(A3[2], 2));
    Vec3d r2 = r1.cross(r3);
    Mat Rt = (Mat_<double>(3, 3));
    //将向量赋值给Rt矩阵
    for (int i = 0; i < 3; i++)
    {
        Rt.at<double>(0, i) = r1[i];
        Rt.at<double>(1, i) = r2[i];
        Rt.at<double>(2, i) = r3[i];
    }
    Mat R = Rt.t();
    //平移矩阵T=pK(-1)b  这里p为放缩系数，K(-1)为相机内参矩阵的逆，b矩阵为M矩阵里面分解出来的
    //首先根据求出来的内部参数，构建内参矩阵K
    Mat K = (Mat_<double>(3, 3));
    K.at<double>(0, 0) = alpha;
    K.at<double>(0, 1) = -alpha*(costhea/sinthea);
    K.at<double>(0, 2) = U0;
    K.at<double>(1, 0) = 0;
    K.at<double>(1, 1) = beta/sinthea;
    K.at<double>(1, 2) = V0;
    K.at<double>(2, 0) = 0;
    K.at<double>(2, 1) = 0;
    K.at<double>(2, 2) = 1;
    //求解T矩阵
    Mat T = (Mat_<double>(3,1));
    T = factor1*K.inv()*b;

    return 0;
}

double ModulusofCrossProduct(Vec3d v1, Vec3d v2)
{
    //向量维度不一样，直接返回零值
    if (v1.cols!=v2.cols&&v1.rows!=v2.rows)
    {
        return 0;
    }
    //两个向量的叉乘是一个矩阵
    Vec3d m = v1.cross(v2);
    //返回矩阵的行列式
    double temp = sqrt(pow(m[0], 2) + pow(m[1], 2) + pow(m[2], 2));
    return temp;
}

最后得到相机的内参矩阵K、外参旋转矩阵R、外参平移矩阵T，其结果如图所示：

至此，相机标定部分完成，接下来将实现单视图三维重建。

码农公寓

图像坐标点的获取

生成图像坐标点到世界坐标点的映射矩阵

相机内外参数的求取

相关文章