·946· 智能系统学报 第15卷 个误差，将误差求和构建最小二乘问题，然后优 的空间坐标为P=[X'YZ',它的相机投影模型为 化相机位姿： X' Y w=fz+c,v=元+6g (15) ep=min=】 o-iKepen 10) 其中，、、cx、c,为相机内参，为相机出厂时 式中ep为像素坐标误差，是二维向量。 标定。 在ICP问题中，考虑一组数量为M的筛选后 在求导上本文使用李代数的左乘扰动模型 的匹配点对Q=XYZP和Q=[XYZ]I,它的 对左乘扰动量，然后使用链式法则，列写如下： 李代数位姿表达式为 dep ends⊕_eraP (16) Q:exp()0' (11) OP'06E 类似式(10)，可以构建误差函数： 式中：⊕指李代数上的左乘扰动；右边两项，其中 第1项是关于投影点的导数，由式(15)可得 eo=min= ∑le.-exp(Eg (12) j: 0 式中ee为空间坐标误差，是三维向量。 (17) 将式(10)与(12)融合为一个BA问题，构成 72 五维代价函数： 第2项是误差关于位姿李代数的导数，由文献[12] 可推导出： e LKexp()P. OP' (18) min =.-P"] Meo ∑Ie,-ep0f 将式(17)与(18)相乘即可得单个误差雅克比 M (13) 矩阵Jp: 其中，e为五维向量，下面对式(13)进行优化求解。 0-x fX'Y 2 72 在使用高斯牛顿法求解之前，需要知道每个 22 +Ax2 22 -2 a65 误差项关于优化变量专的一阶导数，即雅克比矩 乡-点--， E 22 阵J以及专的初始位姿。的取值为 Jo的推导与之相似，空间点Q=XYZ]T经位姿 6=对产n5+n (14) 变换为Q=XY"Z"「，使用李代数扰动模型即 可得 关于J的形式是本文关键所在，在ep中，像 000-Z"Y" 素坐标误差为二维，位姿为六维，J将是2×6的 0 10Z0-X" 矩阵；在ee中，空间坐标误差为三维，J。将是 B8E 001-YX0 3×6的矩阵，故误差e的雅克比矩阵为5×6矩阵。 综上，可得由N个3D-2D点对与M个3D- 首先分析J的形式，记变换到相机坐标系下 3D点对组成的BA代价函数e的雅克比矩阵J为 0 学 0 -乃出 户X,Y 宁X M 0 0 0 -2 0 M 0 0 0 得到了初始位姿与雅克比矩阵J后调用 2)对于第k次迭代，求出当前的雅克比矩阵 高斯牛顿法，算法流程如下： J与误差e。 1)给定初始值5。 3)求解增量方程H△5=g。个误差，将误差求和构建最小二乘问题，然后优 化相机位姿： eP = min ξ = ∑N i=1 pi − 1 si Kexp(ξ ∧ )Pi 2 (10) 式中 eP 为像素坐标误差，是二维向量。 M Q = [Xi Yi Zi] T Q ′ = [X ′ i Y ′ i Z ′ i ] T 在 ICP 问题中，考虑一组数量为 的筛选后 的匹配点对 和 ，它的 李代数位姿表达式为 Qi = exp(ξ ∧ )Q ′ i (11) 类似式 (10)，可以构建误差函数： eQ = min ξ = ∑M i=1 Qi −exp(ξ ∧ )Q ′ i 2 (12) 式中 eQ 为空间坐标误差，是三维向量。 将式 (10) 与 (12) 融合为一个 BA 问题，构成 五维代价函数： e = min ξ =   1 N eP 1 M eQ   =   1 N ∑N i=1 pi − 1 si Kexp(ξ ∧ )Pi 2 1 M ∑M i=1 Qi −exp(ξ ∧ )Q ′ i 2   (13) 其中， e 为五维向量，下面对式 (13) 进行优化求解。 ξ J ξ ξ0 ξ0 在使用高斯牛顿法求解之前，需要知道每个 误差项关于优化变量 的一阶导数，即雅克比矩 阵 以及 的初始位姿 。 的取值为 ξ0 = N M +N ξP + M M +N ξQ (14) J eP JP eQ JQ e 关于 的形式是本文关键所在，在 中，像 素坐标误差为二维，位姿为六维， 将是 2×6 的 矩阵；在 中，空间坐标误差为三维 ， 将是 3×6 的矩阵，故误差 的雅克比矩阵为 5×6 矩阵。 首先分析 JP 的形式，记变换到相机坐标系下 P ′ = [X ′ Y ′ Z ′ ] 的空间坐标为 T，它的相机投影模型为 u = fx X ′ Z ′ +cx , v = fy Y ′ Z ′ +cy (15) fx、f 其中， y、cx、cy 为相机内参，为相机出厂时 标定。 ξ ∧ δξ 在求导上本文使用李代数的左乘扰动模型[16] ， 对 左乘扰动量 ，然后使用链式法则，列写如下： ∂eP ∂δξ = lim δξ→0 eP(δξ ⊕ξ) δξ = ∂eP ∂P′ ∂P ′ ∂δξ (16) 式中： ⊕ 指李代数上的左乘扰动；右边两项，其中 第 1 项是关于投影点的导数，由式 (15) 可得 ∂eP ∂P′ = −   fx Z ′ 0 − fxX ′ Z ′2 0 fy Z ′ − fyY ′ Z ′2   (17) 第 2 项是误差关于位姿李代数的导数，由文献 [12] 可推导出： ∂P ′ ∂δξ = [I,−P ′∧ ] (18) JP 将式 (17) 与 (18) 相乘即可得单个误差雅克比 矩阵 ： ∂eP ∂δξ = −   fx Z ′ 0 − fxX ′ Z ′2 − fxX ′Y ′ Z ′2 fx + fxX ′2 Z ′2 − fxY ′ Z ′2 0 fy Z ′ − fyY ′ Z ′2 −fy − fyY ′2 Z ′2 fyX ′Y ′ Z ′2 fyX ′ Z ′2   JQ Q ′ = [X ′ Y ′ Z ′ ] T Q ′′ = [X ′′ Y ′′ Z ′′] T 的推导与之相似，空间点 经位姿 变换为 ，使用李代数扰动模型即 可得 ∂eQ ∂δξ = −   1 0 0 0 −Z ′′ Y ′′ 0 1 0 Z ′′ 0 −X ′′ 0 0 1 −Y ′′ X ′′ 0   N M e J 综上，可得由 个 3D-2D 点对与 个 3D- 3D 点对组成的 BA 代价函数 的雅克比矩阵 为 J =   ∑N i=1 fx Z ′ i 0 − ∑N i=1 fxX ′ i Z ′2 i − ∑N i=1 fxX ′ iY ′ i Z ′2 i N fx + ∑N i=1 fxX ′2 i Z ′2 i − ∑N i=1 fxY ′′ i Z ′2 i 0 ∑N i=1 fY Z ′ i − ∑N i=1 fyY ′ i Z ′2 i −N fy − ∑N i=1 fyY ′2 i Z ′2 i ∑N i=1 fyX ′ iY ′ i Z ′2 i ∑N i=1 fyX ′ i Z ′2 i M 0 0 0 − ∑M i=1 Z ′′ i ∑M i=1 Y ′′ i 0 M 0 ∑M i=1 Z ′′ i 0 − ∑M i=1 X ′′ i 0 0 M − ∑M i=1 Y ′′ i ∑M i=1 X ′′ i 0   得到了初始位姿 ξ0 与雅克比矩阵 J 后调用 高斯牛顿法，算法流程如下： 1) 给定初始值 ξ0。 J e 2) 对于第 k 次迭代，求出当前的雅克比矩阵 与误差 。 3) 求解增量方程 H∆ξk = g。 ·946· 智 能 系 统 学 报 第 15 卷