If you wish for peace, prepare for war

彻底解决 sudo rosdep init 和 rosdep update失败问题 2021-09-14|ROSROS Kinetic知识

如果sudo rosdep init 和 rosdep update没有执行成功，有些ROS功能会无法执行，比如rqt_tf_tree会报错：

1 2	the rosdep view is empty: call 'sudo rosdep init' and 'rosdep update' [WARN] wait_for_service(/rqt_gui_py_node_26204/tf2_frames): failed to contact, will keep trying

这两个老大难其实都是网络的问题，sudo rosdep init的目的是为了在/etc目录下新建/ros/rosdep/sources.list.d/20-default.list文件。

小鱼基于rosdep源码制作了rosdepc，专门服务国内ROS用户，使用pip3 和 rosdepc 可以彻底消灭这个问题

sudo apt-get install python3-pip 
sudo pip3 install rosdepc

sudo rosdepc init
rosdepc update

rosdep update根据20-default.list文件中的网址链接去下载相应的文件，最终在/etc/ros/rosdep下放5个文件，分别是base.yaml,gentoo.yaml,osx-homebrew.yaml,python.yaml,ruby.yaml. 但又做了一些配置，所以不是下载文件能解决的。

以下步骤先从网上下载rosdistro文件夹，放到/etc/ros

执行`sudo rosdep init`后报错

1 2	ERROR: cannot download default sources list from: https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/sources.list.d/20-default.list Website may be down

从网上直接下载这个文件，然后放到/etc/ros/rosdep/sources.list.d，再修改如下:

# os-specific listings first

yaml file:///etc/ros/rosdistro/rosdep/osx-homebrew.yaml osx

# generic
yaml file:///etc/ros/rosdistro/rosdep/base.yaml
yaml file:///etc/ros/rosdistro/rosdep/python.yaml
yaml file:///etc/ros/rosdistro/rosdep/ruby.yaml
gbpdistro file:///etc/ros/rosdistro/releases/fuerte.yaml fuerte

# newer distributions (Groovy, Hydro, ...) must not be listed anymore, they are being fetched from the rosdistro index.yaml instead

rosdep update 报错

kinetic 要修改python2.7的路径, melodic和noetic要修改python3 ，当然另一种方法是从其他电脑拷贝已经生成的文件。

kinetic 的修改

1 2	reading in sources list data from /etc/ros/rosdep/sources.list.d ERROR: unable to process source

对下面三个文件做修改

/usr/lib/python2.7/dist-packages/rosdep2/gbpdistro_support.py
修改: FUERTE_GBPDISTRO_URL = 'file:///etc/ros/rosdistro/releases/fuerte.yaml'


/usr/lib/python2.7/dist-packages/rosdep2/rep3.py
修改: REP3_TARGETS_URL = 'file:///etc/ros/rosdistro/releases/targets.yaml'


/usr/lib/python2.7/dist-packages/rosdistro/__init__.py
修改: DEFAULT_INDEX_URL = 'file:///etc/ros/rosdistro/index-v4.yaml'

启动新终端，再次执行rosdep update

melodic和noetic 的修改：clone代码仓https://github.com/ros/rosdistro到本地，并更改其文件rosdep/sources.list.d/20-default.list，将其url改成本地文件路径，内容类似如下：

# os-specific listings first
yaml file:///home/baby/rosdistro/rosdep/osx-homebrew.yaml osx

# generic
yaml file:///home/baby/rosdistro/rosdep/base.yaml
yaml file:///home/baby/rosdistro/rosdep/python.yaml
yaml file:///home/baby/rosdistro/rosdep/ruby.yaml
gbpdistro file:///home/baby/rosdistro/releases/fuerte.yaml fuerte

由于rosdep使用python3，可直接该动源码。我们需要改动三个文件：

