深蓝-视觉slam-第三节习题

最新推荐文章于 2022-12-12 22:02:54 发布

思考之路

最新推荐文章于 2022-12-12 22:02:54 发布

阅读量1.4k

点赞数 2

分类专栏：深蓝orb-slam 文章标签： slam

本文链接：https://blog.csdn.net/weixin_46417419/article/details/124027080

版权

深蓝orb-slam 专栏收录该内容

22 篇文章 17 订阅

订阅专栏

1，群的性质
在这里插入图片描述
群(Group)：一种集合 + 一种运算的代数结构。把集合记作A，运算记作.,群可记作G =(A,.)。能否成群需要满足以下几个性质：
（1）.封闭性： $\forall$ a1,a2 $\in$ A, a1 $\cdot$ a2 $\in$ A
（2）.结合律： $\forall$ a1,a2,a3 $\in$ A, (a1 $\cdot$ a2) $\cdot$ a3 = a1 $\cdot$ (a2 $\cdot$ a3)
（3）.幺元： $\exists$ a0 $\in$ A, s.t $\forall$ a $\in$ A, a0 $\cdot$ a = a $\cdot$ a0 =a.
（4）.逆： $\forall$ a $\in$ A, $\exists$ a-1 $\in$ A, s.t a $\cdot$ a-1 = a0.

1,{Z,+}是群
所有的整数都属于Z，任意两整数相加还是整数还是属于Z集合，满足了封闭性。
整数的加法满足结合律
幺元满足整数集合加法成群的幺元是0，因为0加任何整数都等于整数本身。
整数对应的负值是该整数的逆，因为整数加该整数的负数等于0 ，0就是幺元，而且还属于该集合
以上四条都满足成群的要求，所以{Z,+}成群。
2,{N,+}不是群
找不到集合中的一个元素满足幺元的性质，即无幺元
同样也找不到唯一的一个元素满足逆的性质，即无逆
所以{N, +}不成群
3,阿贝尔群：又称为交换群或加群，是这样一类群：
它由自身的集合G和二元运算*构成。它除了满足一般的群公理，G有单位元，所有G的元素都有逆元之外，还满足交换律和结合律，群元素乘积的值与乘法运算的次序无关。
矩阵一般不形成在乘法下的阿贝尔群，因为矩阵的乘法一般是不可交换的，即元素乘积的值和乘法的运算次序有关。
3，验证向量叉乘的李代数性质
在这里插入图片描述
1，封闭性：X，Y是集合V= R3中的任意两个向量元素，[X,Y]运算，表示将X向量与Y向量相称，由于X是三维的列向量，Y也是三维的列向量，两个同维度的向量作叉积，相当与X对应的反对称矩阵点乘向量Y，得到的值仍是一个3 x 1 的列向量，仍属于集合V
2，双线性：
在这里插入图片描述
3，自反性：

4,雅可比等价：
在这里插入图片描述
3，推导S（3）的指数映射

4，伴随

由https://math.stackexchange.com/questions/
2190603/derivation-of-adjoint-for-so3可知：

5，常见函数的求导应用

扰动模型：对R进行一次扰动 $\Delta$ R,看结果相对于i扰动的变化率。这个扰动可以乘在左边也可以乘在右边。扰动 $\Delta$ 对应的李代数为 $\Psi$
在这里插入图片描述

6，轨迹的描述
在这里插入图片描述
（1），在画轨迹的时候，我们可以把“”轨迹“画成一系列点组成的序列，其实就是机器人（相机）坐标系的原点在世界坐标系中的坐标，机器人坐标系的原点为OR,此时OW就是这个原点在世界坐标系下的坐标：
OW = TWR * OR = tWR这正是TWR的平移部分。因此可以直接从TWR中看出相机在何处。
（2），实现代码如下：

#include <sophus/se3.hpp>
#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include <unistd.h>

#include <pangolin/pangolin.h>

using namespace std;
using namespace Eigen;

string trajectory_file = "../trajectory.txt";
void DrawTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>>);


