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 >, 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 >, 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);
}
}