PCL小工具二：使用kitti的GT(ground truth)建立激光点云地图

此代码合并了每一帧的激光点云数据,构成了激光点云地图.
ground truth，即真值，在本文简写成GT。
在kitti数据集中，GT代表了世界坐标系下相机的位姿真值，也就是Tcam2world
经过我的一番推导，
Pworld = Tcam2world * Tlidar2cam * Plidar
（其实不一定对hhh）
最终写成了以下代码

src/sum_liadrCloud.cpp

//合并每一帧的激光点云数据,构成GT激光点云地图.
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>    //  pcl::transformPointCloud 用到这个头文件#include <Eigen/Core>
#include <Eigen/Dense>using namespace std;
using namespace Eigen;string GT_path = "/home/jy/Desktop/2011_09_30/2011_09_30_drive_0027_sync/groundtruth/07.txt";
string LidarCloud_path = "/home/jy/Desktop/2011_09_30/2011_09_30_drive_0027_sync/velodyne_points/pcd_data";
string out_path = "/home/jy/Desktop/2011_09_30/2011_09_30_drive_0027_sync/velodyne_points/out.pcd";int main(int argc, char *argv[])
{pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);//世界lidar cloudifstream GT; //读取ground truth的数值,一行12个值,组成3*4矩阵;拼接上0001后得4*4的变换矩阵std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> vT_c2w;GT.open(GT_path);    while (!GT.eof()){Eigen::Matrix4d mT_c2w = Eigen::Matrix4d::Zero(); //camera to world ,store ground truthstring s;getline(GT,s);double gt11=0,gt12=0,gt13=0,gt14=0,gt21=0,gt22=0,gt23=0,gt24=0,gt31=0,gt32=0,gt33=0,gt34=0;if(!s.empty()){stringstream ss;ss << s;ss >> gt11;ss >> gt12;ss >> gt13;ss >> gt14;ss >> gt21;ss >> gt22;ss >> gt23;ss >> gt24;ss >> gt31;ss >> gt32;ss >> gt33;ss >> gt34;mT_c2w << gt11, gt12, gt13, gt14, gt21, gt22, gt23, gt24, gt31, gt32, gt33, gt34, 0, 0, 0, 1;}vT_c2w.push_back(mT_c2w);}// for (int i=0; i<4; ++i){//     for (int j=0; j<4; ++j){//         cout << vT_c2w[1100](i,j) << "\t";//     }//     cout << endl;// }// cout << vT_c2w.size() << endl;Eigen::Matrix4d mT_l2c = Eigen::Matrix4d::Zero(); //T_l2c矩阵,lidar to camera的变换矩阵,外参mT_l2c << 7.027555e-03, -9.999753e-01, 2.599616e-05, -7.137748e-03,-2.254837e-03, -4.184312e-05, -9.999975e-01, -7.482656e-02,9.999728e-01, 7.027479e-03, -2.255075e-03, -3.336324e-01,0,0,0,1;// Eigen::Matrix4d mT = Eigen::Matrix4d::Zero();// mT = vT_c2w[1] * mT_l2c;for (int k=1; k<vT_c2w.size(); ++k){ //遍历每一个pcd文件,对其做变换,转到世界坐标系下.string LidarCloud_path_k = LidarCloud_path + "/" + to_string(k) + ".pcd";pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_k(new pcl::PointCloud<pcl::PointXYZI>);//每一帧的lidar cloudpcl::PointCloud<pcl::PointXYZI>::Ptr cloud_k_transformed(new pcl::PointCloud<pcl::PointXYZI>);//每一帧的lidar cloudpcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>());//  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);if (pcl::io::loadPCDFile<pcl::PointXYZI>(LidarCloud_path_k, *cloud_k) == -1){PCL_ERROR("Couldn't read file k.pcd\n");return (-1);}cout << "cloud " << k << " is loaded !" << endl;Eigen::Matrix4d mT_final = vT_c2w[k] * mT_l2c;Eigen::Affine3d aT_final(mT_final);std::cout << aT_final.matrix() << std::endl;pcl::transformPointCloud (*cloud_k, *cloud_k_transformed, aT_final);pcl::VoxelGrid<pcl::PointXYZI> sor;   // 创建滤波器对象sor.setInputCloud (cloud_k_transformed);   //给滤波器对象设置需要过滤的点云sor.setLeafSize (0.10f, 0.10f, 0.10f); //设置滤波时创建的体素大小为1cm立方体sor.filter (*cloud_filtered); //执行滤波处理*cloud = *cloud + *cloud_filtered;}pcl::PCDWriter writer;// Save DoN featureswriter.write<pcl::PointXYZI> (out_path, *cloud, false);return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)set(CMAKE_BUILD_TYPE "Release")
# 添加c++ 11标准支持
set(CMAKE_CXX_FLAGS "-std=c++11 -O2")# Eigen
include_directories("/usr/include/eigen3")find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})add_executable (sum_liadrCloud src/sum_liadrCloud.cpp)
target_link_libraries(sum_liadrCloud ${PCL_LIBRARIES})

就这样~有问题可在评论区留言

PCL小工具二：使用kitti的GT(ground truth)建立激光点云地图相关推荐

【深度学习笔记】关键点检测——标签（Ground Truth）构建
首先介绍一下关键点回归的Ground Truth的构建问题,主要有两种思路,Coordinate和Heatmap,Coordinate即直接将关键点坐标作为最后网络需要回归的目标,这种情况下可以直接得 ...
图像中里面的Ground Truth
经常会在一些项目中,遇到Ground Truth,例如把其中的某种图像类型叫做Ground True, 下面介绍一下: 一.Ground True的介绍下从机器学习说起,什么叫做Ground Tru ...
北斗导航 | 读取ground truth data（python源代码）
================================================ 博主github:https://github.com/MichaelBeechan 博主CSDN:h ...
在机器学习中，ground truth是什么意思？
维基百科对Ground Truth在机器学习领域的解释是: 在机器学习中,"ground truth"一词指的是训练集对监督学习技术的分类的准确性.这在统计模型中被用来证明或否定研 ...
数据集 —— ground truth 数据集
1. matlab 自带含 ground truth 数据集 %% 加载停车标志数据到内存: data = load('stopSignsAndCars.mat', 'stopSignsAndCars ...
论文笔记之：Playing for Data: Ground Truth from Computer Games
Playing for Data: Ground Truth from Computer Games ECCV 2016 Project Page:http://download.visinf.tu- ...
【论文阅读术语】benchmark、baseline、backbone、ground truth
[最基础术语一]benchmark.baseline.backbone.ground truth 1. benchmark benchmark是一种评价方式,其指的是一个过程.具体来说就是针对不同Mo ...
图像中里面的Ground Truth是什么意思
图像中里面的Ground Truth是什么意思文章目录: 一.Ground True的介绍二.图像中的Ground True的介绍经常会在一些项目中,遇到Ground Truth,例如把其中的某 ...
ground truth的含义
ground truth在不同的地方有不同的含义,下面是参考维基百科的解释,ground truth in wikipedia. 1.在统计学和机器学习中在机器学习中ground truth表示有监 ...

PCL小工具二：使用kitti的GT(ground truth)建立激光点云地图

PCL小工具二：使用kitti的GT(ground truth)建立激光点云地图相关推荐

最新文章

热门文章