一、图像成像原理
-
世界坐标系到相机坐标系
世界坐标系是空间中根据需求任意指定的坐标系，相机坐标系是以镜头为原点，光心方向为Z轴方向的坐标系。
同一个点在不同坐标系下的坐标可以通过平移旋转的仿射变换得到。设点在世界坐标系下的坐标为 ( X , Y , Z ) T (X,Y,Z)^T (X,Y,Z)T，在相机坐标系下的坐标为 ( X c , Y c , Z c ) T (X_c,Y_c,Z_c)^T (Xc,Yc,Zc)T，旋转矩阵 R = [ r 00 r 01 r 02 r 10 r 11 r 12 r 20 r 21 r 22 ] R=\left[ \begin{array}{ccc} r_{00} & r_{01} & r_{02}\\ r_{10} & r_{11} & r_{12}\\ r_{20} & r_{21} & r_{22}\\ \end{array} \right] R=⎣⎡r00r10r20r01r11r21r02r12r22⎦⎤,平移矩阵 T = [ T x T y T z ] T=\left[ \begin{array}{ccc} T_x\\ T_y\\ T_z\\ \end{array} \right] T=⎣⎡TxTyTz⎦⎤，则有
[ X c Y c Z c ] = R [ X Y Z ] + T \left[ \begin{array}{ccc} X_c\\ Y_c\\ Z_c\\ \end{array} \right]=R\left[ \begin{array}{ccc} X\\ Y\\ Z\\ \end{array} \right]+T ⎣⎡XcYcZc⎦⎤=R⎣⎡XYZ⎦⎤+T
即
[ X c Y c Z c 1 ] = [ r 00 r 01 r 02 T x r 10 r 11 r 12 T y r 20 r 21 r 22 T z 0 0 0 1 ] [ X Y Z 1 ] \left[ \begin{array}{ccc} X_c\\ Y_c\\ Z_c\\ 1 \end{array} \right]=\left[ \begin{array}{ccc} r_{00} & r_{01} & r_{02}&T_x\\ r_{10} & r_{11} & r_{12}&T_y\\ r_{20} & r_{21} & r_{22}&T_z\\ 0&0&0&1 \end{array} \right]\left[ \begin{array}{ccc} X\\ Y\\ Z\\ 1 \end{array} \right] ⎣⎢⎢⎡XcYcZc1⎦⎥⎥⎤=⎣⎢⎢⎡r00r10r200r01r11r210r02r12r220TxTyTz1⎦⎥⎥⎤⎣⎢⎢⎡XYZ1⎦⎥⎥⎤ -
相机坐标系到图像坐标系
图像坐标系是在图像成像平面上建立的2维坐标系。相当于相机坐标系中的 Z = f Z=f Z=f平面单独拿出来作为坐标系，其中 f f f是镜头焦距。根据小孔成像原理用相似三角形计算位置
如上图，设点P在相机坐标系下的坐标为 ( X c , Y c , Z c ) T (X_c,Y_c,Z_c)^T (Xc,Yc,Zc)T，在成像平面上投影点为 ( x c , y c ) T (x_c,y_c)^T (xc,yc)T，则有
Z c f = X c x c = Y c y c \frac{Z_c}{f}=\frac{X_c}{x_c}=\frac{Y_c}{y_c} fZc=xcXc=ycYc
从而有
[ x c y c ] = f Z c [ X c Y c ] = [ f Z c 0 0 f Z c ] [ X c Y c ] \left[ \begin{array}{ccc} x_c\\ y_c\\ \end{array} \right]=\frac{f}{Z_c}\left[ \begin{array}{ccc} X_c\\ Y_c\\ \end{array} \right]=\left[ \begin{array}{ccc} \frac{f}{Z_c}&0\\ 0&\frac{f}{Z_c}\\ \end{array} \right]\left[ \begin{array}{ccc} X_c\\ Y_c\\ \end{array} \right] [xcyc]=Zcf[XcYc]=[Zcf00Zcf][XcYc]
即
[ x c y c 1 ] = 1 Z c [ f 0 0 0 f 0 0 0 1 ] [ X c Y c Z c ] \left[ \begin{array}{ccc} x_c\\ y_c\\ 1 \end{array} \right]=\frac{1}{Z_c}\left[ \begin{array}{ccc} f&0&0\\ 0&f&0\\ 0&0&1 \end{array} \right]\left[ \begin{array}{ccc} X_c\\ Y_c\\ Z_c \end{array} \right] ⎣⎡xcyc1⎦⎤=Zc1⎣⎡f000f0001⎦⎤⎣⎡XcYcZc⎦⎤ -
图像坐标系到像素坐标系
像素坐标系原点在图像左上方是离散的，图像坐标系原点在图像中央（不考虑畸变的情况下），是连续的，图像转像素相当于进行坐标系平移，再进行离散化（数据同时除以单个晶圆大小unit size）。设像素坐标为 ( u , v ) T (u,v)^T (u,v)T，有
[ u v ] = [ ( x c + c x ′ ) / d x ( y c + c y ′ ) / d y ] \left[ \begin{array}{ccc} u\\ v\\ \end{array} \right]=\left[ \begin{array}{ccc} (x_c+c_x^{'})/dx\\ (y_c+c_y^{'})/dy\\ \end{array} \right] [uv]=[(xc+cx′)/dx(yc+cy′)/dy]
即
[ u v 1 ] = [ 1 d x 0 c x 0 1 d y c y 0 0 1 ] [ x c y c 1 ] \left[ \begin{array}{ccc} u\\ v\\ 1 \end{array} \right]=\left[ \begin{array}{ccc} \frac{1}{dx}&0&c_x\\ 0&\frac{1}{dy}&c_y\\ 0&0&1 \end{array} \right]\left[ \begin{array}{ccc} x_c\\ y_c\\ 1 \end{array} \right] ⎣⎡uv1⎦⎤=⎣⎡dx1000dy10cxcy1⎦⎤⎣⎡xcyc1⎦⎤
其中 c x ′ , c y ′ c_x^{'},c_y^{'} cx′,cy′分别是两个方向上平移的物理距离，同时除以每个像素的大小后得到平移的像素个数。理想情况下 c x , c y c_x,c_y cx,cy是最终成像图像的像素值一半，即使考虑畸变也不会差很多 -
相机坐标系到像素坐标系
将（2）（3）等式结合,有
[ u v 1 ] = [ 1 d x 0 c x 0 1 d y c y 0 0 1 ] [ x c y c 1 ] \left[ \begin{array}{ccc} u\\ v\\ 1 \end{array} \right]=\left[ \begin{array}{ccc} \frac{1}{dx}&0&c_x\\ 0&\frac{1}{dy}&c_y\\ 0&0&1 \end{array} \right]\left[ \begin{array}{ccc} x_c\\ y_c\\ 1 \end{array} \right] ⎣⎡uv1⎦⎤=⎣⎡dx1000dy10cxcy1⎦⎤⎣⎡xcyc1⎦⎤
= [ 1 d x 0 c x 0 1 d y c y 0 0 1 ] 1 Z c [ f 0 0 0 f 0 0 0 1 ] [ X c Y c Z c ] =\left[ \begin{array}{ccc} \frac{1}{dx}&0&c_x\\ 0&\frac{1}{dy}&c_y\\ 0&0&1 \end{array} \right]\frac{1}{Z_c}\left[ \begin{array}{ccc} f&0&0\\ 0&f&0\\ 0&0&1 \end{array} \right]\left[ \begin{array}{ccc} X_c\\ Y_c\\ Z_c \end{array} \right] =⎣⎡dx1000dy10cxcy1⎦⎤Zc1⎣⎡f000f0001⎦⎤⎣⎡XcYcZc⎦⎤
= 1 Z c [ f x 0 c x 0 f y c y 0 0 1 ] [ X c Y c Z c ] =\frac{1}{Z_c}\left[ \begin{array}{ccc} f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1 \end{array} \right]\left[ \begin{array}{ccc} X_c\\ Y_c\\ Z_c \end{array} \right] =Zc1⎣⎡fx000fy0cxcy1⎦⎤⎣⎡XcYcZc⎦⎤
其中 f x = f d x , f y = f d y f_x=\frac{f}{dx},f_y=\frac{f}{d_y} fx=dxf,fy=dyf -
世界坐标系到像素坐标系
将（1）（4）整合得
Z c [ u v 1 ] = [ f x 0 c x 0 f y c y 0 0 1 ] [ X c Y c Z c ] = [ f x 0 c x 0 0 f y c y 0 0 0 1 0 ] [ X c Y c Z c 1 ] = [ f x 0 c x 0 0 f y c y 0 0 0 1 0 ] [ R T 0 1 ] [ X Y Z 1 ] Z_c\left[ \begin{array}{ccc} u\\ v\\ 1 \end{array} \right]=\left[ \begin{array}{ccc} f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1 \end{array} \right]\left[ \begin{array}{ccc} X_c\\ Y_c\\ Z_c \end{array} \right]\\ =\left[ \begin{array}{ccc} f_x&0&c_x&0\\ 0&f_y&c_y&0\\ 0&0&1&0 \end{array} \right]\left[ \begin{array}{ccc} X_c\\ Y_c\\ Z_c\\ 1 \end{array} \right]\\ =\left[ \begin{array}{ccc} f_x&0&c_x&0\\ 0&f_y&c_y&0\\ 0&0&1&0 \end{array} \right]\left[ \begin{array}{ccc} R&T\\ 0&1\\ \end{array} \right]\left[ \begin{array}{ccc} X\\ Y\\ Z\\ 1 \end{array} \right] Zc⎣⎡uv1⎦⎤=⎣⎡fx000fy0cxcy1⎦⎤⎣⎡XcYcZc⎦⎤=⎣⎡fx000fy0cxcy1000⎦⎤⎣⎢⎢⎡XcYcZc1⎦⎥⎥⎤=⎣⎡fx000fy0cxcy1000⎦⎤[R0T1]⎣⎢⎢⎡XYZ1⎦⎥⎥⎤
二、相机标定
以上完成成像原理理论部分公式计算，下面基于此，进行张正友标定法的理论推导
因为世界坐标系是自己定义的，为了方便计算通常将棋盘格所在平面当作XOY面，即坐标系中的Z=0平面，此时的(5)可以简化为
Z
c
[
u
v
1
]
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
[
r
00
r
01
T
x
r
10
r
11
T
y
r
20
r
21
T
z
]
[
X
Y
1
]
=
A
[
R
0
,
R
1
,
T
]
[
X
Y
1
]
Z_c\left[ \begin{array}{ccc} u\\ v\\ 1 \end{array} \right]=\left[ \begin{array}{ccc} f_x&0&c_x\\ 0&f_y&c_y\\ 0&0&1 \end{array} \right]\left[ \begin{array}{ccc} r_{00}&r_{01}&Tx\\ r_{10}&r_{11}&T_y\\ r_{20}&r_{21}&T_z \end{array} \right]\left[ \begin{array}{ccc} X\\ Y\\ 1 \end{array} \right]\\ =A[R_0,R_1,T]\left[ \begin{array}{ccc} X\\ Y\\ 1 \end{array} \right]
Zc⎣⎡uv1⎦⎤=⎣⎡fx000fy0cxcy1⎦⎤⎣⎡r00r10r20r01r11r21TxTyTz⎦⎤⎣⎡XY1⎦⎤=A[R0,R1,T]⎣⎡XY1⎦⎤
其中
A
A
A是内参矩阵，这几个值相机出厂就定好了，不会改变，
R
,
T
R,T
R,T分别是旋转矩阵和平移矩阵
R
0
,
R
1
R_0,R_1
R0,R1是
R
R
R的前两列。同一张棋盘格照片的内参和外参都是一样的，即同一张图的
A
[
R
0
,
R
1
,
T
]
A[R_0,R1,T]
A[R0,R1,T]是不变的。设
H
=
A
[
R
0
,
R
1
,
T
]
H=A[R_0,R_1,T]
H=A[R0,R1,T]，是一个
3
×
3
3\times3
3×3的矩阵，有
Z
c
[
u
v
1
]
=
A
[
R
0
,
R
1
,
T
]
[
X
Y
1
]
=
H
[
X
Y
1
]
Z_c\left[ \begin{array}{ccc} u\\ v\\ 1 \end{array} \right]=A[R_0,R_1,T]\left[ \begin{array}{ccc} X\\ Y\\ 1 \end{array} \right]=H\left[ \begin{array}{ccc} X\\ Y\\ 1 \end{array} \right]
Zc⎣⎡uv1⎦⎤=A[R0,R1,T]⎣⎡XY1⎦⎤=H⎣⎡XY1⎦⎤
从而有
[
u
v
1
]
=
1
Z
c
H
[
X
Y
1
]
=
1
Z
c
[
H
00
H
01
H
02
H
10
H
11
H
12
H
20
H
21
H
22
]
[
X
Y
1
]
\left[ \begin{array}{ccc} u\\ v\\ 1 \end{array} \right]=\frac{1}{Z_c}H\left[ \begin{array}{ccc} X\\ Y\\ 1 \end{array} \right]=\frac{1}{Z_c}\left[ \begin{array}{ccc} H_{00}&H_{01}&H_{02}\\ H_{10}&H_{11}&H_{12}\\ H_{20}&H_{21}&H_{22} \end{array} \right]\left[ \begin{array}{ccc} X\\ Y\\ 1 \end{array} \right]
⎣⎡uv1⎦⎤=Zc1H⎣⎡XY1⎦⎤=Zc1⎣⎡H00H10H20H01H11H21H02H12H22⎦⎤⎣⎡XY1⎦⎤
展开得
u
=
(
H
00
X
+
H
01
Y
+
H
02
)
/
Z
c
v
=
(
H
10
X
+
H
11
Y
+
H
12
)
/
Z
c
1
=
(
H
20
X
+
H
21
Y
+
H
22
)
/
Z
c
u=(H_{00}X+H_{01}Y+H_{02})/Z_c\\ v=(H_{10}X+H_{11}Y+H_{12})/Z_c\\ 1=(H_{20}X+H_{21}Y+H_{22})/Z_c
u=(H00X+H01Y+H02)/Zcv=(H10X+H11Y+H12)/Zc1=(H20X+H21Y+H22)/Zc
消去
Z
c
Z_c
Zc得到
u
=
H
00
X
+
H
01
Y
+
H
02
H
20
X
+
H
21
Y
+
H
22
v
=
H
10
X
+
H
11
Y
+
H
12
H
20
X
+
H
21
Y
+
H
22
u=\frac{H_{00}X+H_{01}Y+H_{02}}{H_{20}X+H_{21}Y+H_{22}}\\ v=\frac{H_{10}X+H_{11}Y+H_{12}}{H_{20}X+H_{21}Y+H_{22}}
u=H20X+H21Y+H22H00X+H01Y+H02v=H20X+H21Y+H22H10X+H11Y+H12
化成关于
H
i
j
(
i
,
j
=
0
,
1
,
2
)
H_{ij}(i,j=0,1,2)
Hij(i,j=0,1,2)的线性方程组
X
H
00
+
Y
H
01
+
H
02
−
u
X
H
20
−
u
Y
H
21
−
H
22
=
0
XH_{00}+YH_{01}+H_{02}-uXH_{20}-uYH_{21}-H_{22}=0
XH00+YH01+H02−uXH20−uYH21−H22=0
X
H
10
+
Y
H
11
+
H
12
−
v
X
H
20
−
v
Y
H
21
−
H
22
=
0
XH_{10}+YH_{11}+H_{12}-vXH_{20}-vYH_{21}-H_{22}=0
XH10+YH11+H12−vXH20−vYH21−H22=0
由
H
H
H的定义可得
H
22
=
T
z
H_{22}=T_z
H22=Tz肯定不为0，不失一般性可以令
H
22
=
1
H_{22}=1
H22=1,这样矩阵
H
H
H中还有八个变量，每个点提工两个方程组，所有要计算这八个变量至少需要4个点，当点变多可以使用最小二乘拟合。最后得到
H
H
H.
H
H
H计算完成后，下面计算内参，同一张图片的每个内角点的
H
H
H都是一样的，当标定板移动位置后
H
H
H发生变换，但是内参依旧不变，可以基于此计算内参。
H
=
[
h
1
,
h
2
,
h
3
]
=
A
[
R
1
,
R
2
,
T
]
H=[h_1,h_2,h_3]=A[R_1,R_2,T]
H=[h1,h2,h3]=A[R1,R2,T]
h
i
(
i
=
1
,
2
,
3
)
h_i(i=1,2,3)
hi(i=1,2,3)是
H
H
H的三列单独领出来，有
h
1
=
A
R
1
,
h
2
=
A
R
2
h_1=AR_1,h_2=AR_2
h1=AR1,h2=AR2
由
R
R
R作为旋转矩阵的正交性和正规性有
<
R
1
,
R
1
>
=
<
R
2
,
R
2
>
=
1
<R_1,R_1>=<R_2,R_2>=1
<R1,R1>=<R2,R2>=1
<
R
1
,
R
2
>
=
R
1
T
R
2
=
0
<R_1,R_2>=R_1^TR_2=0
<R1,R2>=R1TR2=0
用
A
,
h
i
j
A,h_{ij}
A,hij替换得到
h
1
T
A
−
T
A
−
1
h
2
=
0
h_1^TA^{-T}A^{-1}h_2=0
h1TA−TA−1h2=0
h
1
T
A
−
T
A
−
1
h
1
=
h
2
T
A
−
T
A
−
1
h
2
h_1^TA^{-T}A^{-1}h_1=h_2^TA^{-T}A^{-1}h_2
h1TA−TA−1h1=h2TA−TA−1h2
h
i
(
i
=
1
,
2
)
h_{i}(i=1,2)
hi(i=1,2)已经计算出来了，那么未知变量只有
A
A
A中的4个元素，一个
H
H
H可以提供2个等式，所有2个H就可以计算所有变量
令
B
=
A
−
T
A
−
1
=
[
1
f
x
2
0
−
c
x
f
x
0
1
f
y
2
−
c
y
f
y
−
c
x
f
x
2
−
c
y
f
y
2
c
x
2
f
x
2
+
c
y
2
f
y
2
+
1
]
B=A^{-T}A^{-1}=\left[ \begin{array}{ccc} \frac{1}{f_x^2}&0&-\frac{c_x}{f_x}\\\\ 0&\frac{1}{f_y^2}&-\frac{cy}{fy}\\\\ -\frac{c_x}{f_x^2}&-\frac{c_y}{f_y^2}&\frac{c_x^2}{f_x^2}+\frac{c_y^2}{f_y^2}+1 \end{array} \right]
B=A−TA−1=⎣⎢⎢⎢⎢⎢⎢⎡fx210−fx2cx0fy21−fy2cy−fxcx−fycyfx2cx2+fy2cy2+1⎦⎥⎥⎥⎥⎥⎥⎤
B
T
=
B
B^T=B
BT=B,有效元有6个，可待定系数
令
h
i
T
B
h
j
=
v
i
j
T
b
h_i^TBh_j=v_{ij}^Tb
hiTBhj=vijTb,
b
b
b就是那六个有效元形成的向量
待定系数解得
v
i
j
=
(
h
i
1
h
j
1
,
h
i
2
h
j
1
+
h
i
1
h
j
2
,
h
i
3
h
j
1
+
h
i
1
h
j
3
,
h
i
2
h
j
2
,
h
i
3
h
j
2
+
h
i
2
h
j
3
,
h
i
3
h
j
3
)
v_{ij}=(h_{i1}h_{j1},h_{i2}h_{j1}+h_{i1}h_{j2},h_{i3}h_{j1}+h_{i1}h_{j3},h_{i2}h_{j2},h_{i3}h_{j2}+h_{i2}h_{j3},h_{i3}h_{j3})
vij=(hi1hj1,hi2hj1+hi1hj2,hi3hj1+hi1hj3,hi2hj2,hi3hj2+hi2hj3,hi3hj3)
从而
v
12
T
b
=
0
v_{12}^Tb=0
v12Tb=0
(
v
11
T
−
v
22
T
)
b
=
0
(v_{11}^T-v_{22}^T)b=0
(v11T−v22T)b=0
b有六个变量，一个H提供两个等式，需要至少3个H才能计算，更多的H可以使用最小二乘，最终得到内参矩阵，此时
[
R
1
,
R
2
,
T
]
=
A
−
1
H
[R_1,R_2,T]=A^{-1}H
[R1,R2,T]=A−1H,求出
R
1
,
R
2
R_1,R_2
R1,R2,最后利用正交性求出
R
3
=
R
1
×
R
2
R_3=R_1\times R_2
R3=R1×R2
以上完成理论部分推导，但是没有考虑到畸变。考虑畸变的话可以在不考虑畸变的基础上不断重投影，并用畸变的函数拟合，不停迭代，得到相对较好的结果。
以下直接调opencv库进行标定，并且进行去畸变效果查看
#include <iostream>
#include <opencv2/opencv.hpp>
#include "cv_source.h"
#include "json\json.h"
#include <fstream>
#include <iomanip>
#include <sstream>
using namespace cv;
using namespace std;
#define use_chess 1
#if use_chess
int BOARDSIZE[2]{ 7,7 };///行内点数，列内点数
#else
int BOARDSIZE[2] { 4, 11 };///行内点数，列内点数
#endif
string conver2string(int d)
{
ostringstream os;
os << d;
return os.str();
}
void enlarge_contrast(Mat& gray)
{
Mat tmp = gray.clone();
Mat res = Mat::zeros(tmp.rows, tmp.cols, CV_8UC1);
for(int i = 0; i < res.rows; i += 80)
{
for (int j = 0; j < res.cols; j += 80)
{
int w = 80; int h = 80;
if (i + h > res.rows) h = res.rows - i;
if (j + w > res.cols) w = res.cols - j;
int max_value=0;
int min_value=255;
for (int i1 = 0; i1 < h; i1++)
{
for (int j1 = 0; j1 < w; j1++)
{
int cur_value = tmp.at<uchar>(i + i1, j + j1);
if (cur_value > max_value) max_value = cur_value;
if (cur_value < min_value) min_value = cur_value;
}
}
for (int i1 = 0; i1 < h; i1++)
{
for (int j1 = 0; j1 < w; j1++)
{
int cur_value = tmp.at<uchar>(i + i1, j + j1);
res.at<uchar>(i + i1, j + j1) = (cur_value - min_value) * 255 / (max_value - min_value+1);
}
}
}
}
for (int i1 = 0; i1 < res.rows; i1++)
{
for (int j1 = 0; j1 < res.cols; j1++)
{
int cur_value = res.at<uchar>(i1, j1);
if (cur_value > 30)
res.at<uchar>(i1, j1) = 255;
else
res.at<uchar>(i1, j1) = 0;
}
}
Mat res2=tmp.clone();
for (int i = 0; i < res2.rows; i++)
{
for (int j = 0; j < res2.cols; j++)
{
if (res.at<uchar>(i, j) == 0)
res2.at<uchar>(i, j) /= 2;
else
{
int value = res2.at<uchar>(i, j)*5;
if (value > 255)value = 255;
res2.at<uchar>(i, j) = value;
}
}
}
gray = res2.clone();
}
void ir2gray(Mat ir, Mat& gray)
{
Mat img = ir.clone();
Mat ig = Mat::zeros(img.rows, img.cols, CV_8UC1);
int max = 0;
int min = 99999;
for (int i = 0; i < img.rows; i++)
{
for (int j = 0; j < img.cols; j++)
{
if (img.at<unsigned short>(i, j) > max) max = img.at<unsigned short>(i, j);
if (img.at<unsigned short>(i, j) < min) min = img.at<unsigned short>(i, j);
}
}
float len = max - min;
//len /= 5;
//len *= 8;
int a[256];
memset(a, 0, 256 * 4);
for (int i = 0; i < ig.rows; i++)
{
for (int j = 0; j < ig.cols; j++)
{
int value = img.at<unsigned short>(i, j)*255/3000;
if (value >255)
value = 255;
//else
// value = 0;
ig.at<uchar>(i, j) = value;
a[value]++;
}
}
//enlarge_contrast(ig);
gray = ig.clone();
}
const double fx = 422.3544, fy = 424.7621, px = 324.5533, py = 223.5978, k1 = -0.3251398, k2 = 0.176575767, k3 = -0.069910314, p1 = 0.000748966, p2 = -0.00630394806;
//const double fx = 441.7427, fy = 443.2380, px = 317.7791, py = 225.7686, k1 = -0.33622735, k2 = 0.19443974, k3 = -0.06829328, p1 = -0.00259415, p2 = 0.000794625;自己算的内参0323
//const double fx = 452.213, fy = 453.0398, px = 313.959, py = 226.416, k1 = -0.3426456, k2 = 0.1971993, k3 = -0.076259038, p1 = 0.0016048, p2 = -0.001174547;自己算的内参0322
//const double fx = 441.5798, fy = 441.7299, px = 316.3875, py = 226.6624, k1 = -0.29444, k2 = 0.086351, k3 = 0, p1 = 0.00046384, p2 = -0.00110995;提供的内参
void cal_param()
{
vector<vector<Point3f>> objpoints_img;//保存棋盘格上角点的三维坐标
vector<Point3f> obj_world_pts;//三维世界坐标
vector<vector<Point2f>> images_points;//保存所有角点
vector<Point2f> img_corner_points;//保存每张图检测到的角点
Mat image, img_gray;
//转世界坐标系
#if use_chess
for (int i = 0; i < BOARDSIZE[1]; i++)对称圆形或者棋盘角点
{
for (int j = 0; j < BOARDSIZE[0]; j++)
{
obj_world_pts.push_back(Point3f(i, j, 0));
}
}
#else
for (int i = 0; i < 11; i++)非对称的圆形
{
for (int j = 0; j < 4; j++)
{
if (i % 2 == 0)
{
obj_world_pts.push_back(Point3f(i, 2*j, 0));
}
else
{
obj_world_pts.push_back(Point3f(i, 2*j+1, 0));
}
}
}
#endif
SimpleBlobDetector::Params params;
//params.maxArea = 10e4; //斑点的最大面积
params.minThreshold = 10; //***二值化的起始阈值，即公式1的T1//数值越大偏黑的部分被忽略，数值越大稳定越差？也影响求解速度
params.maxThreshold = 200; //最大二值化值
params.thresholdStep = 10; //阈值越小检测越精确，计算速度变慢
params.filterByInertia = true; //斑点圆度的限制变量，默认是不限制
params.filterByColor = true; //斑点颜色的限制变量
params.blobColor = 0; //255表示只提取白色斑点，0表示只提取黑色斑点
params.filterByArea = true; //斑点面积的限制变量
params.minArea = 60; //斑点的最小面积最大取到120
//最小的斑点距离，不同二值图像的斑点间距离小于该值时，被认为是同一个位置的斑点，否则是不同位置上的斑点
params.minDistBetweenBlobs = 15;//最大22左右,15比较合适
Ptr<FeatureDetector> blobDetector = SimpleBlobDetector::create(params);
//#endif
int count = 0;
int num = 110;
vector<Mat> vm;
string path_r = "C:\\Users\\user\\Desktop\\pic\\angle\\";//-40,-32,...40，步长8，共11组
string path_t = "C:\\Users\\user\\Desktop\\pic\\T\\";///[0,1,2,3,-1,-2,-3]x[0,1,2,3,-1,-2,-3]
num = 390;
for (int i =0; i <num; i++)
//for(int i=0; i< vm.size(); i++)
{
string image_path = "C:\\Users\\user\\Desktop\\7\\IR\\png\\"+to_string(i*6)+".png";
image = imread(image_path,CV_16UC1);
if (image.empty()) continue;
cout << i << endl;
if (i > 30) break;
//cvtColor(image, img_gray, COLOR_BGR2GRAY);
//image = vm[i].clone();
Mat ig = Mat::zeros(image.rows, image.cols, CV_8UC1);
#if 1
ir2gray(image, ig);
imshow("ig", ig);
waitKey(1);
#else
ig = image.clone();
#endif
img_gray = ig.clone();
//检测角点
#if use_chess
//bool found_success = findChessboardCorners(img_gray, Size(BOARDSIZE[0], BOARDSIZE[1]),img_corner_points, CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE + CALIB_CB_FAST_CHECK);
bool found_success = findCirclesGrid(img_gray, Size(BOARDSIZE[0], BOARDSIZE[1]), img_corner_points, CALIB_CB_SYMMETRIC_GRID);
#else
bool found_success = findCirclesGrid(img_gray, Size(BOARDSIZE[0], BOARDSIZE[1]), img_corner_points, CALIB_CB_ASYMMETRIC_GRID);///, blobDetector 加不加影响不大
#endif
//显示角点
if (found_success)
{
#if use_chess
//迭代终止条件
TermCriteria criteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.001);
//进一步提取亚像素角点
//cornerSubPix(img_gray, img_corner_points, Size(11, 11), Size(-1, -1), criteria);
#endif
//绘制角点
drawChessboardCorners(ig, Size(BOARDSIZE[0], BOARDSIZE[1]), img_corner_points, found_success);
objpoints_img.push_back(obj_world_pts);
images_points.push_back(img_corner_points);
count++;
if (0)//count > 30)
{
cout << "pic_num:" << i << endl;
break;
}
//std::cout << count << endl;
cv::imshow("0", ig);
cv::waitKey(200);
}
}
if (count == 0)
return;
/*
计算内参和畸变系数等
*/
double dis;
Mat cameraMatrix, distCoeffs;// , R, T;//内参矩阵，畸变系数，旋转量，偏移量
vector<Mat> R, T;
dis = cv::calibrateCamera(objpoints_img, images_points, img_gray.size(), cameraMatrix, distCoeffs, R, T);
std::cout << "dis=" << dis << endl;
std::cout << "cameraMatrix:" << endl;
std::cout << cameraMatrix << endl;
std::cout << "*****************************" << endl;
std::cout << "distCoeffs:" << endl;
std::cout << distCoeffs << endl;
std::cout << "*****************************" << endl;
double total_err = 0;
double err = 0;
double totalErr = 0;
double totalPoints = 0;
vector<Point2f> image_points_pro;
for (int i = 0; i < count; i++)
{
projectPoints(objpoints_img[i], R[i], T[i], cameraMatrix, distCoeffs, image_points_pro); //通过得到的摄像机内外参数，对角点的空间三维坐标进行重新投影计算
err = norm(Mat(images_points[i]), Mat(image_points_pro), NORM_L2);
totalErr += err * err;
totalPoints += objpoints_img[i].size();
err /= objpoints_img[i].size();
//cout << "第" << i + 1 << "幅的平均误差：" << err << "像素" << endl;
}
cout << "total_err:" << totalErr << endl;
}
void cal_param1()
{
vector<vector<Point3f>> objpoints_img;//保存棋盘格上角点的三维坐标
vector<Point3f> obj_world_pts;//三维世界坐标
vector<vector<Point2f>> images_points;//保存所有角点
vector<Point2f> img_corner_points;//保存每张图检测到的角点
Mat image, img_gray;
//转世界坐标系
#if use_chess
for (int i = 0; i < BOARDSIZE[1]; i++)对称圆形或者棋盘角点
{
for (int j = 0; j < BOARDSIZE[0]; j++)
{
obj_world_pts.push_back(Point3f(i, j, 0));
}
}
#else
for (int i = 0; i < 11; i++)非对称的圆形
{
for (int j = 0; j < 4; j++)
{
if (i % 2 == 0)
{
obj_world_pts.push_back(Point3f(i, 2 * j, 0));
}
else
{
obj_world_pts.push_back(Point3f(i, 2 * j + 1, 0));
}
}
}
#endif
int count = 0;
int num = 110;
std::string path = "H:\\project\\calibration\\fun_test\\data";
std::string loc[17] = { "-16","-8","0","8", "16", "u1", "u2", "u3", "d1", "d2", "d3", "r1", "r2", "r3", "l1", "l2", "l3" };
for (int i = 0; i < 5; i++)
{
std::vector<std::vector<cv::Point2f>> images_points;//保存所有角点
std::vector<cv::Point2f> img_corner_points;//保存每张图检测到的角点
std::vector<std::vector<cv::Point3f>> objpoints_img;//保存棋盘格上角点的三维坐标
cv::Mat ig;
int cnt = 0;
for (int j = 0; j < 17; j++)
{
std::string path_now = path + "\\" + loc[j] + "\\" + conver2string(i) + ".png";
cv::Mat src = cv::imread(path_now, CV_16UC1);
if (src.empty())
{
//std::cout << log_folder<<"\npath err1\n";
std::string info = "path:" + path_now + ", not found!\n";
//savelog(info, log_folder);
return ;
}
ig = cv::Mat::zeros(src.rows, src.cols, CV_8UC1);
ir2gray(src, ig);
cv::imshow("ig", ig);
bool found_success = findCirclesGrid(ig, Size(BOARDSIZE[0], BOARDSIZE[1]), img_corner_points, cv::CALIB_CB_ASYMMETRIC_GRID);
if (found_success)
{
drawChessboardCorners(ig, Size(BOARDSIZE[0], BOARDSIZE[1]), img_corner_points, found_success);
objpoints_img.push_back(obj_world_pts);
images_points.push_back(img_corner_points);
cnt++;
}
cv::imshow("2", ig);
cv::waitKey(1);
}
if (cnt == 0) continue;
double dis;
cv::Mat cameraMatrix, distCoeffs;// , R, T;//内参矩阵，畸变系数，旋转量，偏移量
std::vector<cv::Mat> R, T;
dis = cv::calibrateCamera(objpoints_img, images_points, ig.size(), cameraMatrix, distCoeffs, R, T);
std::cout << cameraMatrix << std::endl;
std::cout << distCoeffs << std::endl;
}
}
void correction(Mat& src)
{
cv::Mat oridepth = src.clone();
for (int x = 0; x < src.rows; x++)
{
for (int y = 0; y < src.cols; y++)
{
double x0 = 0, y0 = 0;
double x1, x2, y1, y2;
x1 = (y - px) / fx;
y1 = (x - py) / fy;///像素坐标转图像坐标（校正前）
double r2;
r2 = pow(x1, 2) + pow(y1, 2);
x2 = x1 * (1 + k1 * r2 + k2 * pow(r2, 2) + k3 * pow(r2, 3)) + 2 * p1 * y1 + p2 * (r2 + 2 * x1 * x1);
y2 = y1 * (1 + k1 * r2 + k2 * pow(r2, 2) + k3 * pow(r2, 3)) + 2 * p2 * x1 * 1 + p1 * (r2 + 2 * y1 * y1) ;矫正前的图像坐标转矫正后的图像坐标
y0 = fx * x2 + px;
x0 = fy * y2 + py;///校正后的图像坐标转像素坐标
if (x0 >= 0 && y >= 0 && x0 < src.rows && y0 < src.cols)
{
src.at<uchar>(x, y) = oridepth.at<uchar>(x0, y0);
}
else
{
src.at<uchar>(x, y) = 0;
}
}
}
}
Mat mapx, mapy;
int flag = 0;
void get_mapxy()
{
Mat cameraMatrix, distCoeffs;
cameraMatrix = Mat::zeros(3, 3, CV_32FC1);
distCoeffs = Mat::zeros(1, 5, CV_32FC1);
cameraMatrix.at<float>(0, 0) = fx;
cameraMatrix.at<float>(1, 1) = fy;
cameraMatrix.at<float>(0, 2) = px;
cameraMatrix.at<float>(1, 2) = py;
cameraMatrix.at<float>(2, 2) = 1;
distCoeffs.at<float>(0, 0) = k1;
distCoeffs.at<float>(0, 1) = k2;
distCoeffs.at<float>(0, 2) = p1;
distCoeffs.at<float>(0, 3) = p2;
distCoeffs.at<float>(0, 4) = k3;
Mat R = Mat::eye(3, 3, CV_32F);
initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, Size(640, 480), CV_32FC1, mapx, mapy);
}
void correction_opencv(Mat& src)
{
if (flag == 0)
{
flag = 1;
get_mapxy();
}
//Mat dst;
remap(src, src, mapx, mapy, CV_INTER_LINEAR);
//cv::imshow("1", d);
//cv::waitKey(1);
}
void distortion_correction()
{
for (int i = 0; i < 1000; i++)
{
string path = "C:\\Users\\user\\Desktop\\ir\\IR\\png\\" + to_string(i) + ".png";
string path2 = "C:\\Users\\user\\Desktop\\ir\\IR\\png2\\" + to_string(i) + ".png";
Mat dep = cv::imread(path, CV_16UC1);
if (dep.empty()) continue;
Mat gray;
//dep.convertTo(gray, 255.0 / 1000.0);
ir2gray(dep, gray);
Mat new_gray = gray.clone();
//correction_opencv(new_gray);
correction(new_gray);
Mat new_gray_cv = gray.clone();
correction_opencv(new_gray_cv);
cv::imshow("new_gray", new_gray);
cv::imshow("new_graycv", new_gray_cv);
cv::imwrite(path2, new_gray_cv);
cv::imshow("gray", gray);
cv::waitKey(100);
}
}
/**
* 读图转成灰度图后展示
**/
void readImg()
{
int num = 6000;
for (int i = 0; i < num; i++)
{
string path = "C:\\Users\\user\\Desktop\\realsense\\rgb\\" + to_string(i) + ".png";
string path2 = "C:\\Users\\user\\Desktop\\realsense\\depth\\" + to_string(i) + ".png";
Mat img = imread(path);
Mat dep = imread(path2, CV_16UC1);
Mat gray;
dep.convertTo(gray, 255.0 / 4000);
imshow("img", img);
imshow("gray", gray);
waitKey(1);
}
}
#if 0画对称的圆形标定板
#define WINDOW_WIDTH 2100 //定义窗口大小的宏
#define WINDOW_LENTH 2900
void drawFilledCircle(cv::Mat img, cv::Point center) {
int thickness = -1;
int lineType = 8;
cv::circle(img,
center,
80,//WINDOW_WIDTH / 24,
cv::Scalar(0, 0, 0),
thickness,
lineType);
}
void drawPic()
{
cv::Mat atomImage(WINDOW_WIDTH, WINDOW_LENTH, CV_8UC3, Scalar(255, 255, 255));
for (int i = 200 + WINDOW_WIDTH / 12; i < WINDOW_LENTH-350; i = i + 350 )
{
for (int j = 180 + WINDOW_WIDTH / 12; j < WINDOW_WIDTH-250; j = j + 350)
{
drawFilledCircle(atomImage, cv::Point(i, j));
}
}
cv::imwrite("C:\\Users\\user\\Desktop\\相机标定\\1.png", atomImage);///存图地址
cv::imshow("1", atomImage);
}
#endif
int main()
{
//test_time();
//setParam();
//readJson();
//saveJson();
//drawPic();
cal_param();
//distortion_correction();
return 0;
}
6714
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
