Vslam14讲(复习四)

李群与李代数

主要目标：

1. 理解李群与李代数的概念，掌握SO(3),SE(3)与对应的李代数的表示方式。
2. 理解BCH近似的意义。
3. 学会在李代数上的扰动模型。
4. 使用Sophus对李代数进行运算。

上个博客学习的主要是三维世界中刚体运动的描述方式，包括旋转矩阵、旋转向量、欧拉角、四元数等突感种方式。我们重点介绍了旋转的表示，，但是在SLAM这个系统中，我们还要对他们进行估计和优化。

现在呢，SLAM中相机位姿是未知的，所以我们可能需要解决"什么样的相机位姿最符合当前的观测数据"这样的问题。一种方式是把它构建成一个优化问题，求最优解的R和t，使得误差最小化。

之前说过，旋转矩阵自身带有约束(正交且行列式为1),。他们作为优化变量时，会引入额外的约束，使优化变得额外困难。通过李群——李代数之间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式。

4.1 李群与李代数基础

旋转矩阵和变换矩阵地定义还记得么？三维旋转矩阵构成了 特殊正交矩阵SO(3),而变换矩阵构成了 特殊欧式群 SE(3)。

写起来如下：

当时并未详解 群 的含义。 变换矩阵或者旋转矩阵对加法不封闭。  换句话说，对于任意两个旋转矩阵，按照矩阵加法的定义，和不再是一个旋转矩阵：可以

这两种矩阵没有良好定义的加法，或者通常的矩阵加法对这两个集合不封闭。

相对地，他们只有一种较好的运算：乘法。SO(3)、SE(3)关于乘法是封闭的：

4.1.1 群

4.1.2 李代数的引出

可以这么理解：R随时间不断变化，但是始终满足正交这个要求。

对于上面那个等式。是始终成立的，我们可以两边对时间求导：

上面的操作是求导加移项外加提括号转置。

我们观察一下，反对称矩阵，它转置加负号等于它本身。下面是根据反对称矩阵得出的等式：

上面红框中，是t固定时。这里需要好好理解下上面的泰勒展开。上面让我们看清楚谁反应了导数性质。

4.1.3 李代数的定义

封双自雅？？

4.1.4 李代数 so(3)

4.2 指数与对数映射

4.2.1 SO(3)上的指数映射

又看到罗德里格斯了，还记得怎么推导罗德里格斯么？？？？

复习罗德里格斯！！！

记住：旋转矩阵的倒数可以由旋转向量指定，指导着如何在旋转矩阵中进行微积分运算。

4.2.2 SE(3)上的指数映射

 

推导的过程主要是泰勒展开 余弦和正弦 (凑出这两个的泰勒展开，然后替换)

4.3 李代数求导与扰动模型

前面我们了解和推导了下如何从李代数到李群。如何把李群到李代数呢？同样还要依靠泰勒展开：

简单看下：

好吧，继续这一节的主题吧。

4.3.1 BCH公式与近似形式

使用李代数的一大动机是优化，而在优化过程中，导数是非常重要的信息（后面非线性优化会说明这一点）。

我们上面虽然推导了李群与李代数的关系。

BCH公式与近似形式都弄完了，也理解了。

4.3.2 SO(3) 上的李代数求导

下面来讨论一个带有李代数的函数，以及关于李代数的求导问题。

构建了一个最小二乘问题。

搞清楚这段话在说什么。

4.3.3 李代数求导

看懂框中的操作。

4.3.4 扰动模型

4.3.5 SE(3)上的李代数求导

4.4 实践：Sopuhs库的应用

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

