SemanticKITTI点云拼接+PCL可视化

点云拼接

参考：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;
}