Sophus::SO3SE3_3dso36dse3_trajectoryErr

引言

ch4.

0.常用

李群：

• SO(3);R(3*3)
• SE(3);T(4*4)

李代数：

• so(3);(3*1)=>三个旋转
• se(3);(6*1)=>前三维平移，后三维旋转

定义：

• Sophus::SO3 SO3_R(R ); //旋转矩阵定义李群SO(3)
• Sophus::SO3 SO3_q(Q );//四元数定义李群SO(3)
• Sophus::SE3 SE3_Rt(R, t);//R,t构造SE(3)
• Sophus::SE3 SE3_qt(q,t); //q,t构造SE(3)
• Sophus::Vector3d so3 = SO3_R.log();//李代数so3为李群SO(3)的对数映射
• Sophus::Vector6d se3 = SE3_Rt.log();//李代数se(3) 是一个6维向量为李群SE3 的对数映射
• SE3_Rt = se3.exp();//李群SE3是李代数se3的指数映射
• SO3_R = so3.exp();//李群SO3是李代数so3的指数映射

注意：

• vector<Sophus::SE3, Eigen::aligned_allocator< Sophus::SE3>> poses;//这句实际上表达的是vector< Sophus::SE3> poses的意思。但是在Eigen管理内存和C++11中的方法是不一样的，所以需要单独强调元素的内存分配和管理。(特别注意Eigen库数据结构内存对齐问题)

1.Sophus库

Eigen库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。但是Eigen库提供了集合模块，但没有提供李代数的支持。一个较好的李群和李代数的库是Sophus库，它很好的支持了SO(3),so(3),SE(3)和se(3)。Sophus库是基于Eigen基础上开发的，继承了Eigen库中的定义的各个类。因此在使用Eigen库中的类时，既可以使用Eigen命名空间，也可以使用Sophus命名空间。但最好使用Sophus命名空间，便于阅读。

Eigen::Matrix3d和Sophus::Matrix3d
Eigen::Vector3d和Sophus::Vector3d

此外，为了方便说明SE(4)和se(4)，Sophus库还typedef了Vector4d、Matrix4d、Vector6d和Matrix6d等，即：

Sophus::Vector4d
Sophus::Matrix4d
Sophus::Vector6d
Sophus::Matrix6d

2.Sophus安装方式：

git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff
mkdir build 
cd build
cmake ..
make
sudo make install

3.Sophus的使用教程：

几个需要注意的地方：

1. Sophus库的各种形式的表示如下：

   李代数so(3)：Sophus：：Vector3d //因为so(3)仅仅只是一个普通的3维向量
   李代数se(3)：Sophus：：Vector6d //因为se(3)仅仅只是一个普通的6维向量

2. SO3的构造函数为：

   SO3 ();
   SO3 (const SO3 & other);
   explicit SO3 (const Matrix3d & _R);
   explicit SO3 (const Quaterniond & unit_quaternion);
   SO3 (double rot_x, double rot_y, double rot_z);

3. SE3的构造函数为：
   SE3 ();
   SE3 (const SO3 & so3,const Vector3d & translation);
   SE3 (const Matrix3d & rotation_matrix,const Vector3d & translation);
   SE3 (const Quaterniond & unit_quaternion,const Vector3d & translation_);
   SE3 (const SE3 & other);

4. SO3,SE3和se3的输出说明：
   尽管SO3对应于矩阵群，但是SO3在使用cout时是以so3形式输出的，输出的是一个3维向量.SE3在使用cout输出时输出的是一个6维向量，其中前3维为对应的so3的值，后3维为实际的平移向量t.se3在使用cout输出时输出的也是一个6维向量，但是其前3维为平移值ρρ（注意此时的ρρ与SE3输出的t是不同的，t=Jρρ,其中J是雅克比矩阵），后3维为其对应的so3.

4.SO3，so3，SE3和se3初始化以及相互转换关系

1.转换关系图：

2.代码示意：
SO3，so3，SE3和se3初始化以及相互转换useSophusc:

#include <iostream>
#include <cmath>
using namespace std; 

