MultiCol-SLAM鱼眼相机的相机模型

106 阅读 0 评论 70 点赞

我是靠谱客的博主清爽咖啡，这篇文章主要介绍MultiCol-SLAM鱼眼相机的相机模型，现在分享给大家，希望可以做个参考。

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
%YAML:1.0

# Camera Parameters. Adjust them!
# Camera calibration parameters camera back
Camera.Iw: 754
Camera.Ih: 480

# hyperparameters
Camera.nrpol: 5
Camera.nrinvpol: 12

# forward polynomial f(rho)
Camera.a0: -209.200757992065
Camera.a1: 0.0 
Camera.a2: 0.00213741670953883
Camera.a3: -4.2203617319086e-06
Camera.a4: 1.77146086919594e-08

# backward polynomial rho(theta)
Camera.pol0: 293.667187375663
Camera.pol1: 149.982043337335
Camera.pol2: -10.448650568161
Camera.pol3: 28.2295300683376
Camera.pol4: 7.13365723186292
Camera.pol5: 0.056303218962532
Camera.pol6: 10.4144677485333
Camera.pol7: 0.166354960773665
Camera.pol8: -5.86858687381081
Camera.pol9: 1.18165998645705
Camera.pol10: 3.1108311354746
Camera.pol11: 0.810799620714366

# affine matrix
Camera.c: 0.999626131079017
Camera.d: -0.0034775192597376
Camera.e: 0.00385134991673147

# principal point
Camera.u0: 392.219508388648
Camera.v0: 243.494438476351

# create a mirror mask for fisheye cameras
Camera.mirrorMask: 1

以上是multicol-SLAM的相机内参，forward polynomial f(rho) 和backward polynomial rho(theta) 分别代表的物理意义是什么？以及仿射变换矩阵affine matrix在这里的作用是什么？

对于同样具有鱼眼相机的openvslam系统，其鱼眼相机的配置文件：

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
Camera.name: "TUM VI monocular"
Camera.setup: "monocular"
Camera.model: "fisheye"

Camera.fx: 190.97847715128717
Camera.fy: 190.9733070521226
Camera.cx: 254.93170605935475
Camera.cy: 256.8974428996504

Camera.k1: 0.0034823894022493434
Camera.k2: 0.0007150348452162257
Camera.k3: -0.0020532361418706202
Camera.k4: 0.00020293673591811182

Camera.fps: 20
Camera.cols: 512
Camera.rows: 512

Camera.color_order: "Gray"

参数就非常的简洁，且很明了，k1-k4代表相机的径向畸变参数（无切向畸变），那么对于multiCol-SLAM，他的参数我就不那么懂了，尤其是

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
Camera.a0: -209.200757992065
Camera.a1: 0.0 
Camera.a2: 0.00213741670953883
Camera.a3: -4.2203617319086e-06
Camera.a4: 1.77146086919594e-08

Camera.pol0: 293.667187375663
Camera.pol1: 149.982043337335
Camera.pol2: -10.448650568161
Camera.pol3: 28.2295300683376
Camera.pol4: 7.13365723186292
Camera.pol5: 0.056303218962532
Camera.pol6: 10.4144677485333
Camera.pol7: 0.166354960773665
Camera.pol8: -5.86858687381081
Camera.pol9: 1.18165998645705
Camera.pol10: 3.1108311354746
Camera.pol11: 0.810799620714366

分别代表的是什么？希望得到大神的解答，不胜感激。

自己的理解

经过很久的论文阅读，对multiCol-SLAM的鱼眼相机模型由了一定的理解,记录下来以备查验，也希望有人能指正和指教。

标定工具（google：Scaramuzza_OCamCalib_v3.0_win）
两篇重要的论文（https://download.csdn.net/download/ouyangandy/12545885）
标定的各项参数的用处，不会使用md编辑公式，在word中编辑后插入这里：
对应上述公式的代码

复制代码