/usr/lib/python3/dist-packages/rosdep2/sources_list.py，修改 DEFAULT_SOURCES_LIST_URL = 'file:///home/baby/rosdistro/rosdep/sources.list.d/20-default.list'
/usr/lib/python3/dist-packages/rosdep2/rep3.py，修改 REP3_TARGETS_URL = 'file:///home/baby/rosdistro/releases/targets.yaml'
/usr/lib/python3/dist-packages/rosdep2/gbpdistro_support.py，修改 FUERTE_GBPDISTRO_URL = 'file:///home/baby/rosdistro/releases/fuerte.yaml'
/usr/lib/python3/dist-packages/rosdistro/__init__.py，修改 DEFAULT_INDEX_URL = 'file:///home/baby/rosdistro/index-v4.yaml'

完成后先把/etc/ros/rosdep/sources.list.d/20-default.list删除，再执行sudo rosdep init与rosdep update就可以成功了。

参考:
解决 rosdep update

emplace 和 emplace_back 2021-08-02|C++C++ 模板与STL

常见用法：

vector<int> v;
for(int i=0; i<4; i++)
{
    v.emplace_back();
    auto& num = v.back();
    num = i;
    qDebug()<< "size: "<<v.size() <<"  v.back: "<<v.back();
}

结果

size:  1   v.back:  0
size:  2   v.back:  1
size:  3   v.back:  2
size:  4   v.back:  3

#include <iostream>
#include <map>
#include <string>
#include <vector>
using namespace std;

class A{
public:
    A(const string& s) {cout << "A construct " <<endl;}
    A(const A& a) {cout <<"A copy"<<endl; }
private:
    string s;
};

int main()
{
    std::map<int, string> m;
    vector<A> v;
    A a1("test");  // a1 构造函数
    cout << "------------------" << endl;
    v.push_back(a1);   // a1 拷贝构造函数
    v.emplace_back(a1);
   
    //先运行构造函数，后拷贝构造函数
    v.push_back(A("test") );
    v.emplace_back(A("test") );

    // v.push_back("test");  报错
    // 这个才是真正的用途
    v.emplace_back("test");   // 隐式转换，只有拷贝构造函数
    cout << "******************" << endl;
    return 0;
}

对于C++ 11里vector的emplace_back函数比较失望，都说提高了效率，其实它仅对于元素做隐式转换的情况有效，此时没有产生临时对象。对其他情况，和push_back没区别。

这里用到的c++11特性完美转发：将接收下来的参数原样完美地传递给对象的构造函数，这带来另一个方便性就是即使是构造函数声明为 explicit 它还是可以正常工作，因为它不存在临时变量和隐式转换。

map::emplace

map就只有emplace，机制也是一样的。元素是直接构建的，既不复制也不移动，仅当键不存在时才插入。
但是map有个问题：emplace 方法把它接收到的所有的参数都一起转发给 pair 的构造函数。但是对于一个 pair 来说，它既需要构造它的 key 又需要构造它的 value。如果我们按照之前普通的语法使用变参模板的话，则它是无法区分哪些参数用来构造 key, 哪些用来构造 value的。比如下面的代码

// 无法区分哪个参数用来构造 key 哪些用来构造 value
// 有可能是 std::string s("hello", 1), std::complex<double> cpx(2) 
// 也有可能是 std::string s("hello"), std::complex<double> cpx(1, 2)
std::map<std::string, std::complex<double>> scp;
scp.emplace("hello", 1, 2);

考虑使用可以接受异构、变长参数的 tuple 来对参数进行分组。再使用 C++11 提供的一个特殊类型 piecewise_construct_t 来帮助它们找到各自正确的构造函数了。std::piecewise_construct_t 是一个空类，全局变量 std::piecewise_construct 就是该类型的一个变量。

std::piecewise_construct作为构造 pair 对象的第一个参数传递，以选择构造函数形式，通过将两个元组对象的元素转发给它们各自的构造函数来构造其成员。即第一个 tuple 作为 key，第二个 tuple 作为 value。使用 forward_as_tuple，该函数会构造一个 tuple 并转发给 pair。