#include <Eigen/Core>
#include <Eigen/Geometry>

// 李群李代数 库 
#include "sophus/so3.h"
#include "sophus/se3.h"
/*********************************
 * sophus 库安装 
 * 本库为来版本 非模板的版本
 * git clone https://github.com//strasdat/Sophus.git
 * git checkout a621ff   版本
 * 在cmake编译
 * *
 欧拉角定义：
 旋转向量转旋转矩阵            Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
 旋转矩阵转四元数              Eigen::Quaterniond q(R);            // 或者四元数(从旋转矩阵构造)
 平移Eigen::Vector3d t(1,0,0);    // 沿X轴平移1


 SO(n) 特殊正交群       对应 n*n 的旋转矩阵  群(集合)
 SE(n+1) 特殊欧式群     对应 n*n 的旋转矩阵和 n*1的平移向量组合成的变换矩阵 群(集合)
 so(n)  对应的李代数为so(n)  n×1 列向量,使得向量和代数一一对应可以使用代数的更新方法来更新矩阵


 SO(3)  表示三维空间的 旋转矩阵 集合  3×3
 SE(3)  表示三维空间的 变换矩阵 集合  4×4
 李代数 so3的本质就是个三维向量，直接Eigen::Vector3d定义.3个旋转
 李代数 se3的本质就是个六维向量，直接Eigen::Matrix<double,6,1>定义,3个旋转 + 3个平移


 旋转向量定义的李群SO(3)       Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // 亦可从旋转向量构造  这里注意，不是旋转向量的三个坐标值，有点像欧拉角构造。
 旋转矩阵定义的李群SO(3)       Sophus::SO3 SO3_R(R);        // Sophus::SO(3)可以直接从旋转矩阵构造
 四元素定义的李群SO(3)         Sophus::SO3 SO3_q(q);
 从旋转矩阵和平移t构造SE3      Sophus::SE3 SE3_Rt(R,t);     // 从R,t构造SE(3)
 从四元素和平移t构造SE3        Sophus::SE3 SE3_qt(q,t);     // 从q,t构造SE(3)


 李代数so3为李群SO(3)的对数映射 Eigen::Vector3d so3 = SO3_R.log();
 李代数se(3)是一个6维向量,为李群SE3 的对数映射
 typedef Eigen::Matrix<double,6,1> Vector6d;// Vector6d指代Eigen::Matrix<double,6,1>
 Vector6d se3 = SE3_Rt.log();
 李代数指数映射成旋转矩阵对应的李群
 Eigen::Vector3d so33 (1, 1, 1);
 Sophus::SO3 SO3 =Sophus::SO3::exp(so33);


 Sophus::SO3::hat(so3) //hat为向量到反对称矩阵,相当于^运算.
 Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose() //vee为反对称矩阵到向量,相当于下尖尖运算 .
 **********************************/



