視覺SLAM十四講（第二版）第四講筆記

 

第四講   李群與李代數

       感覺SLAM十四講真的是深入淺出。第四講是李群和李代數，為什么要引入這個概念呢？

　　在SLAM中位姿是未知的，我們需要解決“什么樣的相機位姿最符合當前觀測數據”，一種典型的方式是把它構建成一個優化問題，求解最優的R,t，使誤差最小化。但旋轉矩陣自身帶有約束（正交且行列式為1），給優化帶來困難。通過李群李代數可以將問題轉化為無約束的優化問題。

4.1李群李代數基礎

　　例 ：特殊正交群SO（3）和特殊歐式群SE(3)對加法不封閉，對乘法封閉

　　對於這種只有一個運算的集合，稱之為群

　　

       群:

　　是一種集合加上一種運算的代數結構。群上運算的條件如下：

　　

      李群是指具有連續（光滑）性質的群。  “想象一個剛體連續在空間中運動”---李群、

     通過旋轉矩陣引出李代數。過程簡單總結一下。主要是從 

　 1. 旋轉矩陣的正交性RR^T=I 出發，在實際中相機位姿隨時間變化，將其構造為時間t的函數   R(t)R(t)^T=I .

     2.然后兩邊對時間求導，得出R(t)'R(t)^T 是反對稱矩陣，就可用一個向量表示。

     3.最后把R（t）在t=0處泰勒展開，解微分方程，得出旋轉矩陣可以由exp(Φ t)計算出。這個Φ描述了R在局部的導數關系，這就是SO(3)上的李代數。

        

    下面為純公式：

  ->  ->兩邊求導-》

 

 

-> ->兩邊右乘R（t）

->泰勒展開。設R(0)=I->

 

 

 

 ->解微分方程

 

 

   意義：

 

 

    李代數的定義

　 李代數由一個集合v，一個數域F和一個二元運算[,]組成：

 

 

 李代數so(3)  

 

 

 

 

 

 

 

 

   其與so（3）的關系有指數映射給定

 

 

         R=exp(φ^)

李代數  se(3)

 

 

4.2指數與對數映射

　　具體推導略。主要關注與SLAM利用的公式。推出SO(3)的指數映射 

　　   指數映射即為羅德里格斯公式

 

 

             但是指數映射不是單射，可能存在多個李代數中的元素對應到同一個李群。但我們把旋轉角度固定在-+π之間就是一一對應的。

　　　SE(3)指數映射

　　　　

 

 

 

 

 

 

 

 

 

 

 4.3；李代數求導與擾動模型

      BCH公式：

　　對於兩個李代指數映射乘積：

 

 

 

 

 

 以第一個近似為例。該式告訴我們，當對一個旋轉矩陣 R2（李代數為 ϕ2）左乘一個微小旋轉矩陣 R1（李代數為 ϕ1）

時，可以近似地看作，在原有的李代數 ϕ2 上，加上了一項 Jl(ϕ2)-1ϕ1。同理，第二個近似描述了右乘一個微小位移的情況。於是，李代數在 BCH
近似下，分成了左乘近似和右乘近似兩種，在使用時我們須加注意，使用的是左乘模型還是右乘模型。

 

 

 假定對某個旋轉 R，對應的李代數為 ϕ。我們給它左乘一個微小旋轉，記作 ∆R，對應的李代數為 ∆ϕ。

那么，在李群上，得到的結果就是 ∆R · R，而在李代數上，根據 BCH近似，為： Jl-1(ϕ)∆ϕ + ϕ。合並起來，

可以簡單地寫成：

 

 

 反之，如果我們在李代數上進行加法，讓一個 ϕ 加上 ∆ϕ，那么可以近似為李群上帶左右雅可比的乘法：

 

 

 SO(3)的李代數求導

　　SLAM中，設某個時刻相機位姿為T，觀察到了世界坐標系下位於p的點，產生了一個觀測數據z，z=Tp+w.

w為隨機噪聲。   

    誤差e=z-Tp

    所以我們就是在n組這樣的數據中找一個最優T是的整體誤差最小化

  

 

我們經常會構建與位姿有關的函數，然后討論該函數關於位姿的導數，以調整當前的估計值

 

 使用李代數解決求導問題的思路分為兩種：
1. 用李代數表示姿態，然后對根據李代數加法來對李代數求導。
2. 對李群左乘或右乘微小擾動，然后對該擾動求導，稱為左擾動和右擾動模型。
第一種方式對應到李代數的求導模型，而第二種則對應到擾動模型。

假設我們對一個空間點 p 進行了旋轉，得到了 Rp。現在，要計算旋轉之后點的坐標相對於旋轉的導數

-》

 

 

 

 

第二行的近似為 BCH 線性近似，第三行為泰勒展開舍去高階項后近似，第四行至第五行將反對稱符號看作叉積，交換之后變號。

 

 

 

SO3擾動模型（左乘）

  

 

 

 

 

 

SE3 李代數求導

 

   評估軌跡誤差。

  估計軌跡與真實軌跡的差異。真實軌跡由更高精度的系統獲得。

常見的有ATE,絕對軌跡誤差，

RPE，相對軌跡誤差。

4.4 實踐 ：SOPhus

　　

#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度的旋轉矩陣
  Matrix3d 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 << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
  cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
  cout << "they are equal" << endl;

  // 使用對數映射獲得它的李代數
  Vector3d so3 = SO3_R.log();
  cout << "so3 = " << so3.transpose() << endl;
  // hat 為向量到反對稱矩陣
  cout << "so3 hat=\n" << Sophus::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;
  // 對SE(3)操作大同小異
  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::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
  cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

  return 0;
}

打開終端，找到sophus所在位置

mkdir build
cd build/
cmake ..
make

如果按照十四講的做法會找不到  “SE3.CPP”，我把include_directories(${Sophus_INCLUDE_DIRS})注釋掉

換成：include_directories("/home/leo/slambook2/3rdparty/Sophus/")。

然后執行編譯“四件套”。執行結果如下：

　例2：評估軌跡誤差

　　這個例子中偶groundtruth.txt和estimated.txt.兩條軌跡，用以下代碼讀取軌跡，計算誤差。

　　

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

using namespace Sophus;
using namespace std;

string groundtruth_file = "./groundtruth.txt";
string estimated_file = "./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
  }

}

 

RMSE = 2.20728   下面是可視化以后的圖形

4.5相似變換群和李代數sim(3)