int main(int argc, char **argv)
{
	vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> pose;
	ifstream fin(trajectory_file);
	if(!fin) {
		cout << "cannot find trajectory file at" << trajectory_file << endl;
		return 1;
	}

	while(!fin.eof())
	{
		double time,tx,ty,tz,qx,qy,qz,qw;
		fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
		Vector3cd t(tx, ty, tz);
		Quaterniond q(qw, qx, qy, qz);
		Sophus::SE3d SE_qt(q, t);
		pose.push_back(SE_qt);
		

	}
	cout << "read total " << pose.size() << "pose entries" << endl;

	DrawTrajectory(pose);
	return 0;
}

void DrawTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> pose)
{
	if(pose.empty())
	{
		cerr << "Trajectory is empty!" << endl;
		return ;
	}
	pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768); //创建一个窗口
	glEnable(GL_DEPTH_TEST); //启动深度测试
	glEnable(GL_BLEND); //启用混合
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); //混合函数 GL_SRC_ALPHA是源混合因子，GL_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) //对应的是gluLookAt,摄像机位置,参考点位置,up vector(上向量)
	);

// 	//看SimpleDisplay中的边界的设置
	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 < pose.size() - 1; i++) {
			glColor3f(1 - (float)i / pose.size(), 0.0f, (float)i / pose.size());
			glBegin(GL_LINES);
			auto p1 = pose[i], p2 = pose[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);
	}

}

CMakeLists.txt内容：

project(trajectory)
cmake_minimum_required(VERSION 3.16.3)
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
find_package(Sophus REQUIRED)
include_directories("/usr/local/include/eigen3")
set(Sophus_LIBRARIES "/usr/local/lib/libSophus.so")
include_directories( ${Sophus_INCLUDE_DIRS})
add_executable(draw_trajectory draw_trajectory.cpp)
target_link_libraries(draw_trajectory ${Sophus_LIBRARIES})
target_link_libraries(draw_trajectory ${Pangolin_LIBRARIES})

效果：
在这里插入图片描述

7，轨迹的误差
在这里插入图片描述
实现代码：

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

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() 断言，如果括号内表达式的结果为真，则不做任何操作，如果为假，assert()就会报错，并终止程序的执行，错误输出结果为表达式，所在文件，行数
  assert(groundtruth.size() == estimated.size());

  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();   //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) {
     cout << "trajectory " << path << "not found ." << endl;
  }

  while(!fin.eof()) {
    double time , tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Vector3d t(tx, ty, tz);
    Quaterniond q(qw, qx, qy, qz);
    SE3d SE_qt(q, t);
    trajectory.push_back(SE_qt);
  }
  return trajectory;
}

void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti)
{
  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);
      glBegin(GL_LINES);
      auto p1 = gt[i], p2 = gt[i+1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);//3D平移、
      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);
  }
}

CMakeLists.txt内容：

cmake_minimum_required(VERSION 3.16.3)
project(TrajectoryError)
find_package(Pangolin REQUIRED)
find_package(Sophus REQUIRED)

include_directories(${Pangolin_INCLUDE_DIRS})
include_directories(${Sophus_INCLUDE_DIRS})

add_executable(trajectoryError trajectoryError.cpp)
target_link_libraries(trajectoryError ${Pangolin_LIBRARIES} ${Sophus_LIBRAREIS})

实现效果：
在这里插入图片描述

思考之路

关注

2
点赞
踩
4

收藏

觉得还不错? 一键收藏
0
评论
深蓝-视觉slam-第三节习题

1，群的性质群(Group)：一种集合 + 一种运算的代数结构。把集合记作A，运算记作.,群可记作G =(A,.)。能否成群需要满足以下几个性质：（1）.封闭性：∀\forall∀a1,a2∈\in∈A, a1 ⋅\cdot⋅ a2 ∈\in∈A（2）.结合律：∀\forall∀a1,a2,a3 ∈\in∈A, (a1⋅\cdot⋅a2)⋅\cdot⋅a3 = a1⋅\cdot⋅(a2⋅\cdot⋅a3)（3）.幺元：∃\exists∃a0 ∈\in∈A, s.t ∀\forall∀a ∈\i
复制链接

扫一扫