int main( int argc, char** argv )
{
 /*******************************************/
  // 旋转矩阵群/////特殊正交群仅有旋转没有平移
  // 沿Z轴转90度的旋转矩阵
  // 旋转向量　角度　轴　　罗德里格公式进行转换为旋转矩阵　　　
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
    cout<<"RotationMatrix R: \n"<<R<<endl;
    
    /***李群*****/
    Sophus::SO3 SO3_R(R);               // Sophus::SO(3)可以直接从旋转矩阵构造
    Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // 亦可从旋转向量构造  这里注意，不是旋转向量的三个坐标值，有点像欧拉角构造。
    /*
     SO3  ::SO3(double rot_x, double rot_y, double rot_z)  
	  {  
	    unit_quaternion_  
		= (SO3::exp(Vector3d(rot_x, 0.f, 0.f))  
		  *SO3::exp(Vector3d(0.f, rot_y, 0.f))  
		  *SO3::exp(Vector3d(0.f, 0.f, rot_z))).unit_quaternion_;  
	  }
	显示的貌似是三个过程，先转Ｘ轴，再转Ｙ轴，再转Ｚ轴，完全跟旋转向量不搭边。瞅着过程有点像欧拉角的过程，三个轴分了三步。  
	我就有一个(1, 1, 1)旋转向量，如何构造成SO3呢？也就是让它输出(1, 1, 1)。
        Eigen::Vector3d so33 (1, 1, 1);  
        Sophus::SO3 SO3 =Sophus::SO3::exp(so33);  //李代数 指数映射成 旋转矩阵 对于的 李群
         cout<<"SO3=\n"<<SO3<<endl;  
     */
    Eigen::Quaterniond q(R);            // 或者四元数(从旋转矩阵构造)
    Sophus::SO3 SO3_q( q );
    // 上述表达方式都是等价的
    // 输出SO(3)时，以so(3)形式输出
    //从输出的形式可以看出，虽然SO3是李群，是旋转矩阵，但是输出形式还是向量（被转化成李代数输出）。
    // 重载了 << 运算符  out_str << so3.log().transpose() << std::endl;  
    cout<<"SO(3) from matrix: "<<SO3_R<<endl;  //SO(3) from matrix:      0      0 1.5708  
    cout<<"SO(3) from vector: "<<SO3_v<<endl;
    cout<<"SO(3) from quaternion :"<<SO3_q<<endl;
    
    /****李代数*****/
    // 使用对数映射获得它的李代数
    // 所以，李代数 so3的本质就是个三维向量，直接Eigen::Vector3d定义。
    Eigen::Vector3d so3 = SO3_R.log();
    cout<<"so3 = "<<so3.transpose()<<endl;
    // hat 为向量 到反对称矩阵  相当于　^　运算。
    cout<<"so3 hat=\n"<<Sophus::SO3::hat(so3)<<endl;
    // 相对的，vee为 反对称矩阵 到 向量  相当于下尖尖运算 
    cout<<"so3 hat vee= "<<Sophus::SO3::vee( Sophus::SO3::hat(so3) ).transpose()<<endl; // transpose纯粹是为了输出美观一些
    
    /****李代数求导　更新*****/
    // 增量扰动模型的更新
    Eigen::Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3)*SO3_R;// 增量指数映射×原李代数
    cout<<"SO3 updated = "<<SO3_updated<<endl;
    
    
    /********************萌萌的分割线*********************************/
    /************特殊欧式群　变换矩阵群　有旋转有平移*********************/
    cout<<"************我是分割线*************"<<endl;
    // 李群 对SE(3)操作大同小异
    Eigen::Vector3d t(1,0,0);           // 沿X轴平移1
    Sophus::SE3 SE3_Rt(R, t);           // 从R,t构造SE(3)
    Sophus::SE3 SE3_qt(q,t);            // 从q,t构造SE(3)
    cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;
    cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;
    // 李代数se(3) 是一个六维向量，方便起见先typedef一下
    typedef Eigen::Matrix<double,6,1> Vector6d;// Vector6d指代　Eigen::Matrix<double,6,1>
    Vector6d se3 = SE3_Rt.log();
    cout<<"se3 = "<<se3.transpose()<<endl;
    // 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.
    // 同样的，有hat和vee两个算符
    cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;
    cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;
    
    // 最后，演示一下更新
    Vector6d update_se3; //更新量
    update_se3.setZero();
    update_se3(0,0) = 1e-4d;
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
    cout<<"SE3 updated = "<<endl<<SE3_updated.matrix()<<endl;
    
    return 0;
}

5.求轨迹误差

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>



//xiao的方法更直接易懂
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>

using namespace std;

// path to trajectory file
string trajectory_file =  "./../trajectory.txt";  //"./trajectory.txt";
string groundtruth_file =  "./../groundtruth.txt";  //"./groundtruth.txt";
string estimated_file =  "./../estimated.txt";  //"./estimated.txt";

// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);
void Draw3Trajectory();
void computeErr();

int main(int argc, char **argv) {
	//Draw3Trajectory();
	computeErr();
    return 0;
}


