点云bin格式和pcd格式相互转化

本是古典何须时尚 2022-12-26 11:12 363阅读 0赞

## 使用python-pcl, 也就是python版本的pcl库将bin格式转化为pcd格式,然后将点云进行显示 ##

(C++版本的这里暂时不做说明)

参考**python-pcl**文件中的`cluster_extraction.py`

文件中已经添加啦备注, 可作为参考.

# -*- coding: utf-8 -*-
    # Euclidean Cluster Extraction
    # http://pointclouds.org/documentation/tutorials/cluster_extraction.php#cluster-extraction
    import numpy as np
    import pcl
    
    
    def main():
        #   // Read in the cloud data
        #   pcl::PCDReader reader;
        #   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
        #   reader.read ("table_scene_lms400.pcd", *cloud);
        #   std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
        cloud = pcl.load('./table_scene_lms400.pcd')
    
        #   // Create the filtering object: downsample the dataset using a leaf size of 1cm
        #   pcl::VoxelGrid<pcl::PointXYZ> vg;
        #   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
        #   vg.setInputCloud (cloud);
        #   vg.setLeafSize (0.01f, 0.01f, 0.01f);
        #   vg.filter (*cloud_filtered);
        #   std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size ()  << " data points." << std::endl; //*
        vg = cloud.make_voxel_grid_filter()
        vg.set_leaf_size(0.005, 0.005, 0.005)
        cloud_filtered = vg.filter()
    
        #   // Create the segmentation object for the planar model and set all the parameters
        #   pcl::SACSegmentation<pcl::PointXYZ> seg;
        #   pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
        #   pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
        #   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
        #   pcl::PCDWriter writer;
        #   seg.setOptimizeCoefficients (true);
        #   seg.setModelType (pcl::SACMODEL_PLANE);
        #   seg.setMethodType (pcl::SAC_RANSAC);
        #   seg.setMaxIterations (100);
        #   seg.setDistanceThreshold (0.02);
    
        seg = cloud.make_segmenter()
        seg.set_optimize_coefficients(True)
        seg.set_model_type(pcl.SACMODEL_PLANE)
        seg.set_method_type(pcl.SAC_RANSAC)
        seg.set_MaxIterations(100)
        seg.set_distance_threshold(0.005)
    
        #   int i=0, nr_points = (int) cloud_filtered->points.size ();
        #   while (cloud_filtered->points.size () > 0.3 * nr_points)
        #   {
        #     // Segment the largest planar component from the remaining cloud
        #     seg.setInputCloud (cloud_filtered);
        #     seg.segment (*inliers, *coefficients);
        #     if (inliers->indices.size () == 0)
        #     {
        #       std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
        #       break;
        #     }
        #     // Extract the planar inliers from the input cloud
        #     pcl::ExtractIndices<pcl::PointXYZ> extract;
        #     extract.setInputCloud (cloud_filtered);
        #     extract.setIndices (inliers);
        #     extract.setNegative (false);
        #
        #     // Get the points associated with the planar surface
        #     extract.filter (*cloud_plane);
        #     std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
        #
        #     // Remove the planar inliers, extract the rest
        #     extract.setNegative (true);
        #     extract.filter (*cloud_f);
        #     *cloud_filtered = *cloud_f;
        #   }
    
        i = 0
        nr_points = cloud_filtered.size
        # while nr_points > 0.3 * nr_points:
        #     # Segment the largest planar component from the remaining cloud
        #     [inliers, coefficients] = seg.segment()
        #     # extract = cloud_filtered.extract()
        #     # extract = pcl.PointIndices()
        #     cloud_filtered.extract(extract)
        #     extract.set_Indices (inliers)
        #     extract.set_Negative (false)
        #     cloud_plane = extract.filter ()
        #
        #     extract.set_Negative (True)
        #     cloud_f = extract.filter ()
        #     cloud_filtered = cloud_f
    
        # Creating the KdTree object for the search method of the extraction
        # pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
        # tree->setInputCloud (cloud_filtered);
        tree = cloud_filtered.make_kdtree()
        # tree = cloud_filtered.make_kdtree_flann()
    
        # std::vector<pcl::PointIndices> cluster_indices;
        # pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
        # ec.setClusterTolerance (0.02); // 2cm
        # ec.setMinClusterSize (100);
        # ec.setMaxClusterSize (25000);
        # ec.setSearchMethod (tree);
        # ec.setInputCloud (cloud_filtered);
        # ec.extract (cluster_indices);
        ec = cloud_filtered.make_EuclideanClusterExtraction()
        ec.set_ClusterTolerance(0.02)
        ec.set_MinClusterSize(100)
        ec.set_MaxClusterSize(25000)
        ec.set_SearchMethod(tree)
        cluster_indices = ec.Extract()
    
        print('cluster_indices : ' + str(cluster_indices.count) + " count.")
        # print('cluster_indices : ' + str(cluster_indices.indices.max_size) + " count.")
    
        #   int j = 0;
        #   for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
        #   {
        #     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
        #     for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
        #       cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
        #     cloud_cluster->width = cloud_cluster->points.size ();
        #     cloud_cluster->height = 1;
        #     cloud_cluster->is_dense = true;
        #
        #     std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
        #     std::stringstream ss;
        #     ss << "cloud_cluster_" << j << ".pcd";
        #     writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
        #     j++;
        #   }
        #
    
        cloud_cluster = pcl.PointCloud()    # 首先定义pcl的格式
    
        for j, indices in enumerate(cluster_indices):
            # cloudsize = indices
            print('indices = ' + str(len(indices)))
            # cloudsize = len(indices)
            points = np.zeros((len(indices), 3), dtype=np.float32)          # 定义一个全为0的矩阵, 形状与点云相似(n, 3)
            # points = np.zeros((cloudsize, 3), dtype=np.float32)
    
            # for indice in range(len(indices)):
            for i, indice in enumerate(indices):
                # print('dataNum = ' + str(i) + ', data point[x y z]: ' + str(cloud_filtered[indice][0]) + ' ' + str(cloud_filtered[indice][1]) + ' ' + str(cloud_filtered[indice][2]))
                # print('PointCloud representing the Cluster: ' + str(cloud_cluster.size) + " data points.")
                points[i][0] = cloud_filtered[indice][0]    # pcl格式的点云送入到刚才定义的(n,3)矩阵中
                points[i][1] = cloud_filtered[indice][1]
                points[i][2] = cloud_filtered[indice][2]
    
            cloud_cluster.from_array(points)    # 将points矩阵转化为pcd的形式
            ss = "cloud_cluster_" + str(j) + ".pcd" # 定义pcd的文件名
            pcl.save(cloud_cluster, ss) # 将pcd格式点云文件进行保存
    
    
    if __name__ == "__main__":
        # import cProfile
        # cProfile.run('main()', sort='time')
        main()

