从零手写VIO——第一讲作业

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 其他导数

使用右乘$\mathfrak{s}\mathfrak{o}(3)$,推导以下导数

$\frac{d({\mathbf{R}}^{-1}p)}{d\mathbf{R}}$

$$\begin{array}{rl}\frac{d({\mathbf{R}}^{-1}p)}{d\mathbf{R}}=& \underset{\varphi \to 0}{\lim }\frac{(\mathbf{R}\exp ({\varphi }^{\wedge }){)}^{-1}p-{\mathbf{R}}^{-1}p}{\varphi }\\ =& \underset{\varphi \to 0}{\lim }\frac{\exp (-{\varphi }^{\wedge }){\mathbf{R}}^{-1}p-{\mathbf{R}}^{-1}p}{\varphi }\\ =& \underset{\varphi \to 0}{\lim }\frac{(I-{\varphi }^{\wedge }){\mathbf{R}}^{-1}p-{\mathbf{R}}^{-1}p}{\varphi }\\ =& \underset{\varphi \to 0}{\lim }\frac{-{\varphi }^{\wedge }{\mathbf{R}}^{-1}p}{\varphi }\\ =& \underset{\varphi \to 0}{\lim }\frac{({\mathbf{R}}^{-1}p{)}^{\wedge }\varphi }{\varphi }\\ =& ({\mathbf{R}}^{-1}p{)}^{\wedge }\end{array}$$
其中

• 第一行到第二行：
  因为$\mathbf{R},\exp ({\varphi }^{\wedge })$本质上都是旋转矩阵，其分别满足正交性，其乘积亦满足正交性，故逆运算等价于转置运算那么存在：
  $$(\mathbf{R}\exp ({\varphi }^{\wedge }){)}^{-1}=(\mathbf{R}\exp ({\varphi }^{\wedge }){)}^{\mathbf{T}}=\exp {({\varphi }^{\wedge })}^{\mathbf{T}}{\mathbf{R}}^{\mathbf{T}}=\exp {({\varphi }^{\wedge })}^{-1}{\mathbf{R}}^{-1}$$
• 第二行到第三行
  我们对$\exp (-{\varphi }^{\wedge })$进行泰勒展开，得到$\exp (-{\varphi }^{\wedge })=I+(-{\varphi }^{\wedge })$
• 第四行到第五行
  反对称符号$\wedge$具有叉乘含义，同时也有类似于叉乘的性质，即$a\times b=-b\times a=a^{\wedge }b=-b^{\wedge }a$

$\frac{d\ln ({\mathbf{R}}_1{{\mathbf{R}}_2}^{-1}{)}^{\vee }}{d{\mathbf{R}}_2}$

$$\begin{array}{rl}\frac{d\ln ({\mathbf{R}}_1{{\mathbf{R}}_2}^{-1}{)}^{\vee }}{d{\mathbf{R}}_2}& =\underset{\varphi \to 0}{\lim }\frac{\ln ({\mathbf{R}}_1({\mathbf{R}}_2\exp ({\varphi }^{\wedge }){)}^{-1}{)}^{\vee }-\ln ({\mathbf{R}}_1{{\mathbf{R}}_2}^{-1}{)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln ({\mathbf{R}}_1\exp (-{\varphi }^{\wedge }){{\mathbf{R}}_2}^{-1}{)}^{\vee }-\ln {({\mathbf{R}}_1{{\mathbf{R}}_2}^{-1})}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln ({\mathbf{R}}_1{{\mathbf{R}}_2}^{-1}{\mathbf{R}}_2\exp (-{\varphi }^{\wedge }){{\mathbf{R}}_2}^{\mathbf{T}}{)}^{\vee }-\ln {({\mathbf{R}}_1{{\mathbf{R}}_2}^{-1})}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln ({\mathbf{R}}_1{{\mathbf{R}}_2}^{-1}\exp ((-{\mathbf{R}}_2\varphi {)}^{\wedge }){)}^{\vee }-\ln {({\mathbf{R}}_1{{\mathbf{R}}_2}^{-1})}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\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\varphi )-\ln {({\mathbf{R}}_1{{\mathbf{R}}_2}^{-1})}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{-{\mathbf{J}}_r^{-1}(\ln ({\mathbf{R}}_1{{\mathbf{R}}_2}^{-1}{)}^{\vee }){\mathbf{R}}_2\varphi }{\varphi }\\ & =-{\mathbf{J}}_r^{-1}(\ln ({\mathbf{R}}_1{{\mathbf{R}}_2}^{-1}{)}^{\vee }){\mathbf{R}}_2\end{array}$$

• 第三行的变换：存在如下伴随性质
  $$\mathbf{R}\exp ({\varphi }^{\wedge }){\mathbf{R}}^{\mathbf{T}}=\exp ((\mathbf{R}\varphi {)}^{\wedge })$$
• 第四行到第五行的变换，由于
  $$\ln (\mathbf{R}\exp ({\varphi }^{\wedge }){)}^{\vee }=\ln {\mathbf{R}}^{\vee }+{\mathbf{J}}_r^{-1}(\ln {\mathbf{R}}^{\vee })\varphi$$