pointcloud90度分割＋球面投影（pcl+ros+python）

一.pcl+ros实现９０度分割．前文有说明，略微修改，批量处理．

#include <iostream>
#include <string.h>
#include <string>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/conversions.h>

#include <pcl_ros/transforms.h>

#include <pcl/filters/extract_indices.h>

#include <sensor_msgs/PointCloud2.h>
#include<pcl/io/pcd_io.h>//which contains the required definitions to load and store point clouds to PCD and other file formats.
#include <pcl/io/pcd_io.h>//PCL的PCD格式文件的输入输出头文件  
#define CLIP_COR 0 //     x  zuobiao


void clip_above(double clip_cor, const pcl::PointCloud<pcl::PointXYZI>::Ptr in,
                             const pcl::PointCloud<pcl::PointXYZI>::Ptr out)
{
    pcl::ExtractIndices<pcl::PointXYZI> cliper;

    cliper.setInputCloud(in);
    pcl::PointIndices indices;
#pragma omp for
    for (size_t i = 0; i < in->points.size(); i++)
    {
        if (in->points[i].x < clip_cor)
        {
            indices.indices.push_back(i);
        }
    }
    cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));
    cliper.setNegative(true); //ture to remove the indices
    cliper.filter(*out);
}

void clip_xy(const pcl::PointCloud<pcl::PointXYZI>::Ptr in,
                             const pcl::PointCloud<pcl::PointXYZI>::Ptr out)
{
    pcl::ExtractIndices<pcl::PointXYZI> cliper;

    cliper.setInputCloud(in);
    pcl::PointIndices indices;
#pragma omp for
    for (size_t i = 0; i < in->points.size(); i++)
    {
        if (abs(in->points[i].x) < abs(in->points[i].y))
        {
            indices.indices.push_back(i);
        }
    }
    cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));
    cliper.setNegative(true); //ture to remove the indices
    cliper.filter(*out);
}


int
main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);
  
  // Fill in the cloud data
  pcl::PCDReader reader;
  // Replace the path below with the path where you saved your file
  int i=1;
  for( i; i < 10; i = i + 1 )
  {
      char *filename = new char[25];
      char *filename2 = new char[25];

      sprintf(filename,"%d.pcd",i);
      reader.read (filename, *cloud); // Remember to download the file first!

      std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height 
           << " data points (" << pcl::getFieldsList (*cloud) << ").";


      pcl::PointCloud<pcl::PointXYZI>::Ptr cliped_pc_ptr(new pcl::PointCloud<pcl::PointXYZI>);

      clip_above(CLIP_COR, cloud, cliped_pc_ptr);
    
      pcl::PointCloud<pcl::PointXYZI>::Ptr remove_close(new pcl::PointCloud<pcl::PointXYZI>);

      clip_xy(cliped_pc_ptr, remove_close); 

      std::cerr << "PointCloud after filtering: " << remove_close->width * remove_close->height 
           << " data points (" << pcl::getFieldsList (*remove_close) << ").";

      pcl::PCDWriter writer;
      sprintf(filename2,"%d_new.pcd",i);
      writer.write (filename2, *remove_close, false);
  }
  return (0);
}

这个程序中有两点要说明：

１．Pcd点云格式的读取

pcl::PCDReader reader

reader.read(filename,datapointcloudname)

pcl::PCDWriter writer

writer.write(filename,datapointcloud)

2.连续读取文件名称顺序增加的代码

int i=1;
  for( i; i < 10; i = i + 1 )
  {
      char *filename = new char[25];

      sprintf(filename,"%d.pcd",i);
      reader.read (filename, *cloud); // Remember to down

３．如何在利用ros完成c++的编译和运行

1)    在工作空间catkin_make,source,得到node节点．编译

2)　在文件所在文件夹中　roscore,rosrun package_name node_name．运行．

---

---

---

二．python 进行球面投影．

---

1.点云预处理

该部分来自adamshan大佬，博客地址https://blog.csdn.net/AdamShan/article/details/83544089

传统的CNN设计多用于二维的图像模式识别（宽 × 高 ×× 通道数），三维的点云数据格式不符合该模式，而且点云数据稀疏无规律，这对特征提取都是不利的，因此，在将数据输入到CNN之前，首先对数据进行球面投影，从而到一个稠密的、二维的数据，球面投影示意图如下：

其中，ϕ和θ 分别表示点的方位角（azimuth）和顶角（altitude），这两个角如下图所示：

通常来说，方位角是相对于正北方向的夹角，但是，在我们Lidar的坐标系下,方位角为相对于x方向（车辆正前方）的夹角，ϕ和θ 的计算公式为：

 

其中，(x,y,z) 为三维点云中每一个点的坐标。所以对于点云中的每一个点都可以通过其 (x,y,z)计算其(θ,ϕ) ,也就是说我们将三维空间坐标系中的点都投射到了一个球面坐标系，这个球面坐标系实则已经是一个二维坐标系了，但是，为了便于理解，我们对其角度进行微分化从而得到一个二维的直角坐标系：

 

那么，球面坐标系下的每一个点都可以使用一个直角坐标系中的点表示，如下：

通过这么一层变换，我们就将三维空间中任意一点的位置 (x,y,z)投射到了2维坐标系下的一个点的位置 (i,j) 我们提取点云中每一个点的5个特征: (x,y,z,intensity,range)放入对应的二维坐标 (i,j)内。从而得到一个尺寸为 (H,W,C) 张量（其中 5C=5）,由于论文使用的是Kitti的64线激光雷达，所以 H=64 ，水平方向上，受Kitti数据集标注范围的限制，原论文仅使用了正前方90度的Lidar扫描，使用512个网格对它们进行了划分（即水平上采样512个点）。所以，点云数据在输入到CNN中之前，数据被预处理成了一个尺寸为 (64×512×5) 的张量。

---

２．代码实现

import numpy as np
import math


def pcd2npy(filename):
    m = np.empty((64, 512, 6))
    f = open(filename, 'r')
    line = f.readlines()
    for i in range(11,len(line)-1):
        a,b,c,d=line[i].split(' ',3)
        x=float(a)
        y=float(b)
        z=float(c)
        intensity=float(d)
        r = np.sqrt(x ** 2 + y ** 2 + z ** 2)  # Map Distance from sensor
        r_xy=np.sqrt(x ** 2 + y ** 2)


        theta=math.asin(z/r)* 180/math.pi
        if(theta<0):
            theta+=30

        fi=math.asin(y/r_xy)* 180/math.pi
        if(fi<0):
            fi+=90

        theta_num = theta / 0.46875
        fi_num = fi / 0.17578125

        g = math.ceil(theta_num)-1
        j = math.ceil(fi_num)-1

        m[g][j][0] = x
        m[g][j][1] = y
        m[g][j][2] = z
        m[g][j][3] = intensity
        m[g][j][4] = r
        m[g][j][5] = 0.0
        return m

k=0
for k in range(9):
    npyfile=pcd2npy("%01d"%k+"_new.pcd")
    np.save("%01d" % k + "_new.npy", npyfile)
    k+=1

1)pcd文件读取并按照４个一组划分．

2)按顺序读取文件名表达

"%01d" % k + "_new.npy"　　k本身占%01d字符，所以表达应该是　1_new.npy

3)pcd文本样式查看，用open with text editor.打开如下，是从第12行开始是点云．

4)调节微分数，未完待续．

总结：

１）bin转pcd（pcl+ros）

２）点云分割（pcl+ros）

３）球面投影（python）