最符合

ms",verdana,arial,sans-serif;font-size:16px;fill:#333;}#mermaid-svg-JdAcJiMNI4rstIms

.error-icon{fill:#552222;}#mermaid-svg-JdAcJiMNI4rstIms

.error-text{fill:#552222;stroke:#552222;}#mermaid-svg-JdAcJiMNI4rstIms

.edge-thickness-normal{stroke-width:2px;}#mermaid-svg-JdAcJiMNI4rstIms

.edge-thickness-thick{stroke-width:3.5px;}#mermaid-svg-JdAcJiMNI4rstIms

.edge-pattern-solid{stroke-dasharray:0;}#mermaid-svg-JdAcJiMNI4rstIms

.edge-pattern-dashed{stroke-dasharray:3;}#mermaid-svg-JdAcJiMNI4rstIms

.edge-pattern-dotted{stroke-dasharray:2;}#mermaid-svg-JdAcJiMNI4rstIms

.marker{fill:#333333;stroke:#333333;}#mermaid-svg-JdAcJiMNI4rstIms

.marker.cross{stroke:#333333;}#mermaid-svg-JdAcJiMNI4rstIms

ms",verdana,arial,sans-serif;font-size:16px;}#mermaid-svg-JdAcJiMNI4rstIms

ms",verdana,arial,sans-serif;color:#333;}#mermaid-svg-JdAcJiMNI4rstIms

.cluster-label

text{fill:#333;}#mermaid-svg-JdAcJiMNI4rstIms

.cluster-label

span{color:#333;}#mermaid-svg-JdAcJiMNI4rstIms

.label

text,#mermaid-svg-JdAcJiMNI4rstIms

span{fill:#333;color:#333;}#mermaid-svg-JdAcJiMNI4rstIms

.node

rect,#mermaid-svg-JdAcJiMNI4rstIms

.node

circle,#mermaid-svg-JdAcJiMNI4rstIms

.node

ellipse,#mermaid-svg-JdAcJiMNI4rstIms

.node

polygon,#mermaid-svg-JdAcJiMNI4rstIms

.node

path{fill:#ECECFF;stroke:#9370DB;stroke-width:1px;}#mermaid-svg-JdAcJiMNI4rstIms

.node

.label{text-align:center;}#mermaid-svg-JdAcJiMNI4rstIms

.node.clickable{cursor:pointer;}#mermaid-svg-JdAcJiMNI4rstIms

.arrowheadPath{fill:#333333;}#mermaid-svg-JdAcJiMNI4rstIms

.edgePath

.path{stroke:#333333;stroke-width:2.0px;}#mermaid-svg-JdAcJiMNI4rstIms

.flowchart-link{stroke:#333333;fill:none;}#mermaid-svg-JdAcJiMNI4rstIms

.edgeLabel{background-color:#e8e8e8;text-align:center;}#mermaid-svg-JdAcJiMNI4rstIms

.edgeLabel

rect{opacity:0.5;background-color:#e8e8e8;fill:#e8e8e8;}#mermaid-svg-JdAcJiMNI4rstIms

.cluster

rect{fill:#ffffde;stroke:#aaaa33;stroke-width:1px;}#mermaid-svg-JdAcJiMNI4rstIms

.cluster

text{fill:#333;}#mermaid-svg-JdAcJiMNI4rstIms

.cluster

span{color:#333;}#mermaid-svg-JdAcJiMNI4rstIms

div.mermaidTooltip{position:absolute;text-align:center;max-width:200px;padding:2px;font-family:"trebuchet

ms",verdana,arial,sans-serif;font-size:12px;background:hsl(80,

100%,

#aaaa33;border-radius:2px;pointer-events:none;z-index:100;}#mermaid-svg-JdAcJiMNI4rstIms

:root{--mermaid-font-family:"trebuchet

李群-李代数间转换

一个由三维向量组成的集合每个向量对应一个反对称矩阵可以用于表达旋转矩阵的导数。

\mathfrak{so}(3)\{\phi\in\mathbb{R}^3,\bm{\Phi}\phi\^{}\in\mathbb{R}^{3\times3}\}

so(3){ϕ∈R3,Φϕˆ∈R3×3}

\bm{R}exp(\phi^{\land})exp(\***ta\bm{a}^{\land})cos\***ta\bm{I}

(1-cos\***ta)\bm{a}\bm{a}^Tsin\***ta\bm{a}^{\land}

Rexp(ϕ∧)exp(θa∧)cosθI(1−cosθ)aaTsinθa∧

对数映射

\begin{align*}exp(\phi^{\land})

exp(\***ta\bm{a}^{\land})\sum\limits_{n0}^{\infty}\frac{1}{n!}

...

\frac{1}{2!}\***ta^2\bm{a}^{\land}\bm{a}^{\land}

\frac{1}{3!}\***ta^3\bm{a}^{\land}\bm{a}^{\land}\bm{a}^{\land}

\frac{1}{4!}\***ta^4(\bm{a}^{\land})^4···\\

\bm{a}\bm{a}^T

\bm{a}^{\land}\bm{a}^{\land}\bm{a}^{\land}

(\bm{a}^{\land})^3

\frac{1}{2!}\***ta^2\bm{a}^{\land}\bm{a}^{\land}-\frac{1}{3!}\***ta^3\bm{a}^{\land}-\frac{1}{4!}\***ta^4(\bm{a}^{\land})^2···\\

\bm{a}\bm{a}^T

\frac{1}{3!}\***ta^3\frac{1}{5!}\***ta^5-···)\bm{a}^{\land}-(1-\frac{1}{2!}\***ta^2\frac{1}{4!}\***ta^4-···)\bm{a}^{\land}\bm{a}^{\land}\\

\bm{I}sin\***ta

\bm{a}^{\land}-cos\***ta\bm{a}^{\land}\bm{a}^{\land}\\

(1-cos\***ta)\bm{a}^{\land}\bm{a}^{\land}\bm{I}

(1-cos\***ta)(\bm{a}\bm{a}^T-\bm{I})\bm{I}

cos\***ta\bm{I}

exp(ϕ∧)原式原式​exp(θa∧)n0∑∞​n!1​(θa∧)nn0,1,2,3,...Iθa∧2!1​θ2a∧a∧3!1​θ3a∧a∧a∧4!1​θ4(a∧)4⋅⋅⋅将a∧a∧aaT−I;a∧a∧a∧(a∧)3−a∧代入aaT−a∧a∧θa∧2!1​θ2a∧a∧−3!1​θ3a∧−4!1​θ4(a∧)2⋅⋅⋅aaT(θ−3!1​θ35!1​θ5−⋅⋅⋅)a∧−(1−2!1​θ24!1​θ4−⋅⋅⋅)a∧a∧a∧a∧Isinθa∧−cosθa∧a∧(1−cosθ)a∧a∧Isinθa∧(1−cosθ)(aaT−I)Isinθa∧cosθI(1−cosθ)aaTsinθa∧​

exp(\***ta\bm{a}^{\land})cos\***ta\bm{I}

(1-cos\***ta)\bm{a}\bm{a}^Tsin\***ta\bm{a}^{\land}

—————

\sum\limits_{n0}^{\infty}\frac{1}{(n1)!}(\phi^{\land})^n

\sum\limits_{n0}^{\infty}\frac{1}{(n1)!}(\***ta\bm{a}^{\land})^n\\

\bm{I}

\frac{1}{2!}\***ta\bm{a}^{\land}

\frac{1}{3!}\***ta^2(\bm{a}^{\land})^2

\frac{1}{4!}\***ta^3(\bm{a}^{\land})^3

\frac{1}{5!}\***ta^4(\bm{a}^{\land})^4···\\

\frac{1}{\***ta}(

\frac{1}{4!}\***ta^4···)\bm{a}^{\land}

\frac{1}{\***ta}(

\frac{1}{5!}\***ta^5···)(\bm{a}^{\land})^2\bm{I}

\frac{1}{\***ta}(

\frac{1}{\***ta}(\***ta-sin\***ta)(\bm{a}\bm{a}^T-\bm{I})\bm{I}

\frac{sin\***ta}{\***ta}\bm{I}(1-\frac{sin\***ta}{\***ta})\bm{a}\bm{a}^T

\frac{1-cos\***ta}{\***ta}\bm{a}^{\land}\\

\bm{J}

n0∑∞​(n1)!1​(ϕ∧)n​n0∑∞​(n1)!1​(θa∧)nI2!1​θa∧3!1​θ2(a∧)24!1​θ3(a∧)35!1​θ4(a∧)4⋅⋅⋅θ1​(2!1​θ2−4!1​θ4⋅⋅⋅)a∧θ1​(3!1​θ3−5!1​θ5⋅⋅⋅)(a∧)2Iθ1​(1−cosθ)a∧θ1​(θ−sinθ)(aaT−I)Iθsinθ​I(1−θsinθ​)aaTθ1−cosθ​a∧defJ​

\bm{J}\frac{sin\***ta}{\***ta}\bm{I}(1-\frac{sin\***ta}{\***ta})\bm{a}\bm{a}^T

\frac{1-cos\***ta}{\***ta}\bm{a}^{\land}

Jθsinθ​I(1−θsinθ​)aaTθ1−cosθ​a∧

\bm{R}cos\***ta\bm{I}(1-cos\***ta)\bm{a}\bm{a}^T

————

Baker-Campbell-Hausdorff公式(BCH公式)

4.3.2

\sum\limits_{i1}^{N}||\bm{z_i-Tp_i}||_2^2

求解上述问题需要计算目标函数

2、对李群左乘或右乘微小扰动然后对该扰动求导称为左扰动和右扰动模型。

4.3.3

\begin{align*}\frac{\partial(\bm{Rp})}{\partial\bm{R}}

\xlongequal{R对应的李代数为\phi}\frac{\partial(\exp(\phi^{\land})\bm{p})}{\partial\phi}

\lim\limits_{Δ\phi\to0}\frac{\exp((\phiΔ\phi)^{\land})\bm{p}-\exp(\phi^{\land})\bm{p}}{Δ\phi}

式(4.35)\\

\lim\limits_{Δ\phi\to0}\frac{\exp((\bm{J}_lΔ\phi)^{\land})\exp(\phi^{\land})\bm{p}-\exp(\phi^{\land})\bm{p}}{Δ\phi}

\lim\limits_{Δ\phi\to0}\frac{(\bm{I}(\bm{J}_lΔ\phi)^{\land})\exp(\phi^{\land})\bm{p}-\exp(\phi^{\land})\bm{p}}{Δ\phi}

\lim\limits_{Δ\phi\to0}\frac{(\bm{J}_lΔ\phi)^{\land}\exp(\phi^{\land})\bm{p}}{Δ\phi}

a^{\land}

\lim\limits_{Δ\phi\to0}\frac{-(\exp(\phi^{\land})\bm{p})^{\land}\bm{J}_lΔ\phi}{Δ\phi}

\end{align*}

∂ϕ∂(exp(ϕ∧)p)​Δϕ→0lim​Δϕexp((ϕΔϕ)∧)p−exp(ϕ∧)p​由式(4.35)Δϕ→0lim​Δϕexp((Jl​Δϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p​Δϕ→0lim​Δϕ(I(Jl​Δϕ)∧)exp(ϕ∧)p−exp(ϕ∧)p​Δϕ→0lim​Δϕ(Jl​Δϕ)∧exp(ϕ∧)p​a∧等效于a×,因此根据叉乘的性质Δϕ→0lim​Δϕ−(exp(ϕ∧)p)∧Jl​Δϕ​−(Rp)∧Jl​​

exp

\frac{\partial(\exp(\phi^{\land})\bm{p})}{\partial\phi}

——————

\begin{align*}\frac{\partial(\bm{Rp})}{\partial\varphi}

\lim\limits_{\varphi\to0}\frac{\exp(\varphi^{\land})\exp(\phi^{\land})\bm{p}-\exp(\phi^{\land})\bm{p}}{\varphi}\\

\lim\limits_{\varphi\to0}\frac{(\bm{I}

\varphi^{\land})\exp(\phi^{\land})\bm{p}-\exp(\phi^{\land})\bm{p}}{\varphi}\\

\lim\limits_{\varphi\to0}\frac{\varphi^{\land}\exp(\phi^{\land})\bm{p}}{\varphi}\\

\lim\limits_{\varphi\to0}\frac{\varphi^{\land}\bm{Rp}}{\varphi}\\

\lim\limits_{\varphi\to0}\frac{-(\bm{Rp})^{\land}\varphi}{\varphi}\\

-(\bm{Rp})^{\land}

∂φ∂(Rp)​​φ→0lim​φexp(φ∧)exp(ϕ∧)p−exp(ϕ∧)p​φ→0lim​φ(Iφ∧)exp(ϕ∧)p−exp(ϕ∧)p​φ→0lim​φφ∧exp(ϕ∧)p​φ→0lim​φφ∧Rp​φ→0lim​φ−(Rp)∧φ​−(Rp)∧​

4.3.5

Δ\bm{\xi}[Δ\bm{\rho},Δ\bm{\phi}]^T

Δξ[Δρ,Δϕ]T则

\begin{align*}\frac{\partial(\bm{Tp})}{\partial{Δ\bm{\xi}}}

\lim\limits_{Δ\bm{\xi}\to0}\frac{\exp(Δ\bm{\xi}^{\land})\exp(\bm{\xi}^{\land})\bm{p}-\exp(\bm{\xi}^{\land})\bm{p}}{Δ\bm{\xi}}

\lim\limits_{Δ\bm{\xi}\to0}\frac{(\bm{I}

Δ\bm{\xi}^{\land})\exp(\bm{\xi}^{\land})\bm{p}-\exp(\bm{\xi}^{\land})\bm{p}}{Δ\bm{\xi}}

\lim\limits_{Δ\bm{\xi}\to0}\frac{Δ\bm{\xi}^{\land}\exp(\bm{\xi}^{\land})\bm{p}}{Δ\bm{\xi}}

Δ\bm{\phi}^{\land}

\Delta\bm{\phi}^{\land}(\bm{Rpt})\Delta\bm{\rho}\\

\bm{0}^T

\end{bmatrix}}{[Δ\bm{\rho},Δ\bm{\phi}]^T}

\begin{bmatrix}

\overset{\mathrm{def}}{}(\bm{Tp})^{\bigodot}

\end{align*}

∂Δξ∂(Tp)​​Δξ→0lim​Δξexp(Δξ∧)exp(ξ∧)p−exp(ξ∧)p​Δξ→0lim​Δξ(IΔξ∧)exp(ξ∧)p−exp(ξ∧)p​Δξ→0lim​ΔξΔξ∧exp(ξ∧)p​Δξ→0lim​Δξ[Δϕ∧0T​Δρ0​][Rpt1​]​Δξ→0lim​[Δρ,Δϕ]T[Δϕ∧(Rpt)Δρ0T​]​[I0T​−(Rpt)∧0T​]def(Tp)⨀​

$\overset{\mathrm{def}}{}(\bm{Tp})^{\bigodot}$4.4

Sophus应用

2.8)project(useSophus)find_package(Sophus

REQUIRED)

${Sophus_INCLUDE_DIRS})add_executable(useSophus

useSophus.cpp)

target_link_libraries(useSophus

${Sophus_LIBRARIES})#includeiostream

#includecmath

1)).toRotationMatrix();Sophus::SO3

SO3_R(R);Vector3d

Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose()

endl;Vector3d

Sophus::SE3::vee(Sophus::SE3::hat(se3)).transpose()

endl;//

更新量update_se3.setZero();update_se3(0,

1e-4;Sophus::SE3

ATE_{all}\sqrt{\frac{1}{N}\sum\limits_{i1}^{N}||log(\bm{T}_{gt,i}^{-1}\bm{T}_{esti,i})^{\vee}||_2^2}

ATEall​N1​i1∑N​∣∣log(Tgt,i−1​Testi,i​)∨∣∣22​

每个位姿

ATE_{trans}\sqrt{\frac{1}{N}\sum\limits_{i1}^{N}||trans(\bm{T}_{gt,i}^{-1}\bm{T}_{esti,i})||_2^2}

ATEtrans​N1​i1∑N​∣∣trans(Tgt,i−1​Testi,i​)∣∣22​

trans

RPE_{all}\sqrt{\frac{1}{N-\Delta

t}||log((\bm{T}_{gt,i}^{-1}\bm{T}_{gt,i\Delta

t})^{-1}(\bm{T}_{esti,i}^{-1}\bm{T}_{esti,i\Delta

t}))^{\vee}||_2^2}

RPEall​N−Δt1​i1∑N−Δt​∣∣log((Tgt,i−1​Tgt,iΔt​)−1(Testi,i−1​Testi,iΔt​))∨∣∣22​

RPE_{trans}\sqrt{\frac{1}{N-\Delta

t}||trans((\bm{T}_{gt,i}^{-1}\bm{T}_{gt,i\Delta

t})^{-1}(\bm{T}_{esti,i}^{-1}\bm{T}_{esti,i\Delta

t}))||_2^2}

RPEtrans​N−Δt1​i1∑N−Δt​∣∣trans((Tgt,i−1​Tgt,iΔt​)−1(Testi,i−1​Testi,iΔt​))∣∣22​

————————

./trajectoryErrorCMakeLists.txt

2.8)project(trajectoryError)find_package(Sophus

REQUIRED)

${Sophus_INCLUDE_DIRS})option(USE_UBUNTU_20

Set

include_directories(${Pangolin_INCLUDE_DIRS})add_executable(trajectoryError

trajectoryError.cpp)

target_link_libraries(trajectoryError

${Sophus_LIBRARIES})

target_link_libraries(trajectoryError

${FMT_LIBRARIES})

Eigen::aligned_allocatorSophus::SE3

TrajectoryType;void

ReadTrajectory(groundtruth_file);TrajectoryType

estimated

ReadTrajectory(estimated_file);assert(!groundtruth.empty()

!estimated.empty());assert(groundtruth.size()

estimated.size());//

endl;DrawTrajectory(groundtruth,

estimated);return

tz));trajectory.push_back(p1);}return

trajectory;

trajectorypangolin::CreateWindowAndBind(Trajectory

Viewer,

768);glEnable(GL_DEPTH_TEST);glEnable(GL_BLEND);glBlendFunc(GL_SRC_ALPHA,

GL_ONE_MINUS_SRC_ALPHA);pangolin::OpenGlRenderState

s_cam(pangolin::ProjectionMatrix(1024,

768,

1000),pangolin::ModelViewLookAt(0,

-0.1,

pangolin::CreateDisplay().SetBounds(0.0,

1.0,

pangolin::Handler3D(s_cam));while

false)

GL_DEPTH_BUFFER_BIT);d_cam.Activate(s_cam);glClearColor(1.0f,

1.0f,

1];glVertex3d(p1.translation()[0],

p1.translation()[1],

p1.translation()[2]);glVertex3d(p2.translation()[0],

p2.translation()[1],

p2.translation()[2]);glEnd();}for

(size_t

estimatedglBegin(GL_LINES);auto

esti[i],

1];glVertex3d(p1.translation()[0],

p1.translation()[1],

p1.translation()[2]);glVertex3d(p2.translation()[0],

p2.translation()[1],

p2.translation()[2]);glEnd();}pangolin::FinishFrame();usleep(5000);

sleep

\bm{p}^{\prime}\begin{bmatrix}s\bm{R}

\bm{t}\\

原式等效于证明Rp∧RTRRp∧IRp∧对于向量v∈R3Rp∧v上式等号左边等号右边表示证毕。

​(Rp)∧R(Rp)∧R(Rp)∧R(Rp)∧Rv表示向量p,v叉乘后所得向量根据R旋转。

向量p,v分别根据R旋转后叉乘显然得到同一个向量。

​

根据题

\sum\limits_{n0}^{\infty}\frac{(\bm{Rp^{\land}R^T})

^n}{N!}

\sum\limits_{n0}^{\infty}\frac{\bm{Rp^{\land}R^T}·\bm{Rp^{\land}R^T}·····\bm{Rp^{\land}R^T}·\bm{Rp^{\land}R^T}}{N!}

\bm{R^TRI}\\

\sum\limits_{n0}^{\infty}\frac{\bm{R(p^{\land})^nR^T}}{{N!}

}\\

\sum\limits_{n0}^{\infty}\frac{\bm{(p^{\land})^n}}{{N!}

\bm{R}^T

\bm{R}\exp(\bm{p}^{\land})\bm{R}^T

\end{align*}

根据题5的结论exp((Rp)∧)级数展开其中RTRI​(Rp)∧Rp∧RTexp(Rp∧RT)n0∑∞​N!(Rp∧RT)n​n0∑∞​N!Rp∧RT⋅Rp∧RT⋅⋅⋅⋅⋅Rp∧RT⋅Rp∧RT​n0∑∞​N!R(p∧)nRT​Rn0∑∞​N!(p∧)n​RTRexp(p∧)RT​

——————

\bm{T}\exp(\bm{\xi}^{\land})\bm{T}^{-1}

\bm{T}\sum\limits_{n0}^{\infty}\frac{(\bm{\xi}^{\land})^n}{n!}\bm{T}^{-1}

\bm{T^{-1}TI}

\sum\limits_{n0}^{\infty}\frac{\bm{T}\bm{\xi}^{\land}\bm{T}^{-1}·\bm{T}\bm{\xi}^{\land}\bm{T}^{-1}·\bm{T}\bm{\xi}^{\land}\bm{T}^{-1}·\bm{T}\bm{\xi}^{\land}\bm{T}^{-1}·,···,

\bm{T}\bm{\xi}^{\land}\bm{T}^{-1}·\bm{T}\bm{\xi}^{\land}\bm{T}^{-1}}{n!}

\sum\limits_{n0}^{\infty}\frac{(\bm{T}\bm{\xi}^{\land}\bm{T}^{-1})^n}{n!}

\exp(\bm{T}\bm{\xi}^{\land}\bm{T}^{-1})

\end{align*}

Texp(ξ∧)T−1由于T−1TI​Tn0∑∞​n!(ξ∧)n​T−1n0∑∞​n!Tξ∧T−1⋅Tξ∧T−1⋅Tξ∧T−1⋅Tξ∧T−1⋅,⋅⋅⋅,Tξ∧T−1⋅Tξ∧T−1​n0∑∞​n!(Tξ∧T−1)n​exp(Tξ∧T−1)​

\bm{\xi}\begin{bmatrix}\bm{\rho}\\

\bm{\phi}

\end{bmatrix}\bm{\xi}^{\land}\begin{bmatrix}\bm{\phi}^{\land}

\bm{\rho}

\end{bmatrix}\bm{T}\begin{bmatrix}\bm{R}

\bm{t}\\

\end{bmatrix}\bm{T}^{-1}\begin{bmatrix}\bm{R}^T

-\bm{R}^T\bm{t}\\

ξ[ρϕ​]ξ∧[ϕ∧0T​ρ0​]T[R0T​t1​]T−1[RT0T​−RTt1​]

进行转换

\begin{align*}\bm{T}\bm{\xi}^{\land}\bm{T}^{-1}

\bm{t}\\

\end{bmatrix}\begin{bmatrix}\bm{\phi}^{\land}

\bm{\rho}

\end{bmatrix}\begin{bmatrix}\bm{R}^T

-\bm{R}^T\bm{t}\\

\begin{bmatrix}\bm{R}\bm{\phi}^{\land}

\bm{R\rho}\\

\end{bmatrix}\begin{bmatrix}\bm{R}^T

-\bm{R}^T\bm{t}\\

\begin{bmatrix}\bm{R}\bm{\phi}^{\land}\bm{R}^T

-\bm{R}\bm{\phi}^{\land}\bm{R}^T\bm{t

\bm{R\rho}}\\

\begin{bmatrix}(\bm{R}\bm{\phi})^{\land}

-(\bm{R}\bm{\phi})^{\land}\bm{t

\bm{R\rho}}\\

\begin{bmatrix}-(\bm{R}\bm{\phi})^{\land}\bm{t

\bm{R\rho}}\\

\begin{bmatrix}\bm{t}^{\land}\bm{R}\bm{\phi}

\bm{R\rho}\\

\end{bmatrix}\begin{bmatrix}\bm{\rho}\\

\bm{\phi}

Tξ∧T−1由题5的结论由叉乘性质​[R0T​t1​][ϕ∧0T​ρ0​][RT0T​−RTt1​][Rϕ∧0T​Rρ0​][RT0T​−RTt1​][Rϕ∧RT0T​−Rϕ∧RTtRρ0​]Rp∧RT(Rp)∧[(Rϕ)∧0T​−(Rϕ)∧tRρ0​]对比ξ,ξ∧进行转换[−(Rϕ)∧tRρRϕ​]∧[t∧RϕRρRϕ​]∧[R0​t∧RR​][ρϕ​])∧(Ad(T)ξ)∧​

exp

\bm{T}\exp(\bm{\xi}^{\land})\bm{T}^{-1}\exp((Ad(\bm{T})\bm{\xi})^{\land})

Ad(\bm{T})\begin{bmatrix}\bm{R}

\bm{t}^{\land}\bm{R}

\begin{align*}\frac{\partial(\bm{Rp})}{\partial\varphi}

\lim\limits_{\varphi\to0}\frac{\exp(\phi^{\land})\exp(\varphi^{\land})\bm{p}-\exp(\phi^{\land})\bm{p}}{\varphi}\\

\lim\limits_{\varphi\to0}\frac{\exp(\phi^{\land})(\bm{I}

\varphi^{\land})\bm{p}-\exp(\phi^{\land})\bm{p}}{\varphi}\\

\lim\limits_{\varphi\to0}\frac{\exp(\phi^{\land})\varphi^{\land}\bm{p}}{\varphi}\\

\lim\limits_{\varphi\to0}\frac{\bm{R}\varphi^{\land}\bm{p}}{\varphi}\\

由题5

\lim\limits_{\varphi\to0}\frac{(\bm{R}\varphi)^{\land}\bm{Rp}}{\varphi}\\

由叉乘性质\\

\lim\limits_{\varphi\to0}\frac{-(\bm{Rp})^{\land}\bm{R}\varphi}{\varphi}\\

\end{align*}

∂φ∂(Rp)​由题5:Rp∧由叉乘性质​φ→0lim​φexp(ϕ∧)exp(φ∧)p−exp(ϕ∧)p​φ→0lim​φexp(ϕ∧)(Iφ∧)p−exp(ϕ∧)p​φ→0lim​φexp(ϕ∧)φ∧p​φ→0lim​φRφ∧p​(Rp)∧Rφ→0lim​φ(Rφ)∧Rp​φ→0lim​φ−(Rp)∧Rφ​−(Rp)∧R​

SE(3):

Δ\bm{\xi}[Δ\bm{\rho},Δ\bm{\phi}]^T

Δξ[Δρ,Δϕ]T则

\begin{align*}\frac{\partial(\bm{Tp})}{\partial{Δ\bm{\xi}}}

\lim\limits_{Δ\bm{\xi}\to0}\frac{\exp(\bm{\xi}^{\land})\exp(Δ\bm{\xi}^{\land})\bm{p}-\exp(\bm{\xi}^{\land})\bm{p}}{Δ\bm{\xi}}

\lim\limits_{Δ\bm{\xi}\to0}\frac{\exp(\bm{\xi}^{\land})(\bm{I}

Δ\bm{\xi}^{\land})\bm{p}-\exp(\bm{\xi}^{\land})\bm{p}}{Δ\bm{\xi}}

\lim\limits_{Δ\bm{\xi}\to0}\frac{\exp(\bm{\xi}^{\land})Δ\bm{\xi}^{\land}\bm{p}}{Δ\bm{\xi}}

\bm{R}

\end{bmatrix}\bm{p}}{Δ\bm{\xi}}\\

\bm{R}

\bm{R}\Delta\bm{\phi}^{\land}\bm{p}

\bm{0}^T

\end{bmatrix}}{[Δ\bm{\rho},Δ\bm{\phi}]^T}

提出来方便求导\\

(\bm{R}\Delta\bm{\phi})^{\land}\bm{R}\bm{p}\bm{R}\Delta\bm{\rho}\\

\bm{0}^T

\end{bmatrix}}{[Δ\bm{\rho},Δ\bm{\phi}]^T}

-(\bm{R}\bm{p})^{\land}\bm{R}\Delta\bm{\phi}

\bm{0}^T

\end{bmatrix}}{[Δ\bm{\rho},Δ\bm{\phi}]^T}

\begin{bmatrix}

∂Δξ∂(Tp)​把p前的将​Δξ→0lim​Δξexp(ξ∧)exp(Δξ∧)p−exp(ξ∧)p​Δξ→0lim​Δξexp(ξ∧)(IΔξ∧)p−exp(ξ∧)p​Δξ→0lim​Δξexp(ξ∧)Δξ∧p​Δξ→0lim​Δξ[R0​t1​][Δϕ∧0T​Δρ0​]p​矩阵当做旋转矩阵结合向量旋转的性质Δξ→0lim​Δξ[R0​t1​][Δϕ∧pΔρ0​]​Δξ→0lim​[Δρ,Δϕ]T[RΔϕ∧pRΔρ0T​]​Δρ,Δϕ提出来方便求导根据Rp∧(Rp)∧RΔξ→0lim​[Δρ,Δϕ]T[(RΔϕ)∧RpRΔρ0T​]​Δξ→0lim​[Δρ,Δϕ]T[−(Rp)∧RΔϕRΔρ0T​]​[R0T​−(Rp)∧R0T​]​

——————————————