MSCKF的理解(3.4之(17)-(24))以及附录

从世界坐标系看到的点的坐标（X,Y,Z）和相机看到的点（u,v）之间的关系

[ u v 1 ] = 1 Z [ f x , 0 , c x 0 , f y , c y 0 , 0 , 1 ] [ X Y Z ] = 1 Z K P \begin{bmatrix}u\\v\\1\end{bmatrix} =\frac{1}{Z}\begin{bmatrix}f_x,0,c_x\\0,f_y,c_y\\0,0,1\end{bmatrix}\begin{bmatrix}X \\Y\\Z\end{bmatrix}=\frac{1}{Z}KP ⎣⎡uv1⎦⎤=Z1⎣⎡fx,0,cx0,fy,cy0,0,1⎦⎤⎣⎡XYZ⎦⎤=Z1KP
其中K是内参矩阵。也就是如下关系：
Z p = K P Zp=KP Zp=KP

一般的，在相机外参R，t的情况下，就变成了
Z p = K ( R P + t ) Zp=K(RP+t) Zp=K(RP+t)

其中相机看到的点 P j c a m P^{cam}_j Pjcam和世界坐标系中的点 P j w P^{w}_j Pjw的关系如下：
P j c a m = w c R P j w + w c P P^{cam}_j = _w^{c}RP^w_j+_w^{c}P Pjcam=wcRPjw+wcP

∂ u j ∂ X w = ∂ ( r 11 X w + r 12 Y w + r 13 Z w + w c X r 31 X w + r 32 Y w + r 33 Z w + w c Z ) ∂ X w = − r 11 ( r 31 X w + r 32 Y w + r 33 Z w + w c Z ) 2 = − r 11 ( Z j c a m ) 2 \frac{\partial u_j}{\partial X^w}=\frac{\partial(\frac{r_{11}X^w+r_{12}Y^w+r_{13}Z^w+_w^{c}X}{r_{31}X^w+r_{32}Y^w+r_{33}Z^w+_w^{c}Z})}{\partial X^w}=-\frac{r_{11}}{(r_{31}X^w+r_{32}Y^w+r_{33}Z^w+_w^{c}Z)^2}=-\frac{r_{11}}{{(Z_j^{cam}})^2} ∂Xw∂uj=∂Xw∂(r31Xw+r32Yw+r33Zw+wcZr11Xw+r12Yw+r13Zw+wcX)=−(r31Xw+r32Yw+r33Zw+wcZ)2r11=−(Zjcam)2r11

∂ u j ∂ Y w = − r 12 ( r 31 X w + r 32 Y w + r 33 Z w + w c Z ) 2 = − r 12 ( Z j c a m ) 2 \frac{\partial u_j}{\partial Y^w}=-\frac{r_{12}}{(r_{31}X^w+r_{32}Y^w+r_{33}Z^w+_w^{c}Z)^2}=-\frac{r_{12}}{{(Z_j^{cam}})^2} ∂Yw∂uj=−(r31Xw+r32Yw+r33Zw+wcZ)2r12=−(Zjcam)2r12
…
所以，图像看到的点相对于其世界坐标中的点的导数是：
J 23 = p j c ∂ X w Y w Z w = − 1 ( Z j c a m ) 2 [ r 11 , r 12 , r 13 r 21 , r 22 , r 23 ] J_{23}=\frac{p_j^c}{\partial X^wY^wZ^w}=-\frac{1}{(Z_j^{cam})^2}\begin{bmatrix}r_{11},r_{12},r_{13}\\r_{21},r_{22},r_{23} \end{bmatrix} J23=∂XwYwZwpjc=−(Zjcam)21[r11,r12,r13r21,r22,r23]

从而有：
Δ p j c = J 23 Δ X Y Z \Delta p_j^c=J_{23}\Delta XYZ Δpjc=J23ΔXYZ
但是这个似乎和文章或者网上的推导不一样。

2D特征点的"微分"

