《2D-3D 定位算法》笔记
中英对照：
世界坐标系或实体坐标系(3D)：object coordinate system 。
摄像机坐标系(3D): camera coordinate system 。
图像坐标系(2D): image coordinate system ，在摄像机坐标系下取x 和y 坐标即为图像坐标系。
2D-3D 点对：2D-3D correspondences ，根据投影变换将3D 点投影为2D 点。
平移变换：translation projection 旋转变换：rotation projection 比例变换：scale projection
透视投影变换：perspective projection 正交投影变换：orthographic projection
2D-3D 定位算法：根据 已给出的若干对 3D 点p i （在世界坐标系或实体坐标系下）和 相对应的 2D 点p i '（在图像坐标系下或在摄像机坐标系下取x 和y 坐标），求出之间的投影变换矩阵（旋转变换和平移变换）。
文献1：
《A Comparison of 2D-3D Pose Estimation Methods 》 文献2：
《A Comparison of Iterative 2D-3D Pose Estimation Methods for Real-Time Applications 》 文献3：
《计算机视觉》-马颂德
一、CamPoseCalib(CPC)
1、基本思想：根据非线性最小二乘法，最小化重投影误差求出投影参数
),,,,,(γβαθθθθθθθz y x =。
2、算法过程：
（1）已给出若干点对)'~
,(i i p p ，其中i p 是实体坐标系下的3D 点，'~i p 我理解为事
先给出的图像坐标系下的2D 点，应该是给出的测量值 。
（2）将i p 先经过旋转变换 i z y x p R R R ⋅⋅⋅)()()(γβαθθθ 和平移变换 T
z y x ),,(θθθ ，得
到
摄
像
机坐标系下的点
i z y x T
z y x i p R R R p m ⋅⋅⋅+=)()()(),,(),(γβαθθθθθθθ 。
（3）再将像机坐标系下的点),(i p m θ进行透视投影变换得到图像坐标系下的2D 点：
⎪⎪⎪⎪
⎭
⎫
⎝⎛+⋅+⋅=y i y i z y x i x i z
x i c p m p m s c p m p m s p m ),(),(),(),(),('θθθθθ，比例因子为),(y x s s s =，跟
摄像机的焦距有关，摄像机坐标系的Z 轴与图像平面的交点 跟 图像坐标原点 的距离为
),(y x c c ；其中一般f s s y x ==是焦距，)0,0(),(=y x c c ，都是已知的，并且事
先给出的。
（4）重投影误差2
)
(θi r d =是),('θi p m 跟
'~i p 的二维欧拉距离，
'~),(')(i i i p p m r -=θθ ，2
)(θi r 是关于),,,,,(γβαθθθθθθθz y x =的函数 。
（5）根据已给出m 个点对)'~
,(i i p p ，用非线性最小二乘法求出未知系数
),,,,,(γβαθθθθθθθz y x =，∑==m
i i
r 1
2
))
((min
arg ˆθθθ。
3、求解过程：
（1）给出摄像机的初始位置：焦距f ，即),(y x s s s =，图像坐标中心，即),(y x c c ，
图像范围，即合理的2D 坐标值范围。
（2）共有6个未知数，至少需要3个)'~
,(i i p p ，即m>=3 ； （3）算法执行停止条件为：限制迭代次数，设置每次重投影误差的变动值大小阈值。
4、我目前的进展：
（1）现在在看非线性最小二乘法，了解怎么最终获得解。
（2）上面有下划线的地方，不知道我理解的对不对？
二、POSIT
1、基本思想：算法分两部分
（1）带有比例系数的正交投影变换SOP ，根据线性方程组求出旋转矩阵和平移向量； （2）由得出的旋转矩阵和平移向量系数，更新比例系数（scale factor ），再由比例系数更新原有的点，进行迭代。
2、算法过程：
（1）假设旋转矩阵 ⎥⎥⎥
⎦⎤⎢⎢⎢
⎣⎡=3332
31
232221
131211R R R R R R R R R R =⎥⎥⎥⎦⎤
⎢⎢⎢⎣⎡T
T T R R R 321和平移向量⎥⎥⎥⎦
⎤
⎢⎢⎢⎣⎡=z y x T T T T ，f 是焦距；在
透视投影变换中 i i
i X Z f x ⋅=
，i i
i Y Z f y ⋅=
，而在SOP 中i i X Z f x ⋅=
，
i i Y Z f y ⋅=
，其中比例因子是 0
Z f s = ；
（2）作基本的透视投影变换，将3D 点 T
z y x a a a a ),,(=透视投影到图像平面上得到齐
次坐标T w wy wx m ),,(=，变换过程为⎥⎥⎥⎥
⎦
⎤⎢⎢⎢⎢⎣⎡⋅⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡1321z y x z y x T
T T
a a a T fT fT R fR fR w wy wx ，（有点不明白）因为m 是齐次坐标，所以等式右边除以z T ，不会受影响，则得到
⎥⎥⎥
⎥
⎦
⎤
⎢⎢⎢⎢⎣⎡⋅⎥⎥⎥⎦⎤⎢⎢⎢⎣
⎡=⎥⎥⎥
⎦⎤
⎢⎢⎢⎣⎡11
321z y
x y x T R T T
a a a sT sT sR sR w
wy wx
z T
，其中
z
T f s =
，即得到
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎣⎡⋅⎥⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡12
1
z y x y
x T T
a a a sT sT sR sR wy wx ，其中13+∙=
z
T a R w ；
（3）现在变换过程为][]][
1[21wy wx sT sT sR sR Z
Y
X y
x
=，即为方程组
wy
sT ZsR
YsR
XsR
wx sT ZsR YsR XsR y x =+++=+++23
22
21
131211{
，w 初始值为1；
（4）令T
x sT sR sR R s K )(13
1211
1= ，T
y sT sR sR sR K )
(2322212=，
⎥⎥
⎥⎥⎦⎤⎢⎢⎢
⎢⎣⎡=1 (11111)
000n
n
n
Z Y X Z Y X Z Y X A ，A 为(n+1)⨯4矩阵，⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=n x x x b .1
01，⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎣⎡=n y y y b .102；然后初
始方程组变成 2
2
11{
b AK
b AK ==，应用最小二乘法，得到解2
1
2111)
()({
b A A A K b A A A K T
T
T
T --==；
（5）至少有4个不共面的2D-3D 点对，求出K1，K2后，将其除以已知的定值s ，可得到R1，R2，Tx ，Ty ，然后得到R3=R1⨯R2，并且将R1，R2，R3归一化为单位向量； （6）然后更新13+∙=
z
T a R w ，因为对不同的2D-3D 点对，z
T f s
=
是定值，f 是焦
距，是已知的定值参数，z T 也是已知的定值参数，可以看做是所有的3D 点Z 坐标的平均值，（这样可以吗？）；对不同的3D 点，a 不同，所以w 也就不同，这样将原来的2D 点变为T
wy wx ),(；
（7）再重步骤（2）开始，由原有的3D 点和更新后的2D 点，用最小二乘法解方程组，得到新的K1，K2；再更新w ，更新2D 点坐标； 3、求解过程：
（1）给出摄像机的初始位置：焦距f ，图像坐标中心，即),(y
x c c ，图像范围，即合
理的2D 坐标值范围。
（2）共有8个未知数，至少需要4个2D-3D 点对； （3）第一个2D-3D 点对必须是（0,0）-（0,0,0）；
（3）算法执行停止条件为：限制迭代次数，设置每次2D 点的变动值大小（精确度）阈值。
4、我目前的进展：
（1）我不明白z T 一开始是不是给出的，还是计算出来的？如果看做是所有的3D 点Z 坐标的平均值或者最小值或中值，这样可以吗？
（2）上面有下划线的地方，不知道我理解的对不对？
三、Direct Linear Transform （DLT ）
这种方法，马颂德的《计算机视觉》 4.1节 线性模型摄像机定标 说的比较详细， 文献1中的DLT 解法跟这个类似，这个应该就是DLT 算法，所以我就没有仔细写了。
四、PosIt for coplanar points
这个方法主要是处理对于PosIt 中不同的3D 点正交投影到图像平面上同一个2D 点这种情况，具体的解法看的还不是太明白。