// 本程序演示sophus的基本用法
int main(int argc,char **argv){
//沿z轴转90度的旋转矩阵
Martix3d R = AngleAxisd(M_PI/2,Vector3d(0,0,1)).toRotationMatrix();
//或四元数
Quaterniond q(R);
Sophus::SO3d SO3_R(R);    //Sophus::SO3d可以直接从旋转矩阵构造
Sophus::SO3d SO3_q(q);    //也可以通过四元数构造
//二者是等价的
cout<<"SO3 from matricx:\n"<<SO3_R.matrix()<<endl;
cout<<"SO3 from quaternion:\n"<<SO3_q.matrix()<<endl;

//使用对数映射获得它的李代数
Vertor3d so3 = SO3_R.log();
cout<<"so3 = "<<so3.transpose()<<endl;
//hat为向量到反对称矩阵
cout<<"so3 hat=\n"<<Sopuhs::SO3d::hat(so3)<<endl;
//相对地，vee为反对称矩阵到向量
cout<<"so3 hat vee= "<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()<<endl;

//增量扰动模型的更新
Vector3d update_so3(1e-4,0,0); //假设更新量为这么多
Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3)*SO3_R;
cout<<"SO3 updated =  \n"<<SO3_updated.matrix()<<endl;

cout<<"*****************************"<<endl;
//对SE3操作大同小异
Vector3d t(1,0,0);    //沿X轴平移1
Sophus::SE3d SE3_Rt(R,t);  //从R、t构造SE(3)
Sophus::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"<<Sophus::SE3d::hat(se3)<<endl;
cout<<"se3 hat vee = "<<Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose()<<endl;

//最后，演示更新

Vector6d update_se3;  //更新量
update_se3.setZero();
update_se3(0,0) = 1e-4d;
Sophus::SE3 SE3_updated = Sophus::SE3d::exp(updated_se3)*SE3_RT;
cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;

return 0;
}

4.4.2 例子：评估轨迹的误差

代码：

#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.h>

using namespace Sophus;
using namespace std;

string groundtruth_file = "./example/groundtruth.txt";
string estimated_file = "./example/estimated.txt";

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;

void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti);

TrajectoryType ReadTrajectory(const string &path);

int main(int argc, char **argv) {
  TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
  TrajectoryType estimated = ReadTrajectory(estimated_file);
  assert(!groundtruth.empty() && !estimated.empty());
  assert(groundtruth.size() == estimated.size());

  // compute rmse
  double rmse = 0;
  for (size_t i = 0; i < estimated.size(); i++) {
    Sophus::SE3d p1 = estimated[i], p2 = groundtruth[i];
    double error = (p2.inverse() * p1).log().norm();
    rmse += error * error;
  }
  rmse = rmse / double(estimated.size());
  rmse = sqrt(rmse);
  cout << "RMSE = " << rmse << endl;

  DrawTrajectory(groundtruth, estimated);
  return 0;
}

TrajectoryType ReadTrajectory(const string &path) {
  ifstream fin(path);
  TrajectoryType trajectory;
  if (!fin) {
    cerr << "trajectory " << path << " not found." << endl;
    return trajectory;
  }

  while (!fin.eof()) {
    double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Sophus::SE3d p1(Eigen::Quaterniond(qx, qy, qz, qw), Eigen::Vector3d(tx, ty, tz));
    trajectory.push_back(p1);
  }
  return trajectory;
}

void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
      pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
      pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
      .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
      .SetHandler(new pangolin::Handler3D(s_cam));


  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

    glLineWidth(2);
    for (size_t i = 0; i < gt.size() - 1; i++) {
      glColor3f(0.0f, 0.0f, 1.0f);  // blue for ground truth
      glBegin(GL_LINES);
      auto p1 = gt[i], p2 = gt[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }

    for (size_t i = 0; i < esti.size() - 1; i++) {
      glColor3f(1.0f, 0.0f, 0.0f);  // red for estimated
      glBegin(GL_LINES);
      auto p1 = esti[i], p2 = esti[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }

}

求误差那里李代数进行了norm()操作，是求二范数操作。因为这里的李代数是向量。

向量的一范数，二范数。回忆一下。

4.5 相似变换与李代数

单目视觉中经常使用相似变换群Sim(3),以及对应的李代数sim(3)。

单目的尺度不确定性我们已经讨论过了。

验证构成群，这在课后题中有考察。。

我之前主要手推了指数映射(从李代数到李群)。对数映射是从李群到李代数，也需要泰勒展开。

 

课后习题：

上面的推导，仔细思考下，需要参照前面欧式群。

接下来好好准备学习雅可比矩阵海信矩阵吧。

https://blog.csdn.net/qq_17032807/article/details/84942548