slam十四讲第四次作业_SLAM十四讲第三次作业-深蓝学院

最新推荐文章于 2022-07-25 20:45:55 发布
最新推荐文章于 2022-07-25 20:45:55 发布
阅读量243 收藏 1
点赞数 1
文章标签： slam十四讲第四次作业
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_32440261/article/details/113541060
版权

f805b365eb07e23db6fae1e226fff7ca.png

8f24b1e40172776d88e2fe2b91d8a773.png

9d4fda39a19c91f8ad0f490f307b886e.png

0a94d13c1f4632b548e36f1fba97912b.png

d681708e8c7ee87d0f9cdf77b5e61446.png

99a17f04fa1c3ac5890311b08af0ea12.png

094459e9816eaa3152f76f0c01c06384.png

cbe15a42b2295e46be3e7c2996d8aca0.png

0abd9407c77640767c2d0442d2f358e4.png

cmake_minimum_required(VERSION 2.8)

project(draw_trajectory)

# Check C++11 or C++0x support

include(CheckCXXCompilerFlag)

CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)

CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)

if(COMPILER_SUPPORTS_CXX11)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")

add_definitions(-DCOMPILEDWITHC11)

message(STATUS "Using flag -std=c++11.")

elseif(COMPILER_SUPPORTS_CXX0X)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")

add_definitions(-DCOMPILEDWITHC0X)

message(STATUS "Using flag -std=c++0x.")

else()

message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")

endif()

include_directories( "/usr/include/eigen3" )

find_package(Pangolin REQUIRED) #P要大写

include_directories(${Pangolin_INCLUDE_DIRS})

find_package(Sophus REQUIRED)

include_directories(${Sophus_INCLUDE_DIRS})

add_executable(draw_trajectory draw_trajectory.cpp)

target_link_libraries( draw_trajectory ${Sophus_LIBRARIES} ${Pangolin_LIBRARIES})

#include

#include

#include

#include

// need pangolin for plotting trajectory

#include

using namespace std;

// path to trajectory file

string trajectory_file = "./trajectory.txt";

// function for plotting trajectory, don't edit this code

// start point is red and end point is blue

void DrawTrajectory(vector<:se3 eigen::aligned_allocator>>);

int main(int argc, char **argv) {

vector<:se3 eigen::aligned_allocator>> poses;

// implement pose reading code

ifstream fin(trajectory_file); //从文件中读取数据

double t,tx,ty,tz,qx,qy,qz,qw;

string line;

while(getline(fin,line))

{

istringstream record(line); //从string读取数据

record>>t>>tx>>ty>>tz>>qx>>qy>>qz>>qw;

Eigen::Vector3d t(tx,ty,tz);

Eigen::Quaterniond q = Eigen::Quaterniond(qw,qx,qy,qz).normalized(); //四元数的顺序要注意

Sophus::SE3 SE3_qt(q,t);

poses.push_back(SE3_qt);

}

// draw trajectory in pangolin

DrawTrajectory(poses);

return 0;

}

/*******************************************************************************************/

void DrawTrajectory(vector<:se3 eigen::aligned_allocator>> 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());

glBegin(GL_LINES);

auto p1 = poses[i], p2 = poses[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

}

}

8ae0b2a5c42772dc35c1a643354d15c9.png

5541d16f4230024570c98178c551fdbc.png

cmake_minimum_required(VERSION 2.8)

project(draw_trajectory)

# Check C++11 or C++0x support

include(CheckCXXCompilerFlag)

CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)

CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)

if(COMPILER_SUPPORTS_CXX11)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")

add_definitions(-DCOMPILEDWITHC11)

message(STATUS "Using flag -std=c++11.")

elseif(COMPILER_SUPPORTS_CXX0X)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")

add_definitions(-DCOMPILEDWITHC0X)

message(STATUS "Using flag -std=c++0x.")

else()

message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")

endif()

include_directories( "/usr/include/eigen3" )

find_package(Pangolin REQUIRED) #P要大写

include_directories(${Pangolin_INCLUDE_DIRS})

find_package(Sophus REQUIRED)

include_directories(${Sophus_INCLUDE_DIRS})

add_executable(error_trajectory error_trajectory.cpp)

target_link_libraries( error_trajectory ${Sophus_LIBRARIES} ${Pangolin_LIBRARIES})

#include

#include

#include

#include

#include

#include

#include

#include

using namespace std;

using namespace Eigen;

void ReadData(string FileName ,vector<:se3 eigen::aligned_allocator>> &poses);

double ErrorTrajectory(vector<:se3 eigen::aligned_allocator>> poses_g,

vector<:se3 eigen::aligned_allocator>> poses_e);

void DrawTrajectory(vector<:se3 eigen::aligned_allocator>> poses_g,

vector<:se3 eigen::aligned_allocator>> poses_e);

int main(int argc, char **argv)