void cam2world(double point3D[3], double point2D[2], struct ocam_model* myocam_model)
{
	double *pol = myocam_model->pol;
	double xc = myocam_model->xc;
	double yc = myocam_model->yc;
	double c = myocam_model->c;
	double d = myocam_model->d;
	double e = myocam_model->e;
	int length_pol = myocam_model->length_invpol;
	double invdet = 1 / (c - d*e);// 1/det(A), A = [c,d;e,1] 行列式的值

/*
	[m.x     [c d  *  [ut	 + [u0	
	 m.y]  =  e 1]     vt]	    v0]	//其中ut,vt为sensor plane，m.x m.y为image plane
	*/
	double xp = invdet*((point2D[0] - xc) - d*(point2D[1] - yc));	//xp和yp 为sensor plane coordinate
	double yp = invdet*(-e*(point2D[0] - xc) + c*(point2D[1] - yc));//

double r = sqrt(xp*xp + yp*yp);  //ρ= √u^2+v^2
	double zp = pol[0];
	double r_i = 1;
	//霍纳法则 : f(ρ)=a0 + a1*ρ + a2*ρ^2 + a3*ρ^3 + a4*ρ^4
	for (int i = 1; i < length_pol; i++)
	{
		r_i *= r;
		zp += r_i*pol[i];
	}
	double invnorm = 1 / sqrt(xp*xp + yp*yp + zp*zp);
	point3D[0] = invnorm*xp;
	point3D[1] = invnorm*yp;
	point3D[2] = invnorm*zp;
};

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
void cam2world(double point3D[3], double point2D[2], struct ocam_model* myocam_model)
{
	double *pol = myocam_model->pol;
	double xc = myocam_model->xc;
	double yc = myocam_model->yc;
	double c = myocam_model->c;
	double d = myocam_model->d;
	double e = myocam_model->e;
	int length_pol = myocam_model->length_invpol;
	double invdet = 1 / (c - d*e);// 1/det(A), A = [c,d;e,1] 行列式的值

	/*
	[m.x     [c d  *  [ut	 + [u0	
	 m.y]  =  e 1]     vt]	    v0]	//其中ut,vt为sensor plane，m.x m.y为image plane
	*/
	double xp = invdet*((point2D[0] - xc) - d*(point2D[1] - yc));	//xp和yp 为sensor plane coordinate
	double yp = invdet*(-e*(point2D[0] - xc) + c*(point2D[1] - yc));//

	double r = sqrt(xp*xp + yp*yp);  //ρ= √u^2+v^2
	double zp = pol[0];
	double r_i = 1;
	//霍纳法则 : f(ρ)=a0 + a1*ρ + a2*ρ^2 + a3*ρ^3 + a4*ρ^4
	for (int i = 1; i < length_pol; i++)
	{
		r_i *= r;
		zp += r_i*pol[i];
	}
	double invnorm = 1 / sqrt(xp*xp + yp*yp + zp*zp);
	point3D[0] = invnorm*xp;
	point3D[1] = invnorm*yp;
	point3D[2] = invnorm*zp;
};

也就是从point2D到point3D的mapping过程，也就是说，从鱼眼相机的image plane 转换到sensor plane，然后再转换到Normalized sphere，因此上面参数中的a0-a4也就是多项式（4）的参数。

反过来，对于一个三维点，对应到image plane上的点，也就有：

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
void world2cam(double point2D[2], double point3D[3], struct ocam_model* myocam_model)
{
	double *invpol = myocam_model->inv_pol;
	double xc = myocam_model->xc;
	double yc = myocam_model->yc;

	double c = myocam_model->c;
	double d = myocam_model->d;
	double e = myocam_model->e;
	int width = myocam_model->width;
	int height = myocam_model->height;
	int length_invpol = myocam_model->length_invpol;

	double norm = sqrt(point3D[0] * point3D[0] + point3D[1] * point3D[1]); // x^2+y^2
	double theta = atan(point3D[2] / norm); //z/(x^2+y^2)
	double t, t_i;
	double rho, x, y;
	double invnorm;
	int i;

	if (norm != 0)
	{
		invnorm = 1 / norm;
		t = theta;
		rho = invpol[0];
		t_i = 1;

		for (i = 1; i < length_invpol; i++)
		{
			t_i *= t;
			rho += t_i*invpol[i];
		}
		x = point3D[0] * invnorm*rho;
		y = point3D[1] * invnorm*rho;

		point2D[0] = x*c + y*d + xc;
		point2D[1] = x*e + y + yc;
	}
	else
	{
		point2D[0] = xc;
		point2D[1] = yc;
	}

};