按照文章公式（23）的理解，应该是：
J 23 = ∂ p ∂ X Y Z = [ ∂ u ∂ X Y Z ∂ v ∂ X Y Z ] = [ ∂ ( X Z ) ∂ X Y Z ∂ ( Y Z ) ∂ X Y Z ] = [ 1 Z , 0 , − X Z 2 0 , 1 Z , − Y Z 2 ] = 1 Z [ 1 , 0 , − X Z 0 , 1 , − Y Z ] J_{23}=\frac{\partial p}{\partial XYZ}=\begin{bmatrix}\frac{\partial u}{\partial XYZ}\\\frac{\partial v}{\partial XYZ}\end{bmatrix}=\begin{bmatrix}\frac{\partial (\frac{X}{Z})}{\partial XYZ}\\\frac{\partial (\frac{Y}{Z})}{\partial XYZ}\end{bmatrix}=\begin{bmatrix}\frac{1}{Z},0,-\frac{X}{Z^2}\\0,\frac{1}{Z},-\frac{Y}{Z^2}\end{bmatrix}=\frac{1}{Z}\begin{bmatrix}1,0,-\frac{X}{Z}\\0,1,-\frac{Y}{Z}\end{bmatrix} J23=∂XYZ∂p=[∂XYZ∂u∂XYZ∂v]=[∂XYZ∂(ZX)∂XYZ∂(ZY)]=[Z1,0,−Z2X0,Z1,−Z2Y]=Z1[1,0,−ZX0,1,−ZY]

从而：
p − p ^ = Δ p c = J 23 Δ X Y Z = J 23 Δ P c p-\hat{p}=\Delta p^c=J_{23}\Delta XYZ=J_{23}\Delta P^c p−p^=Δpc=J23ΔXYZ=J23ΔPc
等方程求解的时候， Δ p \Delta p Δp和 J 23 J_{23} J23都是已知量（其中 p p p是图像获取， p ^ \hat{p} p^是估计量，是根据 P P P的估计 P ^ \hat{P} P^来的），求得 Δ P \Delta P ΔP这个未知量。这个未知量的新鲜血液来自于图像的2D特征点坐标 p p p。其实这个小p和大P通过雅可比的转换是相机坐标系下的自说自话。

根据3.3的说法，也即是公式（14），相机的pose是IMU的pose而来。相机和IMU之间的外参是固定的，所以这两者知道一个就知道另一个了。

根据3.4的说法，要先搞定相机的pose，然后再用观测的点的2D坐标来做最小二乘法来确定2D坐标的估计值。所以附录解释了如何确定相机的pose。

公式(19), P c = w c R ( P w − P c a m w ) P^c = _w^cR(P^w-P^w_{cam}) Pc=wcR(Pw−Pcamw), 其中 P c a m w P_{cam}^w Pcamw代表相机的原点在世界坐标系中的位置。换种说法， P w = c w R P c + P c a m w P^w = _c^wRP^c+P^w_{cam} Pw=cwRPc+Pcamw，这个公式和本文开头的公式 P j c a m = w c R P j w + w c P P^{cam}_j = _w^{c}RP^w_j+_w^{c}P Pjcam=wcRPjw+wcP的意义是一样的。但是对于公式(19)两边进行旋转和平移的求导是可以的。

说说公式(21)是怎么来的,首先在相机坐标系的框架内有：
Δ p = J X Δ X + J 23 Δ P c \Delta p^=J_{X}\Delta X_{}+J_{23}\Delta P^c Δp=JXΔX+J23ΔPc
这个公式的来源有两部分，分别是 Δ X \Delta X ΔX和 Δ P c \Delta P^c ΔPc, 这两部分都和 Δ p \Delta p Δp有着直接的关系：
和最后一个相机的状态有关系吗？ Δ p = p − p ^ \Delta p=p-\hat{p} Δp=p−p^
p ^ = p N + R P \hat{p}=p_N+RP p^=pN+RP
其中，
Δ X = [ Δ X I M U , Δ θ 1 c , Δ P 1 w , Δ θ 2 c , Δ P 2 w , . . . , Δ θ N c , Δ P N w ] \Delta{X}=[\Delta X_{IMU}, \Delta \theta^c_1,\Delta P^w_1, \Delta \theta^c_2,\Delta P^w_2,..., \Delta \theta^c_N,\Delta P^w_N] ΔX=[ΔXIMU,Δθ1c,ΔP1w,Δθ2c,ΔP2w,...,ΔθNc,ΔPNw]

