SLAM十四講第三次作業-深藍學院
阿新 • • 發佈：2018-12-06
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 <sophus/se3.h> #include <string> #include <iostream> #include <fstream> // need pangolin for plotting trajectory #include <pangolin/pangolin.h> 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<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>); int main(int argc, char **argv) { vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> 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<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> 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 <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>
#include <cmath>
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;
void ReadData(string FileName ,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &poses);
double ErrorTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_g,
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_e);
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_g,
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_e);
int main(int argc, char **argv)
{
string GroundFile = "./groundtruth.txt";
string ErrorFile = "./estimated.txt";
double trajectory_error_RMSE = 0;
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_g;
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> 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<<endl;
DrawTrajectory(poses_g,poses_e);
}
/***************************讀取檔案的資料，並存儲到vector型別的pose中**************************************/
void ReadData(string FileName ,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &poses)
{
ifstream fin(FileName); //從檔案中讀取資料
//這句話一定要加上，保證能夠正確讀取檔案。如果沒有正確讀取，結果顯示-nan
if(!fin.is_open()){
cout<<"No "<<FileName<<endl;
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<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_g,
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_e )
{
double RMSE = 0;
Matrix<double ,6,1> se3;
vector<double> error;
for(int i=0;i<poses_g.size();i++){
se3=(poses_g[i].inverse()*poses_e[i]).log(); //這裡的se3為向量形式，求log之後是向量形式
//cout<<se3.transpose()<<endl;
error.push_back( se3.squaredNorm() ); //二範數
// cout<<error[i]<<endl;
}
for(int i=0; i<poses_g.size();i++){
RMSE += error[i];
}
RMSE /= double(error.size());
RMSE = sqrt(RMSE);
return RMSE;
}
/*****************************繪製軌跡*******************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses_g,
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> 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
}
}結果展示：