{

string GroundFile = "./groundtruth.txt";

string ErrorFile = "./estimated.txt";

double trajectory_error_RMSE = 0;

vector<:se3 eigen::aligned_allocator>> poses_g;

vector<:se3 eigen::aligned_allocator>> poses_e;

ReadData(GroundFile,poses_g);

ReadData(ErrorFile,poses_e);

trajectory_error_RMSE = ErrorTrajectory(poses_g, poses_e);

cout<

DrawTrajectory(poses_g,poses_e);

}

/***************************读取文件的数据，并存储到vector类型的pose中**************************************/

void ReadData(string FileName ,vector<:se3 eigen::aligned_allocator>> &poses)

{

ifstream fin(FileName); //从文件中读取数据

//这句话一定要加上，保证能够正确读取文件。如果没有正确读取，结果显示-nan

if(!fin.is_open()){

cout<

return;

}

double t,tx,ty,tz,qx,qy,qz,qw;

string line;

while(getline(fin,line)) {

istringstream record(line); //从string读取数据

record >> t >> tx >> ty >> tz >> qx >> qy >> qz >> qw;

Eigen::Vector3d t(tx, ty, tz);

Eigen::Quaterniond q = Eigen::Quaterniond(qw, qx, qy, qz).normalized(); //四元数的顺序要注意

Sophus::SE3 SE3_qt(q, t);

poses.push_back(SE3_qt);

}

}

/*******************************计算轨迹误差*********************************************/

double ErrorTrajectory(vector<:se3 eigen::aligned_allocator>> poses_g,

vector<:se3 eigen::aligned_allocator>> poses_e )

{

double RMSE = 0;

Matrix se3;

vector error;

for(int i=0;i

se3=(poses_g[i].inverse()*poses_e[i]).log(); //这里的se3为向量形式，求log之后是向量形式

//cout<

error.push_back( se3.squaredNorm() ); //二范数

// cout<

}

for(int i=0; i

RMSE += error[i];

}

RMSE /= double(error.size());

RMSE = sqrt(RMSE);

return RMSE;

}

/*****************************绘制轨迹*******************************************/