Δ X I M U = [ Δ θ I M U , Δ P I M U , Δ v I M U , Δ b a , Δ b g ] \Delta X_{IMU}=[\Delta\theta_{IMU},\Delta P_{IMU},\Delta v_{IMU},\Delta b_a,\Delta b_g] ΔXIMU=[ΔθIMU,ΔPIMU,ΔvIMU,Δba,Δbg]

∂ Δ p ∂ Δ X I M U = [ ∂ Δ p ∂ Δ θ I M U , ∂ Δ p ∂ Δ P I M U , ∂ Δ p ∂ Δ v I M U , 0 , 0 ] = = [ 0 , 0 , 0 , 0 , 0 ] \frac{\partial \Delta p}{\partial \Delta X_{IMU}}=[\frac{\partial\Delta{p}}{\partial \Delta\theta_{IMU}},\frac{\partial\Delta{p}}{\partial \Delta P_{IMU}},\frac{\partial\Delta{p}}{\partial \Delta v_{IMU}},\textbf{0},\textbf{0} ]==[\textbf{0},\textbf{0},\textbf{0},\textbf{0},\textbf{0}] ∂ΔXIMU∂Δp=[∂ΔθIMU∂Δp,∂ΔPIMU∂Δp,∂ΔvIMU∂Δp,0,0]==[0,0,0,0,0]

著名的李代数求导：

∂ Δ p j ∂ Δ θ j = ∂ Δ p j ∂ Δ P j ∂ Δ P j ∂ Δ θ j = J 23 ∂ Δ R ∂ Δ θ = J 23 R p ^ \frac{\partial \Delta p_j}{\partial \Delta \theta_j}=\frac{\partial \Delta p_j}{\partial \Delta P_j}\frac{\partial \Delta P_j}{\partial \Delta \theta_j}=J_{23}\frac{\partial \Delta R}{\partial \Delta\theta}=J_{23}Rp_{} \hat{} ∂Δθj∂Δpj=∂ΔPj∂Δpj∂Δθj∂ΔPj=J23∂Δθ∂ΔR=J23Rp^

∂ Δ p j ∂ Δ P j = J 23 \frac{\partial \Delta p_j}{\partial \Delta P_j}=J_{23} ∂ΔPj∂Δpj=J23

其中,
J 23 Δ P c = J 23 w c R Δ P w = J f Δ P w J_{23}\Delta P^c=J_{23}{_w^c}R\Delta P^w=J_f\Delta P^w J23ΔPc=J23wcRΔPw=JfΔPw

所以：
Δ p = J X Δ X i m u + J f Δ P w \Delta p^=J_{X}\Delta X_{imu}+J_f\Delta P^w Δp=JXΔXimu+JfΔPw

其中
J f = J 23 w c R J_f=J_{23}{_w^c}R Jf=J23wcR

特征点三角化
从最小二乘法的原理开始理解，就是有一堆已知坐标的点，{ x i , y i x_i,y_i xi,yi},现在需要求 y = k x + b y=kx+b y=kx+b的k,b，使得 e = Σ ∣ y i − ( k x i + b ) ∣ 2 e=\Sigma|y_i-(kx_i+b)|^2 e=Σ∣yi−(kxi+b)∣2最小。
∂ e ∂ k = − Σ ( y i − ( k x i + b ) ) x i = 0 \frac{\partial e}{\partial k}=-\Sigma (y_i-(kx_i+b))x_i=0 ∂k∂e=−Σ(yi−(kxi+b))xi=0
∂ e ∂ b = − Σ ( y i − ( k x i + b ) ) = 0 \frac{\partial e}{\partial b}=-\Sigma (y_i-(kx_i+b))=0 ∂b∂e=−Σ(yi−(kxi+b))=0
轻松得出：
k ^ = Σ x i Σ y i − n Σ x i y i n Σ x i 2 − ( Σ x i ) 2 \hat k=\frac{\Sigma x_i\Sigma y_i-n\Sigma x_iy_i}{n\Sigma x_i^2-(\Sigma x_i)^2} k^=nΣxi2−(Σxi)2ΣxiΣyi−nΣxiyi
b ^ = Σ x i Σ x i y i − n Σ x i 2 Σ y i n Σ x i 2 − ( Σ x i ) 2 \hat b=\frac{\Sigma x_i\Sigma x_iy_i-n\Sigma x_i^2\Sigma y_i}{n\Sigma x_i^2-(\Sigma x_i)^2} b^=nΣxi2−(Σxi)2ΣxiΣxiyi−nΣxi2Σyi