void computeErr()
{

	ifstream g_file(groundtruth_file);
	ifstream e_file(estimated_file);


    if (!g_file||!e_file) {
      cout<<"unable to read file!"<<endl;
	exit(1);
    }
    
	int count = 0;
	double rmse_square = 0;

	double data1[8] = {0};	
	double data2[8] = {0};
    while(!g_file.eof() && !e_file.eof()) {
	  for (auto &p:data1)
	  		g_file >> p;
      Eigen::Quaterniond q1(data1[7], data1[4], data1[5], data1[6]);
      Eigen::Vector3d t1(data1[1], data1[2], data1[3]);
      Sophus::SE3 SE3_g(q1,t1);
     
      for (auto &p:data2)
			e_file >> p;
	  Eigen::Quaterniond q2(data2[7], data2[4], data2[5], data2[6]);
      Eigen::Vector3d t2(data2[1], data2[2], data2[3]);
	  Sophus::SE3 SE3_e(q2,t2);


		Eigen::Matrix<double,4,4> m = (SE3_g.matrix().inverse())*SE3_e.matrix();//公式！
		Eigen::Matrix<double,3,3> R = m.topLeftCorner<3,3>();//函数名很直观
		Eigen::Matrix<double,3,1> t = m.topRightCorner<3,1>();
		Eigen::Quaterniond q(R);
		Eigen::Vector3d tt(t);
		Sophus::SE3 SE3_dot(q,tt);	//q,t构建李群
		Sophus::Vector6d se3_dot = SE3_dot.log();//转为李代数
		
		double err = se3_dot.norm();//二范数
		rmse_square = (rmse_square + err*err);	//公式
		count ++ ;
    }
    g_file.close();
	e_file.close();
	
	double rmse = sqrt(rmse_square/count);
	cout<< "the RMSE is:\n"<<rmse<<endl;

}



//画三段轨迹
void Draw3Trajectory()
{
	
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;

    /// implement pose reading code
    // start your code here (5~10 lines)
    ///**
    // Read1
    ifstream t_file(trajectory_file);
	ifstream g_file(groundtruth_file);
	ifstream e_file(estimated_file);
    if (!t_file) {
      cout<<"unable to read file!"<<endl;
	exit(1);
    }
    
    while(!t_file.eof()) {
	double data[8] = {0};
      for (auto &p:data)
		t_file >> p;
      Eigen::Quaterniond q(data[7], data[4], data[5], data[6]);
      Eigen::Vector3d t(data[1], data[2], data[3]);
      Sophus::SE3 pose(q,t);
      poses.push_back(pose);
    }
    t_file.close();
    // end your code here



    if (!g_file) {
      cout<<"unable to read file!"<<endl;
	exit(1);
    }
    

    while(!g_file.eof()) {
		double data[8] = {0};
      for (auto &p:data)
		g_file >> p;
      Eigen::Quaterniond q(data[7], data[4], data[5], data[6]);
      Eigen::Vector3d t(data[1], data[2], data[3]);
      Sophus::SE3 pose(q,t);
      poses.push_back(pose);
    }
    g_file.close();



    if (!e_file) {
      cout<<"unable to read file!"<<endl;
	exit(1);
    }

    while(!e_file.eof()) {
		    double data[8] = {0};
      for (auto &p:data)
		e_file >> p;
      Eigen::Quaterniond q(data[7], data[4], data[5], data[6]);
      Eigen::Vector3d t(data[1], data[2], data[3]);
      Sophus::SE3 pose(q,t);
      poses.push_back(pose);
    }
    e_file.close();
	//*/
	//把三个轨迹都导入poses一起画出来就是，不用定义三个poses;
	//这种思路比另一个大神的要强一点！！

    // draw trajectory in pangolin
    DrawTrajectory(poses);
}





/*******************************************************************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses) {
    if (poses.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // 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 < poses.size() - 1; i++) {
            glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size()); // start from red and end with blue color
            // start the drawing
            glBegin(GL_LINES);
            auto p1 = poses[i], p2 = poses[i + 1];
            // define two vertex, 1st is starting point, 2nd is ending point.
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
            // finish the drawing
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }

}