void DrawTrajectory(vector<:se3 eigen::aligned_allocator>> poses_g,

vector<:se3 eigen::aligned_allocator>> poses_e) {

if (poses_g.empty() || poses_e.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);//混合函数glBlendFunc( GLenum sfactor , GLenum dfactor );sfactor 源混合因子dfactor 目标混合因子

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(上向量)

);

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_g.size() - 1; i++) {

glColor3f(1 - (float) i / poses_g.size(), 0.0f, (float) i / poses_g.size());

glBegin(GL_LINES);

auto p1 = poses_g[i], p2 = poses_g[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 j = 0; j < poses_e.size() - 1; j++) {

glColor3f(1 - (float) j / poses_e.size(), 0.0f, (float) j / poses_e.size());

glBegin(GL_LINES);

auto p1 = poses_e[j], p2 = poses_e[j + 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

}

}

结果展示：

4d8ff407780ae987d3e878ed6a748822.png

008972
关注
  • 1
    点赞
  • 踩
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
视觉SLAM入门 -- 学习笔记 - Part3
～勿据散客～的博客
10-23 335
2 群的性质 1. fZ;+g 是否为群？若是，验证其满⾜群定义；若不是，说明理由。 2. fN;+g 是否为群？若是，验证其满⾜群定义；若不是，说明理由。 其中Z 为整数集，N 为⾃然数集。 3 验证向量叉乘的李代数性质 附： ⾃反性是指⾃⼰与⾃⼰的运算为零 4 推导SE(3) 的指数映射(2 分，约1 小时) 课上给出了SO(3) 的指数映射推导，但对于SE(3)，仅介绍了结论，没有给出详细推导。请你完成SE(3) 指数映射部分，有关左雅可⽐的详细推导。 这也正是课件⾥提到的左雅可⽐。 提⽰：
高翔第三讲轨迹的描绘
sunshine
03-01 1984
高翔第三讲轨迹的描绘 我们通常会记录机器人的运动轨迹,来观察它的运动是否符合预期。大部分数据集都会提供标准轨迹 以供参考,如 kitti、TUM-RGBD 等。这些文件会有各自的格式,但首先你要理解它的内容。记世界坐标 系为 W ,机器人坐标系为 C,那么机器人的运动可以用 T W C 或 T CW 来描述。现在,我们希望画出机器 人在世界当中的运动轨迹,请回答以下问题: 事实上,T W C 的...
高翔视觉SLAM十四讲第三节课的课后作业中，解决由于模板类Sophus而导致的问题，第6题draw_trajectory.cpp新版代码
A1451ASD的博客
07-25 227
适用于模板类的Sophus函数的draw_trajectory.cpp
.cxx文件_CMakeLists.txt文件如何编写？（一 基础篇）
weixin_39611161的博客
11-27 1204
本文首发于公众号「3D视觉工坊」——CMakeLists.txt文件如何编写？本文以linux平台下CMakeLists.txt文件书写方法总结。一 开头通用模块1.1 cmake版本要求cmake_minimum_required( VERSION 2.8 )#工程文件名loop_closure,可任取project( loop_closure )1.2 编译模式IF(NOT CMAKE_BUI...
OpenGL的pangolin库绘制3D轨迹图
weixin_46417419的博客
04-07 1662
示例代码 #include <pangolin/pangolin.h> #include <Eigen/Geometry> #include <Eigen/Core> #include <unistd.h> // 本例演示了如何画出一个预先存储的轨迹 using namespace std; using namespace Eigen; // path to trajectory file string trajectory_file = "/home/
尝试运行示例：显示运动轨迹
一个搬运知识的笨小孩
11-29 313
#include <iostream> #include <Eigen/Core> #include <unistd.h> #include <pangolin/pangolin.h> using namespace std; using namespace Eigen; string trajectory_file = "/home/aeronautics/slambook2/ch3/examples/trajectory.txt"; void Dra
视觉SLAM十四讲CH3代码解析及课后习题详解
生活如此美好，致敬每一个热爱“热爱”的人！
10-19 4833
eigenMatrix.cpp #include <iostream> using namespace std; #include <ctime> // Eigen 核心部分 #include <Eigen/Core> // 稠密矩阵的代数运算（逆，特征值等） #include <Eigen/Dense> using namespace std; using namespace Eigen; #define MATRIX_SIZE 50 ////宏
视觉SLAM十四讲第3次作业-第4题-推导 SE(3)的指数映射（详解版）
06-19
视觉SLAM十四讲，深蓝学院， SE(3)的指数映射
高翔视觉slam十四讲有完整库的代码文件
09-20
因为高翔视觉slam十四讲文件里面的，3rdparty文件夹下下载下来是空的，所以这里有专门的已经下载好了的文件夹供大家使用，不用再自己下载了，避免因版本不同而编译错误。 操作： 1、库文件中如果有build文件夹的...
视觉slam十四讲
02-22
视觉slam十四讲
SLAM十四讲(第二版)所有第三方库库.zip
03-31
SLAM十四讲（第二版）所需库，包括：C e r e s，G 2 O，S o p h u s,D B O W 等等。
SLAM十四讲优秀作业代码及文档.txt
08-28
SLAM十四讲的课件代码，以及标准答案 帮助大家快速入门SLAM
Cmake时出现不支持c++11的error
weixin_40963876的博客
06-09 7114
Cmake时出现error：no c++11 support, Please use a different c++ complier 问题原因： # Check C++11 or C++0x support include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX
深蓝学院-视觉SLAM十四讲-第四章作业
记录和分享
10-26 1544
第四节课作业 2.图像去畸变 本题主要内容是根据提供的公式和参数来实现图像的去畸变。 畸变前后坐标变化公式： {xdistorted=x(1+k1r2+k2r4)+2p1xy+p2(r2+2x2)ydistorted=y(1+k1r2+k2r4)+p1(r2+2y2)+2p2xy \begin{cases} x_{distorted} = x(1+k_1r^2+k_2r^4)+2p_1xy+p_2(r^2+2x^2) \\ y_{distorted} = y(1+k_1r^2+k_2r^4)+p_1(r^2
视觉SLAM学习分享--显示运动轨迹
qq_51106967的博客
08-29 750
1.效果图。 2.plotTrajectory.cpp #include <pangolin/pangolin.h> #include <Eigen/Core> #include <unistd.h> // 本例演示了如何画出一个预先存储的轨迹 using namespace std; using namespace Eigen; // path to trajectory file string trajectory_file = "../trajec.
SLAM十四讲第三次作业-深蓝学院
weixin_41074793的博客
12-06 3482
cmake_minimum_required(VERSION 2.8) project(draw_trajectory) # Check C++11 or C++0x support include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11...
视觉SLAM理论与实践第三讲习题
qq_38795436的博客
04-05 268
1、是 2、是 （都满足封闭性、结合率、么元、逆） 1、 2、
高博SLAM基础课第三次作业——轨迹绘图
任仁人的博客
10-20 868
作业描述 我为你准备了一个轨迹文件(code/trajectory.txt)。该文件的每一行由若干个数据组成,格式为 [t, t x , t y , t z , q x , q y , q z , q w ], 其中 t 为时间,t x , t y , t z 为 T W C 的平移部分,q x , q y , q z , q w 是四元数表示的 T W C 的旋转部分,q w 为四元数实部。同...
视觉SLAM十四讲课后习题之三：李群和李代数
上帝禁区
01-27 404
draw_trajectory.cpp #include &amp;lt;sophus/se3.h&amp;gt; #include &amp;lt;string&amp;gt; #include &amp;lt;iostream&amp;gt; #include &amp;lt;fstream&amp;gt; // need pangolin for plotting trajectory #include &amp;lt;p
视觉slam十四讲 第二版 第三讲
最新发布
08-21
- *1* *2* *3* [《视觉SLAM十四讲 第二版》笔记及课后习题（第三讲）](https://blog.csdn.net/weixin_43619346/article/details/103186005)[target="_blank" data-report-click={"spm":"1018.2226.3001.9630","extra...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值