江门桂城网站建设东莞千寻电子科技有限公司

当前位置： 首页 > news >正文

江门桂城网站建设,东莞千寻电子科技有限公司,做的网站怎么发网上,html电影网页设计实例代码使用非线性优化方法求解ICP 文章目录 使用非线性优化方法求解ICP前情提要ICP问题回顾对矩阵变量求导数 ICP问题的非线性解法代码示例 欢迎访问个人网络日志#x1f339;#x1f339;知行空间#x1f339;#x1f339; 前情提要 在迭代最近点算法ICP及SVD求解中介绍了ICP问…使用非线性优化方法求解ICP 文章目录 使用非线性优化方法求解ICP前情提要ICP问题回顾对矩阵变量求导数 ICP问题的非线性解法代码示例 欢迎访问个人网络日志知行空间 前情提要 在迭代最近点算法ICP及SVD求解中介绍了ICP问题及使用SVD分解求解ICP的方法。除了SVD还可以使用非线性优化的方法来求解ICP。 ICP问题回顾 还记得ICP优化的目标函数为 min ⁡ R , t 1 2 ∑ i n ∣ ∣ p i − ( R q i t ) ∣ ∣ 2 2 \min\limits{R,t}\frac{1}{2}\sum\limits{i}^n||p_i-(Rq_it)||2^2 R,tmin​21​i∑n​∣∣pi​−(Rqi​t)∣∣22​ 我们知道旋转和平移可以写成齐次变换的形式 T [ R t 0 0 ] T\begin{bmatrix} R t\ 0 0 \end{bmatrix} T[R0​t0​] ICP优化的目标函数可以写成 min ⁡ T 1 2 ∑ i n ∣ ∣ p i − T q i ∣ ∣ 2 2 \min\limits{T}\frac{1}{2}\sum\limits_{i}^n||p_i-Tq_i||_2^2 Tmin​21​i∑n​∣∣pi​−Tqi​∣∣22​ 这里要优化的变量是 T T T而 T T T是矩阵如何对矩阵求导数呢需要使用李群与李代数的概念。 对矩阵变量求导数 根据《视觉SLAM十四讲》第4讲的介绍 T T T属于特殊欧式群 S E ( 3 ) SE(3) SE(3) S E ( 3 ) SE(3) SE(3)具有连续性质是李群。 李群局部正切空间上的表示是李代数李代数相对于李群类似于导数对函数的意义。 S E ( 3 ) SE(3) SE(3)对应的李代数 s e ( 3 ) \mathcal{se(3)} se(3)为 s e ( 3 ) { ξ [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } \mathcal{se}(3)\left { \xi \begin{bmatrix} \rho \ \phi \end{bmatrix} \in \mathbb{R}^6, \rho \in \mathbb{R}^3 ,\phi \in \mathcal{so}(3), \xi^{\wedge } \begin{bmatrix} \phi ^{\wedge } \rho \ 0^T 0 \end{bmatrix} \in \mathbb{R}^{4\times 4} \right } se(3){ξ[ρϕ​]∈R6,ρ∈R3,ϕ∈so(3),ξ∧[ϕ∧0T​ρ0​]∈R4×4} 其中 ξ \xi ξ是 s e ( 3 ) \mathcal{se}(3) se(3)的元素,是包含平移 ρ \rho ρ和旋转 ϕ \phi ϕ的六维向量。 ϕ \phi ϕ其实是 s o ( 3 ) \mathcal{so}(3) so(3)的元素。 ∧ {\wedge } ∧符号这里表示将六维向量转换成四维矩阵。 在 t 0 t0 t0附近齐次变换矩阵可以由 T e x p ( ξ 0 t ) Texp(\xi_0t) Texp(ξ0​t)计算出来由此可以看到齐次变换矩阵与一个反对称矩阵 ξ 0 ∧ t \xi_0 ^{\wedge }t ξ0∧​t建立了联系在一时刻对于给定的 T T T,可以求得一个 ξ \xi ξ,其描述了 T T T在局部的导数关系。 矩阵的指数运算显然没有定义如何计算呢这里可以用泰勒公式 e x p ( A ) ∑ n 0 ∞ 1 n ! A n exp(A)\sum\limits {n0}^{\infty }\frac{1}{n!}A^n exp(A)n0∑∞​n!1​An 将 ξ \xi ξ写成旋转加平移的形式 [ ρ , ϕ ] T [\rho, \phi]^T [ρ,ϕ]T,再将 ϕ \phi ϕ写成模长和方向的形式 ϕ θ a \phi\theta a ϕθa,代入上面的公式可得 e x p ( ξ ∧ ) [ ∑ n 0 ∞ 1 n ! ( ϕ ∧ ) n ∑ n 0 ∞ 1 ( n 1 ) ! ( ϕ ∧ ) n ρ 0 1 ] ≜ [ R J ρ 0 T 1 ] T exp(\xi^{\wedge})\begin{bmatrix} \sum\limits{n0}^{\infty}\frac{1}{n!}(\phi ^\wedge )^n \sum\limits_{n0}^{\infty}\frac{1}{(n1)!}(\phi ^\wedge )^n\rho \ 0 1 \end{bmatrix}\triangleq \begin{bmatrix} R J\rho \ 0^T 1 \end{bmatrix}T exp(ξ∧) ​n0∑∞​n!1​(ϕ∧)n0​n0∑∞​(n1)!1​(ϕ∧)nρ1​ ​≜[R0T​Jρ1​]T 当 ϕ θ a \phi\theta a ϕθa时推导可得 J s i n θ θ I ( 1 − s i n θ θ ) a a T 1 − c o s θ θ a ∧ J\frac{sin\theta}{\theta}I(1-\frac{sin\theta}{\theta})aa^T\frac{1-cos\theta}{\theta}a^\wedge Jθsinθ​I(1−θsinθ​)aaTθ1−cosθ​a∧ 当在李群中完成两个矩阵的乘法时也就是依次进行两个变换时李代数上对应会发生怎样的变化呢 S E ( 3 ) SE(3) SE(3)是 s e ( 3 ) \mathcal{se}(3) se(3)的指数映射那么 e x p ( ξ 1 ∧ ) e x p ( ξ 2 ∧ ) exp(\xi_1^\wedge)exp(\xi_2^\wedge) exp(ξ1∧​)exp(ξ2∧​)是否等于 e x p ( ( ξ 1 ξ 2 ) ∧ ) exp((\xi_1\xi_2)^\wedge) exp((ξ1​ξ2​)∧)呢对于矩阵的指数运算上式并不成立。 两个李代数指数映射乘积的完整形式由Baker-Campbell-Hausdorff(BCH)公式给出 l n ( e x p ( A ) e x p ( B ) ) A B 1 2 [ A , B ] 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] … ln(exp(A)exp(B))AB\frac{1}{2}[A,B]\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]… ln(exp(A)exp(B))AB21​[A,B]121​[A,[A,B]]−121​[B,[A,B]]… [,]是李括号表示二元运算。 以旋转矩阵组成的特殊正交群 S O ( 3 ) SO(3) SO(3)为例 考虑 S O ( 3 ) SO(3) SO(3)上的李代数 ( l n ( e x p ( ϕ 1 ∧ ) ( ϕ 2 ∧ ) ) ) ∧ (ln(exp(\phi_1^\wedge)(\phi_2^\wedge)))^\wedge (ln(exp(ϕ1∧​)(ϕ2∧​)))∧,当 ϕ 1 \phi_1 ϕ1​或 ϕ 2 \phi_2 ϕ2​为小量BCH公式中李括号二次以上的项可以忽略掉则可得近似线性表示为 ( l n ( e x p ( ϕ 1 ∧ ) ( ϕ 2 ∧ ) ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 ϕ 2 ϕ 1 为小量 J r ( ϕ 1 ) − 1 ϕ 2 ϕ 1 ϕ 2 为小量 (ln(exp(\phi_1^\wedge)(\phi_2^\wedge)))^\vee\approx\left{\begin{matrix} J_l(\phi_2)^{-1}\phi_1 \phi_2 \phi_1为小量\ J_r(\phi_1)^{-1}\phi_2 \phi_1 \phi_2为小量 \end{matrix}\right. (ln(exp(ϕ1∧​)(ϕ2∧​)))∨≈{Jl​(ϕ2​)−1ϕ1​ϕ2​Jr​(ϕ1​)−1ϕ2​ϕ1​​ϕ1​为小量ϕ2​为小量​ 上面公式中第一种相当于是对原旋转矩阵左乘一个微小变换矩阵第二种相当于是对原旋转矩阵右乘一个微小变换矩阵。乘以微小变换矩阵相当于是发生了微小的位移和姿态变化。 左乘微小变换矩阵的近似雅可比矩阵 J l J_l Jl​同推导 s e ( 3 ) \mathcal{se}(3) se(3)的指数映射中作用在平移向量 ρ \rho ρ上的雅可比 J J J J l J s i n θ θ I ( 1 − s i n θ θ ) a a T 1 − c o s θ θ a ∧ J_lJ\frac{sin\theta}{\theta}I(1-\frac{sin\theta}{\theta})aa^T\frac{1-cos\theta}{\theta}a^\wedge Jl​Jθsinθ​I(1−θsinθ​)aaTθ1−cosθ​a∧。 通过上面的推导就建立了李群乘法和李代数加法之间的关系。 以旋转矩阵为例对某个旋转 R R R,其对应的李代数为 ϕ \phi ϕ,对其左乘一个微小旋转 △ R \triangle R △R,其对应的李代数为 △ ϕ \triangle \phi △ϕ。那么在李群上的变化就为 △ R R \triangle R R △RR,而对应的在李代数的变化根据BCH公式近似就为 J l − 1 ( ϕ ) △ ϕ ϕ J_l^{-1}(\phi)\triangle\phi\phi Jl−1​(ϕ)△ϕϕ,综合以上可以写成 e x p ( △ ϕ ∧ ) e x p ( ϕ ∧ ) e x p ( ( ϕ J l − 1 ( ϕ ) △ ϕ ) ) exp(\triangle \phi^\wedge)exp(\phi^\wedge)exp((\phiJ_l^{-1}(\phi)\triangle \phi)) exp(△ϕ∧)exp(ϕ∧)exp((ϕJl−1​(ϕ)△ϕ)) 上面公式中如果左乘的微小旋转矩阵对应的李代数带雅可比矩阵则上式就变为(以左乘为例) e x p ( ( J l ( ϕ ) △ ϕ ) ∧ ) e x p ( ϕ ∧ ) e x p ( ( ϕ J l − 1 ( ϕ ) J l ( ϕ ) △ ϕ ) ∧ ) e x p ( ( ϕ △ ϕ ) ∧ ) exp((J_l(\phi)\triangle \phi)^\wedge)exp(\phi^\wedge)exp((\phiJ_l^{-1}(\phi)J_l(\phi)\triangle \phi)^\wedge)exp((\phi\triangle \phi)^\wedge) exp((Jl​(ϕ)△ϕ)∧)exp(ϕ∧)exp((ϕJl−1​(ϕ)Jl​(ϕ)△ϕ)∧)exp((ϕ△ϕ)∧) 由上李代数上的加法可近似为李群上带左或右雅可比的乘法 e x p ( ( ϕ △ ϕ ) ∧ ) e x p ( ( J l ( ϕ ) △ ϕ ) ∧ ) e x p ( ϕ ∧ ) e x p ( ϕ ∧ ) e x p ( ( J r ( ϕ ) △ ϕ ) ∧ ) exp((\phi\triangle \phi)^\wedge)exp((J_l(\phi)\triangle \phi)^\wedge)exp(\phi^\wedge)exp(\phi^\wedge)exp((Jr(\phi)\triangle \phi)^\wedge) exp((ϕ△ϕ)∧)exp((Jl​(ϕ)△ϕ)∧)exp(ϕ∧)exp(ϕ∧)exp((Jr​(ϕ)△ϕ)∧) 因为旋转矩阵组成的李群各个变换之间只定义了有意义的乘法而没有定义有意义的加法因此不能直接在李群上定义导数。 而考虑李群与李代数之间的关系以旋转矩阵为例对 S O ( 3 ) SO(3) SO(3)上的李代数求导 ∂ R P ∂ R \frac{\partial RP}{\partial R} ∂R∂RP​ 由于 S O ( 3 ) SO(3) SO(3)上只对乘法满足封闭性因此不能按照常规的微分定义来求导设 R R R对应的李代数为 ϕ \phi ϕ,由 R e x p ( ϕ ∧ ) Rexp(\phi^\wedge) Rexp(ϕ∧)可得 ∂ e x p ( ϕ ∧ ) P ∂ ϕ lim ⁡ δ ϕ → 0 e x p ( ( ϕ δ ϕ ) ∧ ) p − e x p ( ϕ ∧ ) p δ ϕ lim ⁡ δ ϕ → 0 e x p ( J l ( ϕ ) δ ϕ ) ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p δ ϕ lim ⁡ δ ϕ → 0 ( I ( J l ( ϕ ) δ ϕ ) ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p δ ϕ lim ⁡ δ ϕ → 0 ( J l ( ϕ ) δ ϕ ) ∧ e x p ( ϕ ∧ ) p δ ϕ lim ⁡ δ ϕ → 0 − ( e x p ( ϕ ∧ ) p ) ∧ J l ( ϕ ) δ ϕ δ ϕ − ( R p ) ∧ J l ( ϕ ) \begin{align}\frac{\partial exp(\phi^\wedge)P}{\partial \phi}\lim{\delta\phi\to 0}\frac{exp((\phi\delta \phi)^\wedge)p-exp(\phi^\wedge)p}{\delta\phi}\ \lim_{\delta\phi\to 0}\frac{exp(Jl(\phi)\delta \phi)^\wedge)exp(\phi^\wedge)p-exp(\phi^\wedge)p}{\delta\phi}\ \lim{\delta\phi\to 0}\frac{(I(Jl(\phi)\delta \phi)^\wedge)exp(\phi^\wedge)p-exp(\phi^\wedge)p}{\delta\phi}\ \lim{\delta\phi\to 0}\frac{(Jl(\phi)\delta \phi)^\wedge exp(\phi^\wedge)p}{\delta\phi}\ \lim{\delta\phi\to 0}\frac{-(exp(\phi^\wedge)p)^\wedge J_l(\phi)\delta \phi}{\delta\phi}\ -(Rp)^\wedge J_l(\phi)\end{align} ∂ϕ∂exp(ϕ∧)P​​δϕ→0lim​δϕexp((ϕδϕ)∧)p−exp(ϕ∧)p​δϕ→0lim​δϕexp(Jl​(ϕ)δϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p​δϕ→0lim​δϕ(I(Jl​(ϕ)δϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p​δϕ→0lim​δϕ(Jl​(ϕ)δϕ)∧exp(ϕ∧)p​δϕ→0lim​δϕ−(exp(ϕ∧)p)∧Jl​(ϕ)δϕ​−(Rp)∧Jl​(ϕ)​​ 上面推导过程中1到2用到了BCH近似公式2到3用到了泰勒公式4到5用到了反对称矩阵的性质 A − A T A-A^T A−AT最终得到了旋转后的点相对于旋转矩阵李代数的导数。 通过转换成李代数求导得到的最终的导数包含一个 J l Jl Jl​矩阵其形式和计算过于复杂。另外一种李群求导的方法是扰动模型对 R R R进行一个左乘或右乘扰动将结果相对于扰动的变化率作为导数以左乘扰动 △ R \bigtriangleup R △R为例其对应的李代数为 φ \varphi φ推导如下 ∂ R p ∂ R lim ⁡ φ → 0 e x p ( φ ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p φ \frac{\partial Rp}{\partial R}\lim\limits{\varphi \to 0}\frac{exp(\varphi^\wedge)exp(\phi^\wedge)p-exp(\phi^\wedge)p}{\varphi} ∂R∂Rp​φ→0lim​φexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p​ 该式的求导可写成 ∂ R p ∂ R lim ⁡ φ → 0 e x p ( φ ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p φ lim ⁡ φ → 0 ( I φ ∧ ) e x p ( ϕ ∧ ) p − e x p ( ϕ ∧ ) p φ lim ⁡ φ → 0 φ ∧ R p φ lim ⁡ φ → 0 − ( R p ) ∧ φ φ − ( R p ) ∧ \begin{align}\frac{\partial Rp}{\partial R}\lim\limits{\varphi \to 0}\frac{exp(\varphi^\wedge)exp(\phi^\wedge)p-exp(\phi^\wedge)p}{\varphi}\ \lim\limits{\varphi \to 0}\frac{(I\varphi^\wedge)exp(\phi^\wedge)p-exp(\phi^\wedge)p}{\varphi} \ \lim\limits{\varphi \to 0}\frac{\varphi^\wedge Rp}{\varphi} \lim\limits{\varphi \to 0}\frac{-(Rp)^\wedge \varphi}{\varphi} -(Rp)^\wedge\end{align} ∂R∂Rp​​φ→0lim​φexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p​φ→0lim​φ(Iφ∧)exp(ϕ∧)p−exp(ϕ∧)p​φ→0lim​φφ∧Rp​φ→0lim​φ−(Rp)∧φ​−(Rp)∧​​ 相比于对李代数直接求导省去了一个雅可比 J l Jl Jl​的计算。 ICP问题的非线性解法 考虑前面ICP优化的目标函数 e min ⁡ T 1 2 ∑ i n ∣ ∣ p i − T q i ∣ ∣ 2 2 min ⁡ ξ 1 2 ∑ i 1 n ∣ ∣ ( p i − e x p ( ξ ∧ ) q i ) ∣ ∣ 2 2 e \min\limits{T}\frac{1}{2}\sum\limits_{i}^n||p_i-Tq_i||2^2\min\limits\xi\frac{1}{2}\sum_{i1}^{n}||(p_i-exp(\xi^\wedge)q_i)||2^2 eTmin​21​i∑n​∣∣pi​−Tqi​∣∣22​ξmin​21​i1∑n​∣∣(pi​−exp(ξ∧)qi​)∣∣22​ ξ \xi ξ是 S E ( 3 ) SE(3) SE(3) T T T对应的李代数。 单个误差项关于位姿的导数使用李代数的扰动模型可得 ∂ e ∂ ξ lim ⁡ δ ξ → 0 e ( δ ξ ⊕ ξ ) − e ( ξ ) δ ξ \frac{\partial e}{\partial \xi}\lim\limits{\delta\xi\to 0}\frac{e(\delta\xi\oplus\xi)-e(\xi)}{\delta\xi} ∂ξ∂e​δξ→0lim​δξe(δξ⊕ξ)−e(ξ)​ ⊕ \oplus ⊕表示左乘模型 对于n对三维空间中的点 p p p及其投影 q q q,希望计算的相机旋转和平移分别为 R , t R,t R,t其对应的李群为 T T T。对于空间中的一点 p i [ X i , Y i , Z i ] T p_i[X_i,Y_i,Z_i]^T pi​[Xi​,Yi​,Zi​]T其对应的投影点像素坐标为 μ i [ μ i , v i ] T \mu_i[\mu_i,v_i]^T μi​[μi​,vi​]T,根据相机模型有以下关系 s i [ μ i v i 1 ] K T [ X i Y i Z i 1 ] s_i\begin{bmatrix}\mu_i\v_i\1 \end{bmatrix}KT\begin{bmatrix}X_i\Y_i\Z_i\1 \end{bmatrix} si​ ​μi​vi​1​ ​KT ​Xi​Yi​Zi​1​ ​ 写成矩阵的形式为 s i μ i K T p i s_i\mu_iKTp_i si​μi​KTpi​ 上式隐含了一次齐次坐标往三维坐标的转换。 对于n对空间点和对应的图像中投影的观测点要估计相机的位姿目标函数就是最小化两者之间的误差 T ∗ a r g min ⁡ T 1 2 ∑ i 1 n ∣ ∣ μ i − 1 s i K T p i ∣ ∣ T^*arg \min\limitsT\frac{1}{2}\sum\limits{i1}^{n}||\mu_i-\frac{1}{s_i}KTpi|| T∗argTmin​21​i1∑n​∣∣μi​−si​1​KTpi​∣∣ 记通过相机外参变换到相机坐标系下 p p p点对应的点 q q q的坐标为 q ( T p ) 1 : 3 [ X ′ , Y ′ , Z ′ ] q(Tp){1:3}[X,Y,Z] q(Tp)1:3​[X′,Y′,Z′] 根据相机模型关于 q q q点有 s μ K q s\muKq sμKq 展开有 [ s μ s v s ] [ f x 0 c x o f y c y 0 0 1 ] [ X ′ Y ′ Z ′ ] \begin{bmatrix} s\mu\sv\s \end{bmatrix}\begin{bmatrix} f_x0c_x\of_yc_y\001 \end{bmatrix}\begin{bmatrix}X\Y\Z\end{bmatrix} ​sμsvs​ ​ ​fx​o0​0fy​0​cx​cy​1​ ​ ​X′Y′Z′​ ​ 重投影误差的左乘扰动求导数为 ∂ e ∂ ξ ∂ e ∂ q ∂ q ∂ ξ \frac{\partial e}{\partial \xi}\frac{\partial e}{\partial q}\frac{\partial q}{\partial \xi} ∂ξ∂e​∂q∂e​∂ξ∂q​ 根据目标函数和相机模型的公式有 ∂ e ∂ q − [ ∂ μ ∂ X ′ ∂ μ ∂ Y ′ ∂ μ ∂ Z ′ ∂ v ∂ X ′ ∂ v ∂ Y ′ ∂ v ∂ Z ′ ] − [ f x Z ′ 0 − f x X ′ Z ′ 2 0 f y Z ′ − f y Y ′ Z ′ 2 ] \frac{\partial e}{\partial q}-\begin{bmatrix}\frac{\partial \mu}{\partial X} \frac{\partial \mu}{\partial Y} \frac{\partial \mu}{\partial Z}\ \frac{\partial v}{\partial X}\frac{\partial v}{\partial Y}\frac{\partial v}{\partial Z}\end{bmatrix}-\begin{bmatrix}\frac{f_x}{Z} 0 -\frac{f_xX}{Z^2}\ 0\frac{f_y}{Z}-\frac{f_yY}{Z^2}\end{bmatrix} ∂q∂e​−[∂X′∂μ​∂X′∂v​​∂Y′∂μ​∂Y′∂v​​∂Z′∂μ​∂Z′∂v​​]−[Z′fx​​0​0Z′fy​​​−Z′2fx​X′​−Z′2fy​Y′​​] 而第二项为变换后的点关于李代数的导数 ∂ ( T p ) ∂ T [ I − q ∧ 0 0 ] \frac{\partial(Tp)}{\partial T}\begin{bmatrix}I-q^\wedge \ 0 0 \end{bmatrix} ∂T∂(Tp)​[I0​−q∧0​] 根据 q q q定义取出前三维 ∂ q ∂ T [ I , − q ∧ ] T \frac{\partial q}{\partial T}[I,-q^\wedge ]^T ∂T∂q​[I,−q∧]T 最后将两者相乘可以求得 ∂ e ∂ ξ \frac{\partial e}{\partial \xi} ∂ξ∂e​。 代码示例 使用G2O求解非线性优化问题节点就是待求解变量边是观测数据点。 前面推导的是相机模型的点与三维空间点的PnP问题的求解对于ICP直接是成对三维点之间的优化相对于PnP更简单。 定义G2O的顶点和边 class VertexPose : public g2o::BaseVertex6, Sophus::SE3d {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;// 设置初始化的更新值 virtual void setToOriginImpl() override { _estimate Sophus::SE3d();}// left multiplication on SE3virtual void oplusImpl(const double *update) {Eigen::Matrixdouble, 6, 1 update_eigen;update_eigen update[0], update[1], update[2],update[3], update[4], update[5];_estimate Sophus::SE3d::exp(update_eigen) * _estimate;}virtual bool read(std::istream in) override {return true;} virtual bool write(std::ostream out) const override { return true;}};class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge3, Eigen::Vector3d, bcv::VertexPose {public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d point) : _point(point) {}virtual void computeError() override {const VertexPose* p static_castconst VertexPose* (_vertices[0]);_error _measurement - p-estimate() * _point;}virtual void linearizeOplus() override {VertexPose p static_castVertexPose (_vertices[0]);Sophus::SE3d T p-estimate();Eigen::Vector3d xyz_trans T * _point;_jacobianOplusXi.block3, 3(0, 0) -Eigen::Matrix3d::Identity();_jacobianOplusXi.block3, 3(0, 3) Sophus::SO3d::hat(xyz_trans);}bool read(std::istream in) { return true; }bool write(std::ostream out) const { return true; }protected:Eigen::Vector3d _point;}; 定义求解器 void ICPSolver::NLOSolver(std::vectorcv::Point3f pts1,std::vectorcv::Point3f pts2,cv::Mat R, cv::Mat t){typedef g2o::BlockSolverX BlockSolverType;typedef g2o::LinearSolverDenseBlockSolverType::PoseMatrixType LinearSolverType;auto solver new g2o::OptimizationAlgorithmGaussNewton(g2o::make_uniqueBlockSolverType(g2o::make_uniqueLinearSolverType()));g2o::SparseOptimizer optimizer; // graph modeloptimizer.setAlgorithm(solver); // set solveroptimizer.setVerbose(true); // print infobcv::VertexPose *p new VertexPose();p-setId(0);p-setEstimate(Sophus::SE3d());optimizer.addVertex(p);// add edgefor(size_t i 0; i pts1.size(); i) {bcv::EdgeProjectXYZRGBDPoseOnly *e new bcv::EdgeProjectXYZRGBDPoseOnly(Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));e-setVertex(0, p);e-setMeasurement(Eigen::Vector3d(pts1[i].x, pts1[i].y, pts1[i].z));e-setInformation(Eigen::Matrix3d::Identity());optimizer.addEdge(e);}auto t1 std::chrono::system_clock::now();optimizer.initializeOptimization();optimizer.optimize(10);auto t2 std::chrono::system_clock::now();auto d std::chrono::durationcaststd::chrono::milliseconds(t2 - t1).count();std::cout duration: d ms std::endl;std::cout after optim:\n;std::cout T\n p-estimate().matrix() std::endl;Eigen::Matrix3d R p-estimate().rotationMatrix();Eigen::Vector3d t_ p-estimate().translation();std::cout det(R) R.determinant() std::endl;std::cout RR^T R_ * R.transpose() std::endl;std::cout R:\n R std::endl;std::cout t:\n t_ std::endl;R (cv::Matdouble(3, 3) R(0, 0), R(0, 1), R(0, 2),R(1, 0), R(1, 1), R(1, 2),R(2, 0), R(2, 1), R(2, 2));t (cv::Matdouble(3, 1) t(0, 0), t(1, 0), t(2, 0));} 完整可运行代码见https://gitee.com/lx_r/basic_cplusplus_examples 1.视觉SLAM十四讲第二版p196-198