// 想对于 map 避免临时变量的构造的话，就需要构建两个 tuple
std::map<std::string, std::complex<double>> scp;
scp.emplace(std::piecewise_construct, // 此常量值作为构造 pair 对象的第一个参数传递，以选择构造函数形式，通过将两个元组对象的元素转发给它们各自的构造函数来构造其成员。
    std::forward_as_tuple("hello"), // 该函数会构造一个 tuple 并转发给 pair 构造，并存储在 first 字段
    std::forward_as_tuple(1, 2)); //该函数会构造一个 tuple 并转发给 pair 构造，存储在 second 字段

cartographer中的代码就是这样

// std::map<int, ::cartographer::mapping::PoseExtrapolator>  extrapolators_;
  // PoseExtrapolator的构造函数有2个参数：common::Duration   pose_queue_duration,
                                  //      double   imu_gravity_time_constant
  extrapolators_.emplace(
      std::piecewise_construct, 
      std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
          gravity_time_constant)  );

无参的emplace_back

还有一种使用无参的vector::emplace_back() 或者 deque::emplace_back()，下面是从cartographer中学来的代码，确实有一定技巧

    vector<std::unique_ptr<A> > dd;
    dd.emplace_back();
    // 注意这里的 size 也是1
    cout <<"capacity: "<< dd.capacity() <<" size: "<< dd.size() <<endl;
//    cout << dd.at(0)->getValue() <<endl;  这里运行会崩溃
    
    auto*  a1 = &dd.back();  // 这个是 unique_ptr
    a1->reset(new A(5) );  // 不能再用=，unique_ptr不能复制
    cout <<"capacity: "<< dd.capacity() <<" size: "<< dd.size() <<endl;
    cout << dd.at(0)->getValue() <<endl;

运行结果：

capacity: 1 size: 1
A construct 
capacity: 1 size: 1
5

参考：
emplace_back VS push_back

二叉树 2021-07-25|数学基础

四叉树和八叉树就是2D和3D的“二分法”，搜索过程与二叉树搜索也类似，二叉树中是将数组sort后存入二叉树中，从而在查找中实现时间复杂度为log2N；四叉树/八叉树是按平面/空间范围划分有序node，将所有points（坐标已知，但是每个点的point在vector中的index可以认为是随机的，没有规律的，所以不能直接根据index取出point(x,y)）放入所属node中，实现所有points的sorted，进而在搜索时，实现时间复杂度为logN

双线性插值双三次插值 2021-07-16|数学基础

插值指在离散数据的基础上补插连续函数，使得连续曲线通过全部给定的离散数据点。插值的本质 —— 利用已知数据估计未知位置数值。插值和拟合的不同之处在于：对于给定的函数，插值要求离散点“坐落在”函数曲线上从而满足约束；而拟合则希望离散点尽可能地 “逼近” 函数曲线。

双线性插值 Bilinear Interpolation

一次线性插值.png
普通的线性插值我们都很熟悉。双线性插值是有两个变量的插值函数的线性插值扩展，其核心思想是在两个方向分别进行一次线性插值。
示意图.png
二次线性插值的公式.png
看这个推导

双线性插值在三维空间的延伸是三线性插值。

双三次插值 Bicubic interpolation

二维空间中最常用的插值方法。在这种方法中，函数f在点(x , y)的值可以通过矩形网格中最近的十六个采样点的加权平均得到，在这里需要使用两个多项式插值三次函数，每个方向使用一个。

双三次插值通过下式进行计算

$\mathop{\Sigma}\limits_{i=0}\limits^{3} \mathop{\Sigma}\limits_{j=0}\limits^{3} a_{ij}x^iy^j$

计算系数的过程依赖于插值数据的特性。如果已知插值函数的导数，常用的方法就是使用四个顶点的高度以及每个顶点的三个导数。一阶导数与表示x与y方向的表面斜率，二阶相互导数表示同时在x与y方向的斜率。这些值可以通过分别连续对x与y向量取微分得到。对于网格单元的每个顶点，将局部坐标(0,0), (1,0), (0,1)和(1,1)带入这些方程，再解这16个方程