现在有一堆已知pose的相机 R C i , t C i R^{C_i},t^{C_i} RCi,tCi,和一个未知的世界点P(X,Y,Z)在每个相机当中的2D观测量 ( u i , v i ) (u_i,v_i) (ui,vi),求世界点的坐标估计值.
世界点在相机系的2D坐标是(忽略相机内参)： ( u i , v i , 1 ) ′ = 1 Z C i ( R C i P + t C i ) (u_i,v_i,1)'=\frac{1}{Z^{C_i}}(R^{C_i}P+t^{C_i}) (ui,vi,1)′=ZCi1(RCiP+tCi)
其中，相机坐标系下的 P c P^c Pc都是世界坐标系的P的元素的线性组合：
X c = r 11 X w + r 12 Y w + r 13 Z w + w c X X^c=r_{11}X^w+r_{12}Y^w+r_{13}Z^w+_w^{c}X Xc=r11Xw+r12Yw+r13Zw+wcX
…
误差函数：

e = Σ [ ( u i − X C i Z C i ) 2 + ( v i − Y C i Z C i ) 2 ] e=\Sigma[(u_i-\frac{X^{C_i}}{Z^{C_i}})^2+(v_i-\frac{Y^{C_i}}{Z^{C_i}})^2] e=Σ[(ui−ZCiXCi)2+(vi−ZCiYCi)2]

∂ e ∂ X = Σ 2 [ ( u i − X C i Z C i ) ( − 1 Z C i ∂ X C i ∂ X + X C i ( Z C i ) 2 ∂ Z C i ∂ X ) + ( v i − Y C i Z C i ) ( − 1 Z C i ∂ Y C i ∂ X + Y C i ( Z C i ) 2 ∂ Z C i ∂ X ) ] = Σ 2 [ ( u i − X C i Z C i ) ( − 1 Z C i r 11 + X C i ( Z C i ) 2 r 13 ) + ( v i − Y C i Z C i ) ( − 1 Z C i r 12 + Y C i ( Z C i ) 2 r 13 ) ] = 0 \frac{\partial e}{\partial X}=\Sigma 2[(u_i-\frac{X^{C_i}}{Z^{C_i}})(-\frac{1}{Z^{C_i}}\frac{\partial X^{C_i}}{\partial X}+\frac{X^{C_i}}{(Z^{C_i})^2}\frac{\partial Z^{C_i}}{\partial X})+(v_i-\frac{Y^{C_i}}{Z^{C_i}})(-\frac{1}{Z^{C_i}}\frac{\partial Y^{C_i}}{\partial X}+\frac{Y^{C_i}}{(Z^{C_i})^2}\frac{\partial Z^{C_i}}{\partial X})]\\=\Sigma 2[(u_i-\frac{X^{C_i}}{Z^{C_i}})(-\frac{1}{Z^{C_i}}r_{11}+\frac{X^{C_i}}{(Z^{C_i})^2}r_{13})+(v_i-\frac{Y^{C_i}}{Z^{C_i}})(-\frac{1}{Z^{C_i}}r_{12}+\frac{Y^{C_i}}{(Z^{C_i})^2}r_{13})]=0 ∂X∂e=Σ2[(ui−ZCiXCi)(−ZCi1∂X∂XCi+(ZCi)2XCi∂X∂ZCi)+(vi−ZCiYCi)(−ZCi1∂X∂YCi+(ZCi)2YCi∂X∂ZCi)]=Σ2[(ui−ZCiXCi)(−ZCi1r11+(ZCi)2XCir13)+(vi−ZCiYCi)(−ZCi1r12+(ZCi)2YCir13)]=0

