这段代码主要功能:
- 读取保存有pose的文件,文件格式为:timestamp tx ty tz qw qx qy qz;
- 第3个参数dataset_i表示测试的第i个数据集的轨迹,代码中,直接将第i个数据集的groundtruth写死了.
- PCL绘制轨迹,用红色表示第一个文件中的轨迹,绿色表示第二个文件中的轨迹.
**局部效果图: **
代码如下:
#include <iostream>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <boost/format.hpp> // for formating strings
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <string>
using namespace std;
int main( int argc, char** argv )
{
if(argc != 4){
cerr << "Error! ./joinMap poses1.txt poses2.txt index_of_dataset" << endl;
return 0;
}
string path1 = argv[1] , path2 = argv[2];
int dataset_i = atoi(argv[3]);
ifstream fin1, fin2;
fin1.open(path1.c_str());
fin2.open(path2.c_str());
if (!fin1 || !fin2)
{
cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
return 1;
}
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
// 新建一个点云
PointCloud::Ptr pointCloud( new PointCloud );
double t_wcn_gt1[3] = {-0.232988 , 0.178259 , 0.00207278};
double t_wcn_gt2[3] = {0.0099497 , 0.153633 , 0.0190607};
double t_wcn_lastRow1[3] = {0}; //record the lastest row of file
double t_wcn_lastRow2[3] = {0};
string str;
getline(fin1,str);
while(!fin1.eof()){
double data[8] = {0};
for(auto &d : data)
fin1 >> d;
if(fin1.eof())
break;
Eigen::Vector3d t{data[1] , data[2] , data[3]};
PointT p ;
p.x = data[1];
p.y = data[2];
p.z = data[3];
p.b = 0;
p.g = 0;
p.r = 255;
pointCloud->points.push_back( p );
t_wcn_lastRow1[0] = data[1];
t_wcn_lastRow1[1] = data[2];
t_wcn_lastRow1[2] = data[3];
}
getline(fin2,str);
while(!fin2.eof()){
double data[8] = {0};
for(auto &d : data)
fin2 >> d;
if(fin2.eof())
break;
Eigen::Vector3d t{data[1] , data[2] , data[3]};
PointT p ;
p.x = data[1];
p.y = data[2];
p.z = data[3];
p.b = 0;
p.g = 255;
p.r = 0;
pointCloud->points.push_back( p );
t_wcn_lastRow2[0] = data[1];
t_wcn_lastRow2[1] = data[2];
t_wcn_lastRow2[2] = data[3];
}
cout << t_wcn_lastRow1[0] << " " << t_wcn_lastRow1[1] << " " << t_wcn_lastRow1[2] << endl;
cout << t_wcn_lastRow2[0] << " " << t_wcn_lastRow2[1] << " " << t_wcn_lastRow2[2] << endl;
//add the groundtruth of t_wcn;
double gt_center[3];
switch(dataset_i){
case 1:
{
gt_center[0] = t_wcn_gt1[0];
gt_center[1] = t_wcn_gt1[1];
gt_center[2] = t_wcn_gt1[2];
break;
}
case 2:
{
gt_center[0] = t_wcn_gt2[0];
gt_center[1] = t_wcn_gt2[1];
gt_center[2] = t_wcn_gt2[2];
break;
}
default:
break;
}
for(double i = 0;i < 0.3;i += 0.01){
PointT p;
//groundtruth
p.b = 255; p.g = 255; p.r = 255;
p.x = gt_center[0] + i;
p.y = gt_center[1];
p.z = gt_center[2];
pointCloud->points.push_back(p);
//the lastest pose of vio
p.b = 255; p.g = 0; p.r = 255;
p.x = t_wcn_lastRow1[0] + i;
p.y = t_wcn_lastRow1[1];
p.z = t_wcn_lastRow1[2];
pointCloud->points.push_back(p);
//the lastest pose of viwo
p.b = 3; p.g = 168; p.r = 158;
p.x = t_wcn_lastRow2[0] + i;
p.y = t_wcn_lastRow2[1];
p.z = t_wcn_lastRow2[2];
pointCloud->points.push_back(p);
}
for(double i = 0;i < 0.3;i += 0.01){
PointT p;
p.b = 0; p.g = 128; p.r = 255;
p.x = gt_center[0];
p.y = gt_center[1] + i;
p.z = gt_center[2] ;
pointCloud->points.push_back(p);
//the lastest pose of vio
p.x = t_wcn_lastRow1[0];
p.y = t_wcn_lastRow1[1] + i;
p.z = t_wcn_lastRow1[2];
pointCloud->points.push_back(p);
//the lastest pose of viwo
p.x = t_wcn_lastRow2[0];
p.y = t_wcn_lastRow2[1] + i;
p.z = t_wcn_lastRow2[2];
pointCloud->points.push_back(p);
}
for(double i = 0;i < 0.5;i += 0.01){
PointT p;
p.b = 255; p.g = 0; p.r = 0;
p.x = gt_center[0];
p.y = gt_center[1] ;
p.z = gt_center[2] + i;
pointCloud->points.push_back(p);
//the lastest pose of vio
p.x = t_wcn_lastRow1[0];
p.y = t_wcn_lastRow1[1];
p.z = t_wcn_lastRow1[2] + i;
pointCloud->points.push_back(p);
//the lastest pose of viwo
p.x = t_wcn_lastRow2[0];
p.y = t_wcn_lastRow2[1];
p.z = t_wcn_lastRow2[2] + i;
pointCloud->points.push_back(p);
}
cout<<"正在将图像转换为点云..."<<endl;
pointCloud->is_dense = false;
cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
return 0;
}
下面是CMakeLists.txt的内容:
cmake_minimum_required( VERSION 2.8 )
project( point_cloud )
set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )
# opencv
find_package( OpenCV 3.0 REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )
# eigen
include_directories( "/usr/include/eigen3/" )
# pcl
find_package( PCL REQUIRED COMPONENT common io )
include_directories( ${PCL_INCLUDE_DIRS} )
add_definitions( ${PCL_DEFINITIONS} )
add_executable( joinMap joinMap.cpp )
target_link_libraries( joinMap ${OpenCV_LIBS} ${PCL_LIBRARIES} )
运行指令:
./joinMap '/home/xtl/output/f1_poses_vins_fusion.txt' '/home/xtl/output/f1_poses_viwo_fusion.txt' 1
来源：CSDN
作者：踏雪`飞鸿
链接：https://blog.csdn.net/wq1psa78/article/details/103773643