矩阵的分解 2021-07-15|数学基础

choskey分解

Cholesky分解一个重要的应用就是解方程组 Ax = B，其中A是一个正定矩阵。因为A是一个正定矩阵，所以有A =LL^T，其中L是一个下三角矩阵。原方程组可以写成 LL^Tx = B。如果令 y = L^Tx ，则有Ly = B。注意到L是一个下三角矩阵，所以从下向上求解y是非常容易的. 求解出y之后，在按照类似的方法求解y = L^Tx 中的 x，而其中L^T是一个上三角矩阵，所以最终求出 x 也是非常容易的

cholesky分解又称为平方根法，是A为实对称正定矩阵时，LU分解的变形。

协方差矩阵是实对称半正定的，如果对角线元素全为正，则可进行cholesky分解， $\Sigma = LL^T$

计算样本中两个特征向量的距离，可以用马氏距离表示

直接对协方差求逆比较复杂，使用cholesky分解

LDLT

LDLT分解法实际上是Cholesky分解法的改进，优先使用LDLT而不是LLT方法。 Cholesky分解法虽然不需要选主元，但其运算过程中涉及到开方问题，而LDLT分解法则避免了这一问题。 若对称矩阵A的各阶顺序主子式不为零时，则A可以唯一分解为 $A=LDL^T$ 。其中 L 为下三角单位矩阵 (即主对角线元素皆为 1，下三角其他元素不为0)，D为对角矩阵， $L^T$ 为L的转置矩阵。

LDLT则可以应对半正定和负半定问题，精度较LLT更高

QR分解

对于任意的实数矩阵 $A\in C^{n\times n}$，存在 n阶正交矩阵 Q 和 n阶上三角矩阵 R，使得 $A=Q*R$。注意：矩阵A可以是非方阵。正交矩阵： $Q^{-1}=Q^T$

如果A是非奇异的，且限定R的对角线元素为正，则这个分解是唯一的。

实际计算有Givens旋转、Householder变换，以及Gram-Schmidt正交化等。Eigen常用的是 Householder变换

用于求线性方程组

对于直接求解线性方程组的逆，用QR分解的方法求解会更具有数据的稳定性。对于求解一个线性系统Ax = b, 这里A的维度是 m x n。

上三角矩阵R的逆矩阵仍然是上三角矩阵，可以用分块矩阵迭代的方法很容易地求出来

下三角矩阵的逆矩阵也是类似求法

SVD分解 - 奇异值分解

特征值分解仅针对方阵，而不是方阵的矩阵就有了SVD分解： $A=U\Sigma V^T$

其中A为m x n的矩阵，正交矩阵 U(m x m阶) 和 V(n x n阶)。

$\Sigma_{mn} = \left[ \begin{matrix} \Sigma_1 & 0 \\ 0 & 0 \\ \end{matrix} \right]$ $\Sigma_1=diag(\sigma_1....\sigma_r)$

其中 $\sigma_i$ 是矩阵 $\Sigma_{1}$ 的特征值

矩阵U的列称为左奇异向量，是正交的。矩阵V的列向量(也称为右奇异向量)也是正交的.

此时的A如果是方阵，那么逆矩阵也很容易求出: $A^{-1} = V\Sigma^{-1}U^T$

奇异值分解同时包含了旋转、缩放( $\Sigma$ )和投影三种作用。特征值分解只有缩放的效果。

矩阵求导 2021-07-15|数学基础

最常用：

$\frac{\partial Ax}{\partial x} = A^T$ $\frac{\partial x^TA}{\partial x} = A$

矩阵求导 1.jpg

$\frac{dTr(AB)}{dA} = B^T$ $\frac{d(ABA^T)}{dA} = 2AB$

左边为分子布局，右边为分母布局，不推荐固定用哪个

不管啥layout归根结底一句话…保证雅可比矩阵乘起来的时候维度对应即可。如果有方阵，那肯定某两层中间变量的维度相同的，直接强行让两者的维度记号不一样即可。

