点云拼接
参考：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; // label
FILE* 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 label
lower_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;
}