sophus库是一个基于Eigen的C++李群李代数库，可以用来方便地进行李群李代数的运算。
头文件
主要用到以下两个头文件
#include <sophus/so3.h>
#include <sophus/se3.h>
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 <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"
#include "sophus/so3.hpp"
int main(void){
// 优化变量为李代数se(3)的平移向量
typedef Eigen::Matrix<double, 6, 1> Vector6d;
// 数据点
std::vector<Eigen::Vector3d> pts1, pts2;
// 随机数生成器
std::default_random_engine generator;
std::normal_distribution<double> noise(0., 1.);
// 构造相机位姿，作为参考位姿
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);
// 观测数据
for (int i = 0; i < 100; ++i) {
double x = noise(generator), y = noise(generator), z = noise(generator);
pts1.push_back(Eigen::Vector3d(x, y, z)); // 相机坐标系下的点
pts2.push_back(SE3_Rt * pts1[i]); // 世界坐标系下的点
}
// 开始Gauss-Newton迭代，初始位姿为参考位姿+扰动
Sophus::SE3d SE3_estimate(R*Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ()).toRotationMatrix(),
t+Eigen::Vector3d(0.1, 0.0, 0.0));
enum {
DLEFT = 0, // 左扰动
DRIGHT = 1, // 右扰动
};
int disturb = DRIGHT;
for (int iter = 0; iter < 100; ++iter) {
Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
Eigen::Matrix<double, 6, 1> b = Eigen::Matrix<double, 6, 1>::Zero();
double cost = 0;
for (int i = 0; i < pts1.size(); ++i) {
// compute cost for pts1[i] and pts2[i]
Eigen::Vector3d p = pts1[i], pc = pts2[i]; // p为相机坐标系下的点，pc为世界坐标系下的点
Eigen::Vector3d pc_est = SE3_estimate * p; // 估计的世界坐标系下的点
Eigen::Vector3d e = pc_est - pc; // 残差
cost += e.squaredNorm() / 2;
// compute jacobian
Eigen::Matrix<double, 3, 6> J = Eigen::Matrix<double, 3, 6>::Zero();
if(disturb == DRIGHT){
// 右扰动
J.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
J.block<3,3>(0,3) = -SE3_estimate.rotationMatrix() * Sophus::SO3d::hat(p);
} else{
// 左扰动
J.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
J.block<3,3>(0,3) = -Sophus::SO3d::hat(SE3_estimate.rotationMatrix() * p);
}
// compute H and b
H += J.transpose() * J;
b += -J.transpose() * e;
}
// solve dx
Vector6d dx = H.ldlt().solve(b);
if (dx.norm() < 1e-6) {
break;
}
// update estimation
if(disturb == DRIGHT){
// 右乘更新
SE3_estimate.so3() = SE3_estimate.so3() * Sophus::SO3d::exp(dx.block<3,1>(3,0));
SE3_estimate.translation() += dx.block<3,1>(0,0);
} else{
// 左乘更新
SE3_estimate.so3() = Sophus::SO3d::exp(dx.block<3,1>(3,0)) * SE3_estimate.so3();
SE3_estimate.translation() += dx.block<3,1>(0,0);
}
std::cout << "iteration " << iter << " cost=" << cost << std::endl;
}
std::cout << "estimated pose: \n" << SE3_estimate.matrix() << std::endl;
std::cout << "ground truth pose: \n" << SE3_Rt.matrix() << std::endl;
}
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);