叉乘的导数：

举例： $\frac{1}{2} ||Ax- \overrightarrow {b} ||^2$ 对 x 求导，最后结果是 $A^T(Ax - b)$

利文伯格法 2021-07-15|算法推导

利文伯格法是对高斯牛顿法的改进，是把梯度下降法与高斯牛顿法进行线性组合，属于信赖区域法，认为近似只在区域内可靠，控制每一步迭代的步长以及方向。 高斯牛顿法属于线搜索，即先找方向，再确定步长。

推导.png

一直到 $J(x_k)$ 满足Lipschitz连续且非奇异，算法二次收敛。利普希茨连续的定义为：对于函数 f(x)，若其任意定义域中的 $x_1$ , $x_2$ ，都存在 $L>0$ ，使得

$\left| \ f(x_1)-f(x_2) \right| \leq L\left| \ x_1-x_2 \right|$

阻尼因子μ作用如下：

μ非常大，说明此时高斯牛顿的一次泰勒展开近似效果不好，更新方式偏向近似最速下降法。
μ比较小，说明此时高斯牛顿的一次泰勒展开近似效果挺好的，更新方式偏向近似高斯牛顿法。

狗腿法就是引入信赖区域代替LM算法中的阻尼项

利文伯格法的特点：

μ大于0 保证了系数矩阵的正定，解决了高斯牛顿法的缺陷，迭代朝着下降方向进行。
即使初值距离(局部)最优解非常远，利文伯格法仍可以求解成功
利文伯格法的收敛速度要稍微低于高斯牛顿法，但更鲁棒
可以在一定程度避免系数矩阵的奇异和病态问题

对于以上几种方法，不会直接求系数矩阵的逆，而是用矩阵分解，例如QR, Cholesky分解，这个矩阵往往有稀疏形式，为SLAM的实时求解提供了可能。SLAM中，Pose Graph的结构会越来越大，H矩阵如果不是稀疏的，维数会达到几千，求逆矩阵会极其耗时，不可能实时求解。

矩阵之所以是稀疏，根本原因是约束只和它周边少数的节点有关。

线搜索方法：首先找到一个下降的方向，目标函数将沿其下降。然后再确定步长，从而决定沿该方向到底移动多远。下降方向可以通过各种方法计算，如梯度下降法、牛顿法和拟牛顿法。步长可以精确或不精确地确定。
置信域法Trust Region: 在搜索空间的子集内应用模型函数（通常是二次方程）来近似目标函数，这个空间被称为置信域。如果模型函数成功地使真正的目标函数最小化，则扩大置信域。否则收缩置信域并且再次尝试求解模型函数的优化问题。

正定矩阵对称矩阵反对称矩阵奇异矩阵病态矩阵 2021-07-14|数学基础

正定矩阵

定义: A是n阶方阵，如果对任何非零向量x，都有 $x^TAx$ >0，其中 $x^T$ 表示x的转置，就称A正定矩阵。

性质:

行列式恒为正
正定矩阵可逆。若矩阵A正定，则必有|A|（矩阵A的行列式）>0，所以矩阵A可逆
所有的特征值都是正的
实对称矩阵A正定，当且仅当 A与单位矩阵合同
两个正定矩阵的和是正定矩阵，乘积也是正定矩阵

实数域上所说的正定矩阵都是对称矩阵，在复数域上是厄米特矩阵（共轭对称）

判别实对称矩阵A的正定性有两种方法：

求出A的所有特征值，若全为正数，则A是正定的；若特征值全为负数，则A为负定的。
若A的所有顺序主子式都大于零，则A是正定的；若A的奇数阶顺序主子式为负，偶数阶为正，则A为负定的。

半正定矩阵

正定矩阵的几何意义

定义中的 $x^TAx>0$ 变为 $x^TY>0$ ( $Y=Ax$ )

那么有 $cos(\theta)=\frac{x^T Y}{\Vert x^T \Vert \Vert Y \Vert} > 0$

