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