【SLAM十四讲】CH4

1.课上我们讲解了什么是群。请根据群定义，求解以下问题：

1.{Z,+}是否为群？若是，验证其满⾜群定义；若不是，说明理由。

1.1封闭性：两个整数相加依然是整数——成立
1.2结合律：整数的两元加法满足结合律——成立
1.3幺元：任何整数加上0，等于它本身——成立
1.4逆元素:任何整数加上它的相反数等于幺元0，所以逆元素是其相反数——成立
因此{Z,+}是群。

2.{N,+}是否为群？若是，验证其满⾜群定义；若不是，说明理由。其中 Z 为整数集， N 为⾃然数集。

2.1封闭性：两个自然数相加依然是自然数——成立
2.2结合律：两个自然数相加可以互换位置——成立
2.3幺元：任何一个自然数与0相加仍然是自然数本身——成立
2.4逆元素：自然数都是非负数，不存在逆元素与其相加为幺元——不成立
因此{Z，+}不是群。

2.验证向量叉乘的李代数性质

证明

3.推导 SE(3) 的指数映射

推导

4.轨迹描绘

#include "iostream"
#include "fstream"
#include "sophus/se3.hpp"
#include "pangolin/pangolin.h"
#include <unistd.h>
using namespace Sophus;

using namespace std;
typedef vector<Sophus::SE3d,Eigen::aligned_allocator<Sophus::SE3d>>  Trajectorytype;
Trajectorytype  DrawTrajectory(const Trajectorytype &gt, const Trajectorytype &esti);
Trajectorytype  ReadTrajectory(const string &path);


int main(int argc,char **argv)
{
    Trajectorytype groundtruth = ReadTrajectory("/home/rock/Downloads/slambook2-master/ch4/example/groundtruth.txt") ;
    Trajectorytype estimated  = ReadTrajectory("/home/rock/Downloads/slambook2-master/ch4/example/estimated.txt");

    //画轨迹
    DrawTrajectory(groundtruth, estimated);
    return 0;
}

//读取轨迹数据
Trajectorytype  ReadTrajectory(const string &path)
{
    //定义一个文件输入流
    ifstream fin(path);
    //定义Trajectorytype类型的堆栈
    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(qw,qx,qy,qz),Eigen::Vector3d(tx,ty,tz));
        trajectory.push_back(p1);
    }
    return trajectory;

}

Trajectorytype  DrawTrajectory(const Trajectorytype &gt, const Trajectorytype &esti)
{
    pangolin::CreateWindowAndBind("Trajectoy View",1024,768);
    //启动深度测试
    glEnable(GL_DEPTH_TEST);
    //启动色彩混合，并规定混合规则 源色*ALPHA+目标色*（1-ALPHA） ALPHA=0.8
    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.0,1.0,1.0,1.0);
        //设置线宽度
        glLineWidth(2);

        //绘制groundtruth
        for(size_t i=0;i<gt.size()-1;i++)
        {
            //设置真实轨迹为蓝色
            glColor3f(0.0,0.0,1.0);
            //开始画线段
            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();
        }
        //绘制estimated
        for(size_t i=0;i<esti.size()-1;i++)
        {
            //设置估计轨迹为红色
            glColor3f(1.0,0.0,0.0);
            //开始画线段
            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);
    }
}