也就是说，一个向量 x 经过矩阵A变换后(左乘)，与其转置向量的夹角小于90°

反对称矩阵

也就是 $A^T = -A$ ，或者说元素关系满足 $A_{ij} = -A_{ji}$ 主对角线元素皆为0

斜对称矩阵自身相乘的积是对称矩阵。
任意矩阵 $A$ ， $A^T - A$ 是斜对称矩阵。
若 $A$ 是斜对称矩阵， $x$ 是向量， $x^T A x$ = 0

稀疏矩阵

在矩阵中，若数值为0的元素数目远远多于非0元素的数目，并且非0元素分布没有规律时，则称该矩阵为稀疏矩阵

奇异矩阵(singular)

奇异和非奇异矩阵仅针对n阶方阵。奇异矩阵就是对应的行列式等于0的矩阵。若一个n阶矩阵是可逆的，则称它为非奇异矩阵。

奇异矩阵的判定方法: 行列式|A|是否等于0，若等于0，称矩阵A为奇异矩阵

非奇异矩阵的判定方法：

一个矩阵非奇异当且仅当它的行列式不为零
一个矩阵非奇异当且仅当它的秩为n
可逆矩阵就是非奇异矩阵，非奇异矩阵也是可逆矩阵

病态矩阵

求解方程组Ax=b时如果对数据进行较小的扰动，则得出的结果具有很大波动，这样的矩阵称为病态矩阵。

最根本的原因在于矩阵的列之间的相关性过大。

李群李代数 2021-07-14|算法推导

导数的定义是这样的

SLAM问题会构建一个优化问题，求解最优的R t，使误差最小化。这必然涉及求导，SLAM的运动过程是连续的，所以一定是可以求导的。在SLAM中最常见的变换是四维变换矩阵T以及三维旋转矩阵R。但是它们对加法并不封闭: 两个变换矩阵之和明显不是变换矩阵；而旋转矩阵是正交矩阵，但两个正交矩阵之和不是正交矩阵。需要把函数f换成旋转矩阵R，对自变量添加一个微小值来进行，但是前面我们说了，旋转矩阵做加减法运算之后就不再满足旋转矩阵的形式了，没有加减法就意味着不能求导了。

为什么会出现李群李代数的概念，原因就在于此。SLAM应当能求导，只是我们不该用这两种矩阵的形式，而是用李代数。

另外两个原因：

旋转矩阵都是正交矩阵且行列式为1，它们作为优化变量时，会引入额外的约束，使优化更困难了。
优化变量的迭代阶段： $x = x + \Delta x$ 不能直接用两种矩阵相加

引入李群和李代数，就可以解决旋转矩阵求导问题，也就是说李代数se(3)和so(3)对加法封闭，所以用李代数表示机器人的旋转和位移。求导结束后，再对数映射为李群，这个过程都是由优化器完成的，无需我们手动。

李群和李代数

具有连续（光滑）性质的群。整数群是离散的，没有连续性质。SO(3)和SE(3)都是连续的，空间中的机器人的姿态是连续变化的，而不是磕磕绊绊地运动，所以都是李群。

机器人的旋转是一个随时间连续变化的过程，函数是R(t)，根据旋转矩阵的性质

$R(t)R(t)^T = I$

经过一系列推导，发现 $\dot R(t)R(t)^T$ 是一个反对称矩阵，因此写做

$\dot R(t)R(t)^T = \phi(t)^\land$

这里的 $\phi$ 是一个三维向量，对应到 SO(3) 的李代数 $\mathfrak{so}(3)$ ，李群和李代数直接是指数映射。

假设在很小的时间范围内， $\phi(t)$ 是一个恒定值，因为实际中机器人的姿态并不会突变。再经过一系列推导得到

$R = exp(\phi^\land)$

李代数包括一个集合，一个数域和一个二元运算符，它们满足四条性质：封闭性、双线性、自反性、雅克比等价。

