点云拼接

参考：https://blog.csdn.net/sunqin_csdn/article/details/105475082

代码

concat.h

#include <iostream>
#include <vector>
#include <string>
#include <unordered_map>#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/icp.h>
#include <pcl/common/transforms.h>#include <boost/filesystem.hpp>
#include <Eigen/Core>namespace concat{typedef pcl::PointXYZI PointI;
typedef pcl::PointXYZL PointL;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> CloudT;
typedef pcl::PointCloud<PointL> CloudL;
typedef pcl::PointCloud<PointI> CloudI;using IPtr = concat::CloudI::Ptr;
// typedef concat::CloudI::Ptr IPtr;
typedef concat::CloudL::Ptr LPtr;
typedef concat::CloudT::Ptr TPtr;class MultiCloud{public:MultiCloud() = delete;MultiCloud(const std::string& velodyne_dir, const std::string& pose_file, int n);void init(){get_bin_names_();std::sort(vec_of_binNames_.begin(), vec_of_binNames_.end(), vstring_compare_);  // sort the names.（由于get_bin_names函数得到的文件路径是无序的，这里将它按照000000.bin, 000001.bin的顺序排列好）get_transforms_();}void joint_map();void getLocalMap(IPtr& p_out);private:void get_transforms_();void get_bin_names_();static bool vstring_compare_(const std::string &x,const std::string &y);  //&符号不能少void parse_bin_cloud_(const std::string& bin_file, pcl::PointCloud<concat::PointI>& points);private:std::string pose_files_;std::string velodyne_dir_;std::vector<Eigen::Matrix4d> vec_of_poses_;std::vector<std::string> vec_of_binNames_;// std::unordered_map<std::string, CloudI> map_of_cloudI_;// std::unordered_map<std::string, Eigen::Affine3d> map_of_pose_;concat::IPtr p_cloud_src_;concat::IPtr p_cloud_tansformed_;concat::IPtr p_cloud_map_;int Frame_num_;};  // class MultiCloud}

concat.cpp

#include "CloudConcat.h"namespace concat{bool MultiCloud::vstring_compare_(const std::string &x,const std::string &y){return x<y;
}MultiCloud::MultiCloud(const std::string& velodyne_dir, const std::string& pose_files, int n):velodyne_dir_(velodyne_dir), pose_files_(pose_files),Frame_num_(n),p_cloud_src_(new CloudI()),p_cloud_map_(new CloudI()),p_cloud_tansformed_(new CloudI())
{std::cout << "MultiCloud() is called.\n";
}void MultiCloud::get_transforms_(){std::string line;std::ifstream ifs;ifs.open(pose_files_, std::ios::in);if (!ifs){std::cout << "cannot open file: " << pose_files_ << std::endl;return ;}while (std::getline(ifs, line) && ifs.good()){if (line.empty()) return;std::stringstream lineStream(line);std::string cell;std::vector<double> vdata;while (std::getline(lineStream, cell, ' ')){vdata.push_back(std::stod(cell));}Eigen::Matrix4d tform = Eigen::Matrix4d::Identity();Eigen::Matrix3d tf_mat;     // camera旋转矩阵tf_mat << vdata[0], vdata[1], vdata[2], vdata[4], vdata[5], vdata[6], vdata[8], vdata[9], vdata[10];Eigen::Vector3d trans_camera(vdata[3], vdata[7], vdata[11]);Eigen::Quaterniond q_camera(tf_mat);Eigen::Quaterniond q_lidar(q_camera.w(), q_camera.z(), -q_camera.x(), -q_camera.y());Eigen::Matrix3d R_lidar = q_lidar.toRotationMatrix();tform.block<3,3>(0,0) = R_lidar;tform.block<3,1>(0,3) = trans_camera;vec_of_poses_.push_back(tform);// static int count = 0;// std::cout << count++ << "transform: \n" << tform << std::endl;}}void MultiCloud::get_bin_names_(){vec_of_binNames_.clear();boost::filesystem::path full_path(velodyne_dir_);boost::filesystem::recursive_directory_iterator end_iter;for(boost::filesystem::recursive_directory_iterator iter(full_path); iter!=end_iter; ++iter){if(!boost::filesystem::is_directory(*iter) && iter->path().extension().string() == std::string(".bin")){std::string file = iter->path().string();vec_of_binNames_.push_back(iter->path().string());   // get the golbal full path name.  获取该文件的全局路径// boost::filesystem::path file_path(file);// names.push_back(file_path.stem().string());   // get the pure name(no suffix) 获取无后缀的文件名称，即000000, 000001, ...}}  }void MultiCloud::joint_map(){for (int i = 0; i < vec_of_binNames_.size() && i < Frame_num_; ++i){// convert kitti lidar data *.bin to pcl pointcloud type:parse_bin_cloud_(vec_of_binNames_[i], *p_cloud_src_);std::cout << "get points: " << p_cloud_src_->points.size() << std::endl;// transform the point cloud:pcl::transformPointCloud(*p_cloud_src_, *p_cloud_tansformed_, vec_of_poses_[i]);// point cloud merge:if (i == 0){*p_cloud_map_ = *p_cloud_tansformed_;continue;}*p_cloud_map_ += *p_cloud_tansformed_;std::cout << "[" << i+1 << "] map cloud points: " << p_cloud_map_->points.size() << std::endl;}// show save map info:std::cout << "the map has " <<  p_cloud_map_->points.size() << " points" << std::endl;}void MultiCloud::parse_bin_cloud_(const std::string& bin_file, pcl::PointCloud<concat::PointI>& points){points.points.clear();std::fstream input(bin_file.c_str(), std::ios::in | std::ios::binary);if(!input.good()){std::cerr << "Could not read file: " << bin_file << std::endl;exit(EXIT_FAILURE);}// bin2points:input.seekg(0, std::ios::beg);for (int i=0; input.good() && !input.eof(); i++){concat::PointI point;input.read((char *) &point.x, 3*sizeof(float));input.read((char *) &point.intensity, sizeof(float));points.points.push_back(point);}input.close();/*————————————————版权声明：本文为CSDN博主「sunqin_csdn」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。原文链接：https://blog.csdn.net/sunqin_csdn/article/details/105475082*/
}void MultiCloud::getLocalMap(IPtr& p_out){p_out = p_cloud_map_;
}}       // namespace concat

PCL可视化

强度

int PCLViewer::loadBin_with_Intensity(const std::string& binName, pcl::PointCloud<pcl::PointXYZI>::Ptr& p_cloud_i){int32_t num = 1000000;float *data = (float*)malloc(num * sizeof(float));// 点float *px = data + 0;float *py = data + 1;float *pz = data + 2;float *pr = data + 3;//反射强度FILE *stream;const char* filenameInput = binName.c_str();// fopen_s(&stream, filenameInput, "rb");      // fopen_s是微软的版本stream = fopen (filenameInput,"rb");num = fread(data, sizeof(float), num, stream) / 4;//读入点云数据，大概10万+个点// 创建点云// pcl::PointCloud<pcl::PointXYZI>::Ptr p_cloud_i(new pcl::PointCloud<pcl::PointXYZI>());for(size_t i = 0; i<num; i++){pcl::PointXYZI tmp;tmp.x = *px;tmp.y = *py;tmp.z = *pz;tmp.intensity = *pr;p_cloud_i->push_back(tmp);px+=4;py+=4;pz+=4;pr+=4;}fclose(stream);return 1;}

类别


int PCLViewer::loadBin_with_Label(const std::string& binName, const std::string& labelName,pcl::PointCloud<pcl::PointXYZL>::Ptr& p_cloud_l) {int32_t num = 1000000;float* data = (float*)malloc(num * sizeof(float));uint32_t* data_label = (uint32_t*)malloc(num * sizeof(uint32_t));// 点float* px = data + 0;float* py = data + 1;float* pz = data + 2;float* pr = data + 3;           //反射强度uint32_t* pl = data_label + 0;  // labelFILE* stream;FILE* label_stream;const char* filenameInput = binName.c_str();const char* labelnameInput = labelName.c_str();// fopen_s(&stream, filenameInput, "rb");      // fopen_s是微软的版本stream = fopen(filenameInput, "rb");label_stream = fopen(labelnameInput, "rb");num = fread(data, sizeof(float), num, stream) /4;  //读入点云数据，大概10万+个点auto r = fread(data_label, sizeof(uint32_t), num, label_stream);// std::cout << *data_label << std::endl;// std::cout << "num: " << num << ", label num: " << r << std::endl;// 创建点云// pcl::PointCloud<pcl::PointXYZI>::Ptr p_cloud_i(new// pcl::PointCloud<pcl::PointXYZI>());for (size_t i = 0; i < num; i++) {pcl::PointXYZL tmp;tmp.x = *px;tmp.y = *py;tmp.z = *pz;uint32_t upper_half = *pl >> 16;uint32_t lower_half = (*pl) & 0xffff;uint32_t label = upper_half << 16 + lower_half;// upper_half = label >> 16      # get upper half for instances// lower_half = label & 0xFFFF   # get lower half for semantics// lower_half = remap_lut[lower_half]  # do the remapping of semantics// label = (upper_half << 16) + lower_half   # reconstruct full labellower_half = lower_half & 0x0000ffff;tmp.label = lower_half;p_cloud_l->push_back(tmp);px += 4;py += 4;pz += 4;pr += 4;pl += 1;}fclose(stream);fclose(label_stream);return 1;
}

SemanticKITTI点云拼接+PCL可视化相关推荐

基于QT的PCL可视化点云数据处理分析软件
pcl 是我尝试过使用体验最好的点云数据可视化工具,它是c++上著名的软件开发库,虽然还有很多代码没有写完整,但是不妨碍它成为一个优秀的点云工具,本文介绍了PCL:可视化点云软件,对大家相关软件开发具 ...
PCL入门系列 —— 加载ply格式mesh模型、点云数据并作可视化展示
PCL入门系列 -- 加载ply格式mesh模型.点云数据并作可视化展示前言程序说明输出结果代码示例总结前言随着工业自动化.智能化的不断推进,机器视觉(2D/3D)在工业领域的应用和重要 ...
PCL点云库点云拼接
点云拼接有2种方式:两个点云的点数相加,和两个点云的维度相加. (1)点数相加当两个点云的维度(字段)相同时,两个点云可以通过数学符号"+"拼接,形成新的点云,新的点云维度(字段 ...
深度相机Kinect2.0三维点云拼接实验（二）
文章目录摘要 Linux系统下的环境搭建 Ubuntu系统安装 ROS-Melodic安装 Kinect2.0驱动安装 PCL库安装 Open-CV库安装注意事项 Windows10系统下的环境搭 ...
【PBL项目实战】户外智慧农场项目实战系列之4——Mind+Mixly双平台ESP32数据上云及云端可视化实时展示
[PBL项目实战]户外智慧农场项目实战系列之4--Mind+Mixly双平台ESP32数据上云及云端可视化实时展示原文链接 https://mp.weixin.qq.com/s/r_NeJdPoi ...
python点云拼接
点云拼接主要是把不同的点云拼接到一起.通常,为了获得一个完整物体的三维点云,我们可能会在不同的视点进行数据采集,然后把采集的点云数据拼接到一起. 由于视点的不同,所采集到的多个点云的坐标系也会不一致. ...
图像的点云拼接-原理讲解与代码实现
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达理解好图像的点云拼接,需要从相机的模型说起.理解相机的成像原理之后,便可更为深刻的理解图像的点云如何拼 ...
学习笔记：点云库PCL（Point Cloud Library ）介绍
本文简要介绍点云库(PCL),一个用于处理2D和3D数据的开源库,如激光雷达点云. 通过熟悉使用PCL的一些基础知识,以便后续使用PCL进行定位.主要涵盖以下内容: 点云数据Point Cloud D ...
常见Lidar点云数据处理及可视化软件汇总
常见的点云处理及可视化软件有: CloudCompare.Globalmapper.Pix4d.ArcGIS(Pro).Lidar 360.PCL等等. 文章目录 1. CloudCompare 2. ...

SemanticKITTI点云拼接+PCL可视化

点云拼接

代码

concat.h

concat.cpp

PCL可视化

强度

类别

SemanticKITTI点云拼接+PCL可视化相关推荐

最新文章

热门文章