2 四元数和李代数更新
hw_ch1.cpp
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
int main(int argc, char** argv)
{
Eigen::AngleAxisd rotation_vector = Eigen::AngleAxisd (M_PI/4, Eigen::Vector3d(0, 0, 1));
// RotationMatrix
Eigen::Matrix3d R = rotation_vector.toRotationMatrix();
cout << "The RotationMatrix is \n" << R << endl;
Eigen::Vector3d w = Eigen::Vector3d (0.01, 0.02, 0.03);
double theta = w.norm();
Eigen::Vector3d a = w / theta;
Eigen::Matrix3d exp_w_hat;
Eigen::Matrix3d a_hat;
a_hat << 0, -a(2), a(1),
a(2), 0, -a(0),
-a(1), a(0), 0;
exp_w_hat = cos(theta) * Eigen::Matrix3d::Identity() + (1 - cos(theta)) * a * a.transpose() + sin(theta) * a_hat;
Eigen::Matrix3d updated_R;
updated_R = R * exp_w_hat;
cout << "The updated RotationMatrix is \n" << updated_R << endl;
// quaterniond
Eigen::Quaterniond q = Eigen::Quaterniond (rotation_vector);
cout << "The quaternion is \n" << q.coeffs() << endl;
cout << "The matrix is \n" << q.toRotationMatrix() << endl;
Eigen::Quaterniond q_w (1.0, w(0)/2, w(1)/2, w(2)/2);
Eigen::Quaterniond q_updated = q * q_w;
cout << "The updated quaternion is \n" << q_updated.coeffs() << endl;
cout << "The updated matrix is \n" << q_updated.toRotationMatrix() << endl;
Eigen::Matrix3d error_matrix = updated_R - q_updated.toRotationMatrix();
cout << "The error between matrix and quaternion is \n" << error_matrix << endl;
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(homework)
include_directories("/usr/include/eigen3")
add_executable(hw_ch1 hw_ch1.cpp)
3 其他导数
使用右乘 s o ( 3 ) \mathfrak{so}(3) so(3),推导以下导数
d ( R − 1 p ) d R \frac{d(\mathbf{R}^{-1}p)}{d\mathbf{R}} dRd(R−1p)
d
(
R
−
1
p
)
d
R
=
lim
ϕ
→
0
(
R
exp
(
ϕ
∧
)
)
−
1
p
−
R
−
1
p
ϕ
=
lim
ϕ
→
0
exp
(
−
ϕ
∧
)
R
−
1
p
−
R
−
1
p
ϕ
=
lim
ϕ
→
0
(
I
−
ϕ
∧
)
R
−
1
p
−
R
−
1
p
ϕ
=
lim
ϕ
→
0
−
ϕ
∧
R
−
1
p
ϕ
=
lim
ϕ
→
0
(
R
−
1
p
)
∧
ϕ
ϕ
=
(
R
−
1
p
)
∧
\begin{aligned} \frac{d(\mathbf{R}^{-1}p)}{d\mathbf{R}} =& \lim_{\phi \to 0} \frac{(\mathbf{R}\exp{(\phi^{\land})})^{-1}p - \mathbf{R}^{-1}p}{\phi} \\ =& \lim_{\phi\to0} \frac{\exp{(-\phi^{\land})}\mathbf{R}^{-1}p-\mathbf{R}^{-1}p}{\phi} \\ =& \lim_{\phi\to 0} \frac{(I-\phi^{\land})\mathbf{R}^{-1}p-\mathbf{R}^{-1}p}{\phi} \\ =& \lim_{\phi \to 0} \frac{-\phi^{\land}\mathbf{R}^{-1}p}{\phi} \\ =& \lim_{\phi \to 0} \frac{(\mathbf{R}^{-1}p)^{\land}\phi}{\phi} \\ =& (\mathbf{R}^{-1}p)^{\land} \end{aligned}
dRd(R−1p)======ϕ→0limϕ(Rexp(ϕ∧))−1p−R−1pϕ→0limϕexp(−ϕ∧)R−1p−R−1pϕ→0limϕ(I−ϕ∧)R−1p−R−1pϕ→0limϕ−ϕ∧R−1pϕ→0limϕ(R−1p)∧ϕ(R−1p)∧
其中
- 第一行到第二行:
因为 R , exp ( ϕ ∧ ) \mathbf{R},\exp(\phi^{\land}) R,exp(ϕ∧)本质上都是旋转矩阵,其分别满足正交性,其乘积亦满足正交性,故逆运算等价于转置运算那么存在:
( R exp ( ϕ ∧ ) ) − 1 = ( R exp ( ϕ ∧ ) ) T = exp ( ϕ ∧ ) T R T = exp ( ϕ ∧ ) − 1 R − 1 (\mathbf{R}\exp{(\phi^{\land})})^{-1} = (\mathbf{R}\exp{(\phi^{\land})})^{\mathbf{T}} = \exp{(\phi^{\land})}^{\mathbf{T}}\mathbf{R}^{\mathbf{T}} =\exp{(\phi^{\land})}^{-1}\mathbf{R}^{-1} (Rexp(ϕ∧))−1=(Rexp(ϕ∧))T=exp(ϕ∧)TRT=exp(ϕ∧)−1R−1 - 第二行到第三行
我们对 exp ( − ϕ ∧ ) \exp{(-\phi^{\land})} exp(−ϕ∧)进行泰勒展开,得到 exp ( − ϕ ∧ ) = I + ( − ϕ ∧ ) \exp{(-\phi^{\land})} = I+(-\phi^{\land}) exp(−ϕ∧)=I+(−ϕ∧) - 第四行到第五行
反对称符号 ∧ \land ∧具有叉乘含义,同时也有类似于叉乘的性质,即 a × b = − b × a = a ∧ b = − b ∧ a a\times b=-b\times a =a^{\land}b=-b^{\land}a a×b=−b×a=a∧b=−b∧a
d ln ( R 1 R 2 − 1 ) ∨ d R 2 \frac{d\ln{(\mathbf{R_1}\mathbf{R_2}^{-1})^{\vee}}}{d\mathbf{R_2}} dR2dln(R1R2−1)∨
d ln ( R 1 R 2 − 1 ) ∨ d R 2 = lim ϕ → 0 ln ( R 1 ( R 2 exp ( ϕ ∧ ) ) − 1 ) ∨ − ln ( R 1 R 2 − 1 ) ∨ ϕ = lim ϕ → 0 ln ( R 1 exp ( − ϕ ∧ ) R 2 − 1 ) ∨ − ln ( R 1 R 2 − 1 ) ∨ ϕ = lim ϕ → 0 ln ( R 1 R 2 − 1 R 2 exp ( − ϕ ∧ ) R 2 T ) ∨ − ln ( R 1 R 2 − 1 ) ∨ ϕ = lim ϕ → 0 ln ( R 1 R 2 − 1 exp ( ( − R 2 ϕ ) ∧ ) ) ∨ − ln ( R 1 R 2 − 1 ) ∨ ϕ = lim ϕ → 0 ln ( R 1 R 2 − 1 ) ∨ + J r − 1 ( ln ( R 1 R 2 − 1 ) ∨ ) ( − R 2 ϕ ) − ln ( R 1 R 2 − 1 ) ∨ ϕ = lim ϕ → 0 − J r − 1 ( ln ( R 1 R 2 − 1 ) ∨ ) R 2 ϕ ϕ = − J r − 1 ( ln ( R 1 R 2 − 1 ) ∨ ) R 2 \begin{aligned} \frac{d\ln{(\mathbf{R_1}\mathbf{R_2}^{-1})^{\vee}}}{d\mathbf{R_2}} &= \lim_{\phi \to 0} \frac{\ln(\mathbf{R_1}(\mathbf{R_2}\exp{(\phi^{\land})})^{-1})^{\vee}-\ln{(\mathbf{R_1}\mathbf{R_2}^{-1})^{\vee}}}{\phi} \\ &= \lim_{\phi \to 0} \frac{\ln(\mathbf{R_1}\exp{(-\phi^{\land})\mathbf{R_2}^{-1}})^{\vee}-\ln{(\mathbf{R_1}\mathbf{R_2}^{-1})}^{\vee}}{\phi} \\ &=\lim_{\phi \to 0} \frac{\ln(\mathbf{R_1}\mathbf{R_2}^{-1}\mathbf{R_2}\exp{(-\phi^{\land})\mathbf{R_2}^{\mathbf{T}}})^{\vee}-\ln{(\mathbf{R_1}\mathbf{R_2}^{-1})}^{\vee}}{\phi} \\ &= \lim_{\phi \to 0} \frac{\ln(\mathbf{R_1}\mathbf{R_2}^{-1}\exp{((-\mathbf{R_2}\phi)^{\land})})^{\vee}-\ln{(\mathbf{R_1}\mathbf{R_2}^{-1})}^{\vee}}{\phi} \\ &= \lim_{\phi \to 0} \frac{\ln(\mathbf{R_1}\mathbf{R_2}^{-1})^{\vee}+\mathbf{J}_r^{-1}(\ln(\mathbf{R_1}\mathbf{R_2}^{-1})^{\vee})(-\mathbf{R_2}\phi)-\ln{(\mathbf{R_1}\mathbf{R_2}^{-1})}^{\vee}}{\phi} \\ &= \lim_{\phi \to 0} \frac{-\mathbf{J}_r^{-1}(\ln(\mathbf{R_1}\mathbf{R_2}^{-1})^{\vee})\mathbf{R_2}\phi}{\phi}\\ &= -\mathbf{J}_r^{-1}(\ln(\mathbf{R_1}\mathbf{R_2}^{-1})^{\vee})\mathbf{R_2} \end{aligned} dR2dln(R1R2−1)∨=ϕ→0limϕln(R1(R2exp(ϕ∧))−1)∨−ln(R1R2−1)∨=ϕ→0limϕln(R1exp(−ϕ∧)R2−1)∨−ln(R1R2−1)∨=ϕ→0limϕln(R1R2−1R2exp(−ϕ∧)R2T)∨−ln(R1R2−1)∨=ϕ→0limϕln(R1R2−1exp((−R2ϕ)∧))∨−ln(R1R2−1)∨=ϕ→0limϕln(R1R2−1)∨+Jr−1(ln(R1R2−1)∨)(−R2ϕ)−ln(R1R2−1)∨=ϕ→0limϕ−Jr−1(ln(R1R2−1)∨)R2ϕ=−Jr−1(ln(R1R2−1)∨)R2
- 第三行的变换:存在如下伴随性质
R exp ( ϕ ∧ ) R T = exp ( ( R ϕ ) ∧ ) \mathbf{R}\exp(\phi^{\land})\mathbf{R}^{\mathbf{T}}=\exp((\mathbf{R}\phi)^{\land}) Rexp(ϕ∧)RT=exp((Rϕ)∧) - 第四行到第五行的变换,由于
ln ( R exp ( ϕ ∧ ) ) ∨ = ln R ∨ + J r − 1 ( ln R ∨ ) ϕ \ln(\mathbf{R}\exp(\phi^{\land}))^{\vee}=\ln\mathbf{R}^{\vee}+\mathbf{J}_r^{-1}(\ln\mathbf{R}^{\vee})\phi ln(Rexp(ϕ∧))∨=lnR∨+Jr−1(lnR∨)ϕ