SLAM十四讲第三次作业-深蓝学院
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>);
int main(int argc, char **argv) {
vector> 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> 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
}
} 
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> &poses);
double ErrorTrajectory(vector> poses_g,
vector> poses_e);
void DrawTrajectory(vector> poses_g,
vector> poses_e);
int main(int argc, char **argv)
{
string GroundFile = "./groundtruth.txt";
string ErrorFile = "./estimated.txt";
double trajectory_error_RMSE = 0;
vector> poses_g;
vector> poses_e;
ReadData(GroundFile,poses_g);
ReadData(ErrorFile,poses_e);
trajectory_error_RMSE = ErrorTrajectory(poses_g, poses_e);
cout<<"trajectory_error_RMSE = "<< trajectory_error_RMSE<> &poses)
{
ifstream fin(FileName); //从文件中读取数据
//这句话一定要加上，保证能够正确读取文件。如果没有正确读取，结果显示-nan
if(!fin.is_open()){
cout<<"No "<> 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> poses_g,
vector> poses_e )
{
double RMSE = 0;
Matrix se3;
vector error;
for(int i=0;i> poses_g,
vector> 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
}
} 结果展示：