但是 [ ∂ e ∂ X ∂ e ∂ Y ∂ e ∂ Z ] = 0 \begin{bmatrix}\frac{\partial e}{\partial X}\\\frac{\partial e}{\partial Y}\\\frac{\partial e}{\partial Z}\end{bmatrix}=\textbf{0} ⎣⎡∂X∂e∂Y∂e∂Z∂e⎦⎤=0不构成一个线性方程组，如何求得P？

原文中的替代方法什么 α = X Z \alpha=\frac{X}{Z} α=ZX之类的也是没有什么用的，因为改变不了原文中z是一个分数的结构。最终没有办法得出一个线性方程组。我也比较疑惑这么有名的文章为啥会浆糊。

重投影误差的思考
其实我觉得这里的目标函数 e = Σ [ ( u i − X C i Z C i ) 2 + ( v i − Y C i Z C i ) 2 ] e=\Sigma[(u_i-\frac{X^{C_i}}{Z^{C_i}})^2+(v_i-\frac{Y^{C_i}}{Z^{C_i}})^2] e=Σ[(ui−ZCiXCi)2+(vi−ZCiYCi)2]是重投影误差。只是这里需要借助一些非线性最小二乘的方法了。

MSCKF的理解(3.4之(17)-(24))以及附录相关推荐

PL0编译器TurboPascal版再现时间:2009-07-20 17:24:49来源:网络作者:未知点击:52次
PL0编译器TurboPascal版再现时间:2009-07-20 17:24:49来源:网络作者:未知点击:52次 (********************* PL0 编译程序Turbo Pa ...
面试题 17.24. 最大子矩阵
面试题 17.24. 最大子矩阵给定一个正整数.负整数和 0 组成的 N × M 矩阵,编写代码找出元素总和最大的子矩阵. 返回一个数组 [r1, c1, r2, c2],其中 r1, c1 分别代 ...
(17/24) webpack实战技巧：生产环境和开发环境并行设置,实现来回切换
(17/24) webpack实战技巧:生产环境和开发环境并行设置,实现来回切换 1. 概述生产环境和开发环境所需依赖是不同: --开发依赖:就是开发中用到而发布时用不到的.在package.jso ...
汤姆大叔的深入理解JavaScript读后感二（17——24节）
今天要分享的内容是tom大叔的JavaScript系列的读书观后感2 border="0" width="330" height="86" ...
LeetCode代码刷题（17~24）
目录 17. 电话号码的字母组合 18. 四数之和 19. 删除链表的倒数第 N 个结点 20. 有效的括号 21. 合并两个有序链表 22. 括号生成 23. 合并K个升序链表 24. 两两交换链表 ...
深入理解BootStrap -- 面板(panel)17
1.面板面板(Panels)是Bootstrap框架新增的一个组件,其主要作用就是用来处理一些其他组件无法完成的功能.同样在不同的版本中具有不同的源码: ☑ Less版本:对应的源码文件是 pane ...
【Python基础】一文理解Python集合，17个方法全解，看完就够了
一.集合的定义 01 定义与特性 Python中的集合类似于数学中的集合概念,它是一组无序.不可重复元素序列,集合用{value1,value2}创建,某种程度上可以把集合看作是没有值的字典.字典是d ...
java 准备解析_深入理解JAVA虚拟机学习笔记24——类加载的准备和解析
每天进步一点点! 今天我们一起看一下类加载的准备阶段和解析阶段. 先看一下准备阶段:主要任务是在方法区中为类变量(仅static修饰变量,不包含实例变量)分配内存并设置类变量初始化的阶段. 这里面的区 ...
python创建一个集合类型的方法是_【Python基础】一文理解Python集合，17个方法全解，看完就够了...
一.集合的定义 01 定义与特性 Python中的集合类似于数学中的集合概念,它是一组无序.不可重复元素序列,集合用{value1,value2}创建,某种程度上可以把集合看作是没有值的字典.字典是d ...

MSCKF的理解(3.4之(17)-(24))以及附录

MSCKF的理解(3.4之(17)-(24))以及附录相关推荐

最新文章

热门文章