【逆透视变换+刚体变换求解二维图像坐标对应三维点云C++代码】_透视变换 点云

世界坐标系坐标（下标为w）左乘RT矩阵，变换到相机坐标系(下标为c)下

[ R T 0 1 ] [ X w Y w 0 1 ] = [ X c Y c Z c 1 ] ( 1 )

$$\begin{bmatrix}R &T \\ 0&1 \end{bmatrix}$$ $$\begin{bmatrix}X_{w}\\ Y_{w}\\0\\1 \end{bmatrix}$$ = $$\begin{bmatrix}X_{c}\\ Y_{c}\\Z_{c}\\1 \end{bmatrix}$$ (1) [R0​T1​] ​Xw​Yw​01​ ​= ​Xc​Yc​Zc​1​ ​(1)

[ r 11 r 12 r 13 t 1 r 21 r 22 r 23 t 2 r 31 r 32 r 33 t 3 0 0 0 1 ] [ X w Y w 0 1 ] = [ X c Y c Z c 1 ] ( 2 )

$$\begin{bmatrix}r_{11} &r_{12} &r_{13}&t_{1} \\ r_{21} &r_{22} &r_{23}&t_{2}\\r_{31} &r_{32} &r_{33}&t_{3}\\0 &0 &0&1\end{bmatrix}$$ $$\begin{bmatrix}X_{w}\\ Y_{w}\\0\\1 \end{bmatrix}$$ = $$\begin{bmatrix}X_{c}\\ Y_{c}\\Z_{c}\\1 \end{bmatrix}$$ (2) ​r11​r21​r31​0​r12​r22​r32​0​r13​r23​r33​0​t1​t2​t3​1​ ​ ​Xw​Yw​01​ ​= ​Xc​Yc​Zc​1​ ​(2)

上式左右两边可以写为：

$$\begin{gathered} r_{11}{X}_w+r_{12}{Y}_w+t_1=\lambda \frac{u-c_x}{f_x} \\ {r}_{21}{X}_w+r_{22}{Y}_w+t_2=\lambda \frac{v-c_y}{f_y}(3) \\ {r}_{31}{X}_w+r_{32}{Y}_w+t_3=\lambda \end{gathered}$$

其中，等式右边等价为像素坐标uv左乘相机内参的结果，Lambda为scaling factor.

[ X c Y c Z c 1 ] = λ M − 1 [ u v 1 ] = [ λ u − c x f x λ v − c y f y λ ] ( 4 )

$$\begin{bmatrix}X_{c}\\ Y_{c}\\Z_{c}\\1 \end{bmatrix}$$ =\lambda M^{-1} $$\begin{bmatrix}u \\ v\\1\end{bmatrix}$$ = $$\begin{bmatrix}\lambda\frac{u-c_{x}}{f_{x}} \\\lambda\frac{v-c_{y}}{f_{y}}\\\lambda\end{bmatrix}$$ (4) ​Xc​Yc​Zc​1​ ​=λM−1 ​uv1​ ​= ​λfx​u−cx​​λfy​v−cy​​λ​ ​(4)

式(3)可以写为：

$$\begin{gathered} r_{11}{X}_w+r_{12}{Y}_w-\lambda \frac{u-c_x}{f_x}=-t_1 \\ {r}_{21}{X}_w+r_{22}{Y}_w-\lambda \frac{v-c_y}{f_y}=-t_2 \\ {r}_{31}{X}_w+r_{32}{Y}_w-\lambda =-t_3 \end{gathered}$$

于是，上式等价为AX=b, 解出X后，取X(3)即为$\lambda$, 带入式(4)，即可求得$X_c,Y_c,Z_c$

Eigen::Vector3d featureMap2d_3d(cv::Point2d p, cv::Mat camK,  Eigen::Matrix3d R, Eigen::Vector3d t)
{
	double fx = camK.at<double>(0, 0);
	double fy = camK.at<double>(1, 1);
	double cx = camK.at<double>(0, 2);
	double cy = camK.at<double>(1, 2);

	//归一化平面z=1
	Eigen::Vector3d P_normal((p.x - cx) / fx,
		(p.y - cy) / fy,
		1.);


	Eigen::Matrix3d A = R;
	A.block(0, 2, 3, 1) = -P_normal;
	Eigen::Vector3d b = -t;

	Eigen::Vector3d x = A.colPivHouseholderQr().solve(b);

	//x.z()为lambda系数
	Eigen::Vector3d Pc = x.z() * P_normal;

	return Pc;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24