个人博客：http://www.chenjianqu.com/
原文链接：http://www.chenjianqu.com/show-83.html
在三维空间中刚体运动 的基础上，假设机器人某时刻的位姿为T (也就是世界坐标系到机器人坐标系的变换矩阵为T)，它观察到了世界坐标为p的点，产生了观测数据z，则由坐标变换关系，得z=Tp+w，其中w为随机噪声。由于噪声的存在，z不能精确满足z=Tp。由此我们想寻找一个最优的T，使得误差最小。理想观测和实际数据的差为：e=z-Tp。假设有N个路标和观测点，则有：
求解此问题，需要计算目标函数J关于变换矩阵T的导数。举着个例子是想说，我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。
导数的定义如下：
或
而三维旋转矩阵构成特殊正交群SO(3)，三维变换矩阵构成特殊欧式群SE(3)：
SO(3), SE(3) 上并没有良好定义的加法，它们只是群。如果我们把 T 当成一个普通矩阵来处理优化，那就必须对它加以约束。而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。因此用李代数对位姿进行求导会很方便，这就是为什么需要用到李群李代数的原因之一。
概念
反对称矩阵
满足AT=-A的矩阵称为反对称矩阵。性质：
(1).反对称矩阵的主对角线上的元素全为零，而位于主对角线两侧对称的元反号。若A为反对称矩阵，则A'，λA均为反对称矩阵；
(2)若A,B均为反对称矩阵，则A±B也为反对称矩阵；
(3)设A为反对称矩阵，B为对称矩阵，则AB-BA为对称矩阵；
(4)奇数阶反对称矩阵的行列式必为0。
(5)反对称矩阵的特征值是0或纯虚数，并且对应于纯虚数的特征向量的实部和虚部形成的实向量等长且互相正交。
反对称矩阵与向量的转换
^表示一个向量对应一个反对称矩阵，ˇ表示反对称矩阵对应一个向量：
群
群(Group)是一种集合加上一种运算的代数结构，要求这个运算满足“凤姐咬你”，如下：
因此旋转矩阵与矩阵乘法，变换矩阵与矩阵乘法都构成群。矩阵中常见的群有：
• 一般线性群 GL(n) 指 n × n 的可逆矩阵，它们对矩阵乘法成群。
• 特殊正交群 SO(n) 也就是所谓的旋转矩阵群，其中 SO(2) 和 SO(3) 最为常见。
• 特殊欧氏群 SE(n) 也就是前面提到的 n 维欧氏变换，如 SE(2) 和 SE(3)。
李群
李群是指具有连续（光滑）性质的群。SO(n) 和 SE(n) 在实数空间上是连续的，我们能够直观地想象一个刚体能够连续地在空间中运动，所以它们都是李群。
李代数
每个李群都有与之对应的李代数。李代数描述了李群的局部性质，准确地说，是单位元附近的正切空间。李代数由一个集合V，一个数域F和一个二元运算[, ]组成(其中二元运算被称为李括号)。如果它们满足以下几条性质，则称(V, F, [, ]) 为一个李代数，记作g：
比如，三维向量上定义的叉积x是一种李括号，g=(R^3,R,x)构成李代数。
李代数so(3)
SO(3) 对应的李代数是定义在R^3上的向量，记作 ϕ。下式的ϕ是李代数里的向量，ϕ该向量对应的反对称矩阵：
两个向量Ф1,Ф2对应的李括号为：
由于向量 ϕ 与反对称矩阵是一一对应的，在不引起歧义的情况下，就说 so(3) 的元素是三维向量或者三维反对称矩阵。
李代数so(3)里面是由三维向量组成的集合，每个向量对应到一个反对称矩阵，可用于表达旋转矩阵的导数。
李代数se(3)
每个se(3)的元素是一个六维向量，前三维为平移，后三维为旋转，实质上是so(3)中的元素。这里的^被拓展了含义，其中下面的四维矩阵不具有反对称：
李代数se(3)的括号为：
李群和李代数的关系
对于任意旋转矩阵R，有R*RT=I。设R随着时间变化，得R(t)*R(t)T=I。两边对时间求导，得
因此Ŕ(t)*R(t)T是反对称矩阵，可以找到一个三维向量Ф(t)与之对应，即Ŕ(t)*R(t)T=Ф(t)^。等式两边右乘R(t)，得：
即旋转矩阵的求导，只需左乘Ф^(t)即可。设t0=0，R(0)=I，把R(t)在t=0处一阶泰勒展开：
可以看到，Ф(t)反映了R的导数性质，故称Ф(t)在SO(3) 原点附近的正切空间 (Tangent Space) 上。在t0附近时，设Ф保持常数Ф(t0)=Ф0，则有Ŕ(t)=Ф(t0)^R(t)=Ф0^R(t)。解该微分方程，且R(0)=I，得：
旋转矩阵 R 与一个反对称矩阵Ф0^(t)通过指数关系发生了联系。给定某时刻的R，我们就能求得一个Ф，它描述了R在局部的导数关系，ϕ正是对应到 SO(3)上的李代数so(3)。给定Ф计算exp(Ф^)和给定R计算Ф，是李群与李代数之间的指数/对数映射。
so(3)映射到SO(3)
由上面的推导可知，so(3)可以通过指数映射到SO(3)。第一种方法是将矩阵的指数映射可以写成一个泰勒展开：
但是这个定义难以计算。
另一种方法：ϕ 是三维向量，我们可以定义它的模长和它的方向，分别记作 θ 和 a，于是有 ϕ = θa。这里 a 是一个长度为 1 的方向向量，即 |a| = 1。对于a^，可推导得以下两条性质：
根据这两个公式，可以将泰勒展开变形为：
最后得到的公式其实就是用于旋转向量转换到旋转矩阵的罗德里格斯公式：
这表明，so(3)实际上就是由旋转向量组成的空间，而指数映射就是罗德里格斯公式。指数映射是满射，即每个SO(3)的元素可能对应多个so(3)中的元素。映射的种类：
SO(3)映射到so(3)
可以通过对数映射将SO(3)映射到so(3)。对数映射的泰勒展开如下：
利用迹的性质求更快。罗德里格斯公式：
取两边的迹：
最后得：
对于转轴n，由于旋转轴上的向量在旋转后不发生改变，即Rn=n。因此，转轴 n 是矩阵 R 特征值 1 对应的特征向量。求解此方程，再归一化，就得到了旋转轴。
se(3)映射到SE(3)
se(3)的元素：
se(3)上的指数映射：
其中J的推导过程：令Ф=θɑ,其中ɑ是单位向量，则：
最后得：
SE(3)映射到so(3)
同样使用对数映射，se(6)中的旋转向量使用R计算，跟so(3)方法一样；平移向量t=Jρ。
李群和李代数的关系
李代数求导
BCH公式及其意义
SO(3)的乘法并不对应se(3)的加法，而是由Baker-Campbell-Hausdorff(BCH)公式给出：ln( exp(A) exp(B) ) = A + B + [A,B]/2 + [A,[A,B]]/12 - [B,[A,B]]/12 + ...，公式中的[]为李括号。BCH 公式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。
考虑SO(3)上的李代数ln(exp(Ф1ˆ)*exp(Ф2ˆ))ˇ，当Ф1和Ф2为小量时，可以忽略上式中小量二次以上的余项，近似表达为：
以第一个近似为例，该式告诉我们，当对一个旋转矩阵 R2（李代数为 ϕ2）左乘一个微小旋转矩阵 R1（李代数为 ϕ1）时，可以近似地看作，在原有的李代数 ϕ2 上加上了一项Jl(ϕ2)-1*ϕ1。同理，第二个近似描述了R1右乘一个微小位移的情况。其中Jl是左乘近似雅可比：
Jr是右乘近似雅可比：Jr(ϕ)=Jl(-ϕ)。
总结一下BCH近似的意义，假定对某个旋转 R，对应的李代数为 ϕ。有一个微小旋转，记作ΔR，对应的李代数为Δϕ。那么旋转R左乘微小旋转ΔR，在李群上得到的结果为ΔR，在李代数上，根据BCH近似，为 Jl-1( ϕ ) Δϕ+ϕ。合并起来，得：
反之，在李代数上进行加法，ϕ+Δϕ，在李群上近似为带左右雅可比的乘法：
同样的，对于SE(3)，也有类似的BCH近似：
求导思路
前面说过，SO(3), SE(3) 上并没有良好定义的加法，它们只是群。如果我们把 T 当成一个普通矩阵来处理优化，那就必须对它加以约束。而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。因此，使用李代数解决求导问题的思路分为两种：
1. 用李代数表示姿态，然后根据李代数加法来对李代数求导。
2. 对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。
so(3)李代数求导
假设我们对一个空间点 p 进行了旋转，得到了 Rp。现在，要计算旋转之后点的坐标相对于旋转的导数，推导得：
so(3)扰动求导
另一种求导方式是对 R 进行一次扰动 ∆R，看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边，最后结果会有一点儿微小的差异，我们以左扰动为例。设左扰动 ∆R 对应的李代数为 φ。然后，对 φ 求导，即：
se(3)扰动求导
假设某空间点 p经过一次变换 T（对应李代数为 ξ），得到 Tp。给 T 左乘一个扰动 ∆T = exp (δξ^)，设扰动项的李代数为δξ = [δρ, δϕ]T，则：
把最后的结果定义成一个算符 ⊙，它把一个齐次坐标的空间点变换成一个 4 × 6 的矩阵。其中矩阵求导的顺序：
Sophus代码
基于模板的 Sophus 库和 Eigen 一样，是仅含头文件而没有源文件的。
CMakeLists.txt
cmake_minimum_required(VERSION 2.6)
project(sophustest)
set( CMAKE_CXX_FLAGS "-std=c++11" )
include_directories("/home/chenjianqu/software/eigen-3.3.5")
find_package( Sophus REQUIRED )
include_directories( ${Sophus_INCLUDE_DIRS} )
add_executable(sophustest main.cpp)
install(TARGETS sophustest RUNTIME DESTINATION bin)
main.cpp
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/se3.hpp>
using namespace std;
using namespace Eigen;
using namespace Sophus;
int main(int argc, char **argv) {
// 沿Z轴转90度的旋转矩阵
Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
// 或者四元数
Quaterniond q(R);
SO3d SO3_R(R); // Sophus::SO3d可以直接从旋转矩阵构造
SO3d SO3_q(q); // 也可以通过四元数构造,二者是等价的
cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
// 使用对数映射获得它的李代数
Vector3d so3 = SO3_R.log();
cout << "so3 = " << so3<< endl;
// hat 为向量到反对称矩阵
Matrix3d so3_hat=SO3d::hat(so3);
cout << "so3 hat=\n" << so3_hat<< endl;
//vee为反对称到向量
Vector3d v=SO3d::vee(so3_hat);
cout << "so3 hat vee= " << v<< endl;
// 增量扰动模型的更新
Vector3d delta_so3(1e-4, 0, 0); //假设更新量为这么多
SO3d SO3_updated = SO3d::exp(delta_so3) * SO3_R;
cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;
cout << "*******************************" << endl;
// 对SE(3)操作大同小异
Vector3d t(1, 0, 0); // 沿X轴平移1
SE3d SE3_Rt(R, t); // 从R,t构造SE(3)
SE3d SE3_qt(q, t); // 从q,t构造SE(3)
cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
cout << "SE3 from q,t= \n" << SE3_qt.matrix() << endl;
// 李代数se(3) 是一个六维向量，方便起见先typedef一下
typedef Eigen::Matrix<double, 6, 1> Vector6d;
Vector6d se3 = SE3_Rt.log();
cout << "se3 = " << se3.transpose() << endl;
// 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
// 同样的，有hat和vee两个算符
cout << "se3 hat = \n" << SE3d::hat(se3) << endl;
cout << "se3 hat vee = " << SE3d::vee(SE3d::hat(se3)).transpose() << endl;
// 最后，演示一下更新
Vector6d update_se3; //更新量
update_se3.setZero();
update_se3(0, 0) = 1e-4d;
SE3d SE3_updated = SE3d::exp(update_se3) * SE3_Rt;
cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;
return 0;
}
735
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
