Sophus使用记录
sophus库是一个基于Eigen的C++李群李代数库，可以用来方便地进行李群李代数的运算。
头文件
主要用到以下两个头文件
#include SO(3)的使用
SO(3)的定义
// Sophus::SO3可以直接从旋转矩阵构造
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
Sophus::SO3d SO3_R(R);
// 亦可从旋转向量构造
Sophus::SO3d SO3_v = Sophus::SO3d::exp({0, 0, M_PI/2});
// 或者四元数
Eigen::Quaterniond q(R);
Sophus::SO3d SO3_q(q);
SO(3)的运算
// 旋转矩阵
Eigen::Matrix3d R0 = SO3_R.matrix();
// 旋转向量，也即李代数
Eigen::Vector3d so3 = SO3_R.log();
// hat为向量到反对称矩阵
Eigen::Matrix3d so3_hat = Sophus::SO3d::hat(so3);
// 相对的，vee为反对称到向量
Eigen::Vector3d so3_vee = Sophus::SO3d::vee(so3_hat);
// 更新
Eigen::Vector3d update_so3(1e-4, 0, 0);
Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3)*SO3_R;
// 两个SO(3)可以直接相乘
Sophus::SO3d SO3_mult = SO3_R * SO3_q;
// 也可以用对数映射
Eigen::Vector3d so3_mult = SO3_R.log() + SO3_q.log();
// 对数映射的反操作
Sophus::SO3d SO3_mult2 = Sophus::SO3d::exp(so3_mult);
SE(3)的使用
SE(3)的定义
// 从旋转矩阵和平移向量构造
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
Eigen::Vector3d t(1,0,0);
Sophus::SE3d SE3_Rt(R, t);
// 从四元数构造
Eigen::Quaterniond q(R);
Sophus::SE3d SE3_qt(q, t);
SE(3)的运算
// 李代数se(3)是一个六维向量，方便起见先typedef一下
typedef Eigen::Matrix<double, 6, 1> Vector6d;
// SE(3)是一个4*4的矩阵，方便起见先typedef一下
typedef Eigen::Matrix<double, 4, 4> Matrix4d;
// 用对数映射获得它的李代数
Vector6d se3 = SE3_Rt.log();
// hat为向量到反对称矩阵
Matrix4d se3_hat = Sophus::SE3d::hat(se3);
// 相对的，vee为反对称到向量
Vector6d se3_vee = Sophus::SE3d::vee(se3_hat);
// 更新
Vector6d update_se3; update_se3.setZero(); // 平移在前，旋转在后
update_se3(0, 0) = 1e-4;
Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3)*SE3_Rt;
SE(3)、SO(3)与Eigen类型的相互转换
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
Sophus::SO3d SO3_R(R);
Eigen::Vector3d t(1,0,0);
Sophus::SE3d SE3_Rt(R, t);
// 从SO(3)获得单位四元数
Eigen::Quaterniond q0 = SO3_R.unit_quaternion();
// 从SO(3)获得旋转矩阵
Eigen::Matrix3d R0 = SO3_R.matrix();
// 从SE(3)获得旋转矩阵
Eigen::Matrix3d R1 = SE3_Rt.matrix().block<3,3>(0,0);
// 从SE(3)获得单位四元数
Eigen::Quaterniond q1 = SE3_Rt.unit_quaternion();
// 从SE(3)获得平移向量
Eigen::Vector3d t1 = SE3_Rt.translation();
坐标变换
Eigen::Vector3d p1(1, 0, 0); // 令p1=(1,0,0)
Eigen::Vector3d p2 = SE3_Rt * p1; // 相当于R*p1+t
优化问题
空间中存在一组三维点云，已知在世界坐标系下的坐标 P w P_w Pw，以及这些点云在相机坐标系下的坐标 P c P_c Pc，求解相机相对于世界坐标系的位姿 T c w T_c^w Tcw。
残差定义：
r i = R p i + t − q i \begin{aligned} r_{i} &=R p_{i}+t-q_{i} \end{aligned} ri=Rpi+t−qi
目标函数如下：
min R , t ∑ i = 1 N ∥ R p i + t − q i ∥ 2 2 \begin{aligned} \min _{R, t} \sum_{i=1}^{N}\left\|R p_{i}+t-q_{i}\right\|_{2}^{2} \end{aligned} R,tmini=1∑N∥Rpi+t−qi∥22
使用右扰动模型计算得到残差的雅克比矩阵如下：
∂ r ∂ δ θ = − R p ^ ∧ ∂ r ∂ δ p = I 3 × 3 \begin{aligned} \frac{\partial r}{\partial \delta \theta} &=-R \hat{p}^{\wedge} \\ \\ \frac{\partial r}{\partial \delta p} &=I_{3 \times 3} \\ \end{aligned} ∂δθ∂r∂δp∂r=−Rp^∧=I3×3
使用左扰动模型计算得到残差的雅克比矩阵如下：
∂ r ∂ δ θ = − ( R p ^ ) ∧ ∂ r ∂ δ p = I 3 × 3 \begin{aligned} \frac{\partial r}{\partial \delta \theta} &=-(R\hat{p})^{\wedge} \\ \\ \frac{\partial r}{\partial \delta p} &=I_{3 \times 3} \\ \end{aligned} ∂δθ∂r∂δp∂r=−(Rp^)∧=I3×3
使用Guass-Newton迭代求解即可，程序如下：
#include bug记录
右乘se3的exp映射时,结果有问题,而左乘没问题
初步定位到问题,在求导时,不是对SE3求导,而是对平移向量和旋转向量分别求导,然后再组合起来,这和SE3求导结果不同.
暂时不管了，SE3右乘雅可比有点复杂，高翔书中也是分开求导和更新的，就这样吧。
// se3右乘更新, 有问题
SE3_estimate = SE3_estimate * Sophus::SE3d::exp(dx);
// 分开更新，没问题
SE3_estimate.so3() = SE3_estimate.so3() * Sophus::SO3d::exp(dx.block<3,1>(3,0));
SE3_estimate.translation() += dx.block<3,1>(0,0);