在不引起歧义的情况下，我们说李代数 $\mathfrak{so}(3)$ 的元素是三维向量或3维反对称矩阵，不做区别。

对于李群SE(3)，对应的李代数 $\mathfrak{se}(3)$ 每个元素是一个6维向量，前三维是平移，后三维是旋转，实际就是 $\mathfrak{so}(3)$ 元素。此时的 $\land$ 符号是把6维向量转换为四维的变换矩阵 T

对于李代数 $\mathfrak{so}(3)$ 的元素 $\phi$ ，由于它是3维向量，写成 $\phi=\theta\vec a$ ， $\vec a$ 是和 $\phi$ 方向相同的单位向量， $\theta$ 是 $\phi$ 的模长。

经过一连串推导，获得

这实际就是罗德里格斯公式 3.14.

旋转矩阵的李代数实际上就是旋转向量组成的空间。指数映射其实就是通过罗德里格斯公式变换的。

BCH公式

根据BCH公式，第2个公式右边还有一些余项

李代数的求导问题

对于公式 4.39，需要计算目标函数J关于变换矩阵T的导数。我们经常构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。但是旋转矩阵和变换矩阵，它们对加法都没有良好的定义，所以对姿态有关的函数求导，只能通过李代数进行。关于用李代数解决求导问题，有两种思路：

用李代数表示姿态，然后根据李代数加法对李代数进行求导
对李群左乘或者右乘微小扰动，然后对该扰动求导。即左扰动和右扰动模型。

因为旋转矩阵没有加法，所以我们就对旋转矩阵的李代数就行求导，也就是李代数的局部坐标上添加扰动，由于李代数本身对应旋转向量，因此对旋转向量添加扰动相当于同时改变旋转轴和旋转角度。

对一个空间点 $p$ 进行旋转，得到 $Rp$ ，现在计算旋转后的点坐标相对旋转的导数，不严谨地记做 $\frac{∂(Rp)}{∂R}$

由于 $SO(3)$ 没有加法，假设 $R$ 对应的李代数为 $\phi$ ，于是我们计算出

$\frac{∂(exp(\phi^\land)p)}{∂\phi} =−(Rp)^\land J_l$

平时不用这种方法，而是用扰动模型法。

不管是李代数求导还是扰动模型，都是旋转矩阵对李代数的求导。

对R进行一次扰动 $\Delta R$ ，这个扰动可以乘在 $R$ 左边，也可以是右边。假设 $\Delta R$ 对应的李代数为 $\varphi$ ，然后对 $\varphi$ 求导

左扰动模型

$\frac{∂(Rp)}{∂\varphi} =−(Rp)^\land$

右扰动模型

扰动模型，显然计算量上要少非常多。

旋转连乘的求导的推导

逆旋转点的求导

逆旋转函数的求导

高斯牛顿法 2021-07-13|算法推导

推导过程

有些人可能产生疑问，为什么通过最小二乘而不是其他方式获得最优函数，比如说最小化误差的绝对值的和？这里的推导使用最大似然估计推导出 $X^* = argmin\Sigma e_i^T \Omega e_i$ 就提供了概率学基础

或者简单的推导
推导 1.png
推导 2.png

特点：

要求H正定，但实际只保证半正定。如果觉得太抽象，可以把 $J^TJ$ 理解为 $x^2$ ，把正定理解为正数，半正定理解为非负。
可能出现H为奇异矩阵或病态矩阵，此时增量的稳定性较差，算法会不收敛。
即使H非奇异非病态，如果步长 $\Delta x$ 太大，也会导致不收敛。
只能在展开点附近有好的近似效果

根据十四讲的推导过程，牛顿法是对 $\left\|f(x+\Delta x)\right\|_2^2$ 的二阶展开，高斯牛顿法是对 $\left\|f(x) + J\Delta x\right\|_2^2$ 的展开

参考:
graph slam tutorial ：从推导到应用２
 牛顿法高斯牛顿法
 非线性优化整理-2.高斯牛顿法

执行sudo rosdep init后报错