## 举个例子: ##

例子中,将bin格式的点云转化为pcd格式,然后使用pyqtgraph进行显示

# -*- coding: utf-8 -*-
    import os
    import sys
    if hasattr(sys, 'frozen'):
        os.environ['PATH'] = sys._MEIPASS + ";" + os.environ['PATH']
    
    from PyQt5 import QtWidgets
    from PyQt5.QtCore import *
    from PyQt5.QtWidgets import *
    
    
    # import initExample
    from pyqtgraph.Qt import QtCore, QtGui
    import pyqtgraph.opengl as gl
    # from kitti import show_lidar_with_boxes
    import numpy as np
    import csv
    import pandas as pd
    
    
    import socket
    import glob
    # from datetime import datetime, timedelta
    import struct
    import time
    import traceback
    from multiprocessing import Process, Queue, Pool
    import threading
    
    import logging
    import logging.config
    
    from decimal import Decimal
    import math
    import datetime
    import pcl
    
    def inte_to_rgb(pc_inte):
        minimum, maximum = np.min(pc_inte), np.max(pc_inte)
        try:
            if minimum == maximum:
                ratio = 2 * (pc_inte + minimum) / (maximum + minimum)
                b = (np.maximum((1 - ratio), 0))
                r = (np.maximum((ratio - 1), 0))
                g = 1 - b - r
            else:
                ratio = 2 * (pc_inte - minimum) / (maximum - minimum)
                b = (np.maximum((1 - ratio), 0))
                r = (np.maximum((ratio - 1), 0))
                g = 1 - b - r
        except:
            ratio = 2 * (pc_inte + minimum) / (maximum + minimum)
            b = (np.maximum((1 - ratio), 0))
            r = (np.maximum((ratio - 1), 0))
            g = 1 - b - r
        return np.stack([r, g, b, np.ones_like(r)]).transpose()
    
    if __name__ == '__main__':
        app = QtWidgets.QApplication(sys.argv)
        w = gl.GLViewWidget()
        w.setBackgroundColor('w')   # 设置背景为白色，点云显示为其他颜色
        w.opts['distance'] = 20
        w.setWindowTitle('pyqtgraph example: GLScatterPlotItem')
        # g = gl.GLGridItem()   # 显示网格
        # w.addItem(g)
        coord = gl.GLAxisItem() #　显示坐标轴
        coord.setSize(1,1,1)
        ｗ.addItem(coord)
        save_bin_name = 0
        path = "000090.bin"
        points = np.fromfile(path, dtype=np.float32).reshape(-1, 4)[:,:3]
        print("当前点云大小为：", points.shape)
    
        cloud_cluster = pcl.PointCloud()    # 首先定义pcl的格式
        cloud_cluster.from_array(points)    # 将points矩阵转化为pcd的形式
        pcl.save(cloud_cluster, "000000.pcd") # 将pcd格式点云文件进行保存
    
    
    
    
        # # print("new_name.shape", new_name.shape)
        # # 将分割后的点云进行保存,保存为bin格式, 然后使用pointnet进行分类
        # pointcloud_name_path  = os.getcwd() + "/bin2pcd/"
        # pointcloud_name = str(save_bin_name).zfill(6) + ".pcd"
        # pointcloud_name_new = os.path.join(pointcloud_name_path, pointcloud_name)
        # points = points.astype(np.float32)
        # points.tofile(pointcloud_name_new)
        # save_bin_name = save_bin_name + 1
    
    
        # sp2 = gl.GLScatterPlotItem(pos=points, color=(1,1,1,1), size=2) #不带有任何颜色的白点
        sp2 = gl.GLScatterPlotItem(pos=None, color=None, size=2)  # 带有颜色的点
        phase = 0.
        w.addItem(sp2)
        
           
        if points.shape[1] == 4:
            print("global_value.points_var.shape[1]", points.shape[1])
            pc_inte = points[:, 3]
            pc_color = inte_to_rgb(pc_inte)
            sp2.setData(pos=points[:, 0:3], color=pc_color)
            sp2.setGLOptions('translucent') # 设置背景为白色，点云显示为其他颜色
        elif points.shape[1] == 3:
            print("global_value.points_var2.shape[1]", points.shape[1])
            sp2.setData(pos=points[:, 0:3], color=(0,0,0,1))
            sp2.setGLOptions('translucent') # 设置背景为白色，点云显示为其他颜色
    
    
        w.show()
        sys.exit(app.exec_())