點雲3D 目標檢測

點雲

點雲是雷達采集到的信息.
關於點雲基本介紹參考https://zhuanlan.zhihu.com/p/22581673

ros中的點雲消息結構:http://docs.ros.org/jade/api/sensor_msgs/html/msg/PointCloud2.html

# This message holds a collection of N-dimensional points, which may
# contain additional information such as normals, intensity, etc. The
# point data is stored as a binary blob, its layout described by the
# contents of the "fields" array.

# The point cloud data may be organized 2d (image-like) or 1d
# (unordered). Point clouds organized as 2d images may be produced by
# camera depth sensors such as stereo or time-of-flight.

# Time of sensor data acquisition, and the coordinate frame ID (for 3d
# points).
Header header

# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.
uint32 height
uint32 width

# Describes the channels and their layout in the binary data blob.
PointField[] fields

bool    is_bigendian # Is this data bigendian?
uint32  point_step   # Length of a point in bytes
uint32  row_step     # Length of a row in bytes
uint8[] data         # Actual point data, size is (row_step*height)

bool is_dense        # True if there are no invalid points

PointField結構:http://docs.ros.org/melodic/api/sensor_msgs/html/msg/PointField.html

# This message holds the description of one point entry in the
# PointCloud2 message format.
uint8 INT8    = 1
uint8 UINT8   = 2
uint8 INT16   = 3
uint8 UINT16  = 4
uint8 INT32   = 5
uint8 UINT32  = 6
uint8 FLOAT32 = 7
uint8 FLOAT64 = 8

string name      # Name of field
uint32 offset    # Offset from start of point struct
uint8  datatype  # Datatype enumeration, see above
uint32 count     # How many elements in the field

點雲消息數據存儲在PointCloud2.data中.

示例：

header:  // 點雲的頭信息
  seq: 963 //
  stamp:  // 時間戳
    secs: 1541143772
    nsecs: 912011000
  frame_id: "/camera_init"
height: 1   // If the cloud is unordered, height is 1  如果cloud 是無序的 height 是 1
width: 852578  //點雲的長度
fields:  //  sensor_msgs/PointField[] fields 
  - 
    name: "x"
    offset: 0
    datatype: 7 	// 	uint8 INT8    = 1
			//	uint8 UINT8   = 2
			//	uint8 INT16   = 3
			//	uint8 UINT16  = 4
			//	uint8 INT32   = 5
			//	uint8 UINT32  = 6
			//	uint8 FLOAT32 = 7
			//	uint8 FLOAT64 = 8
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
  - 
    name: "intensity"
    offset: 16
    datatype: 7
    count: 1
is_bigendian: False
point_step: 32 // Length of a point in bytes 一個點占的字節數 
row_step: 27282496 // Length of a row in bytes 一行的長度占用的字節數
data: [ .......................................................... ] //  Actual point data, size is (row_step*height)
is_dense: True // 沒有非法數據點

datatype=7對應的類型為PointField.FLOAT32，size為４．ｘ/y/z的偏移都是正常的.為什么intensity的offset變成了16而不是12呢？ros在包裝ＰointCloud2的時候可能在ＰointField之間添加了一些額外信息,這點我們在處理的時候要注意一下.同理還有Ｐoint與Ｐoint之間也可能有額外的信息.

---

點雲rosbag轉numpy

參考https://gist.github.com/bigsnarfdude/eeb156dc7b4caca69f5b31037da54708

我們想將PointCloud2格式的msg轉換為numpy的矩陣格式.即轉換成ｍ行n列,每一列即為ｘ,y,z,intensity．．．
首先我們希望對msg.data做反序列化處理,即

def msg_to_arr(msg):
    arr = np.fromstring(msg.data, dtype_list)

現在問題變成了如何從點雲的datatype轉到numpy的datatype

DUMMY_FIELD_PREFIX = '__'

# mappings between PointField types and numpy types
type_mappings = [(PointField.INT8, np.dtype('int8')), (PointField.UINT8, np.dtype('uint8')), (PointField.INT16, np.dtype('int16')),
                 (PointField.UINT16, np.dtype('uint16')), (PointField.INT32, np.dtype('int32')), (PointField.UINT32, np.dtype('uint32')),
                 (PointField.FLOAT32, np.dtype('float32')), (PointField.FLOAT64, np.dtype('float64'))]

pftype_to_nptype = dict(type_mappings)
nptype_to_pftype = dict((nptype, pftype) for pftype, nptype in type_mappings)

# sizes (in bytes) of PointField types
pftype_sizes = {PointField.INT8: 1, PointField.UINT8: 1, PointField.INT16: 2, PointField.UINT16: 2,
                PointField.INT32: 4, PointField.UINT32: 4, PointField.FLOAT32: 4, PointField.FLOAT64: 8}


def fields_to_dtype(fields, point_step):
    '''
    Convert a list of PointFields to a numpy record datatype.
    '''
    offset = 0  
    np_dtype_list = []
    for f in fields:
        while offset < f.offset:
            # might be extra padding between fields
            np_dtype_list.append(('%s%d' % (DUMMY_FIELD_PREFIX, offset), np.uint8))
            offset += 1

        dtype = pftype_to_nptype[f.datatype]
        if f.count != 1:
            dtype = np.dtype((dtype, f.count))

        np_dtype_list.append((f.name, dtype))
        offset += pftype_sizes[f.datatype] * f.count

    # might be extra padding between points
    while offset < point_step:
        np_dtype_list.append(('%s%d' % (DUMMY_FIELD_PREFIX, offset), np.uint8))
        offset += 1

    return np_dtype_list

代碼邏輯很清楚,pftype_to_nptype和nptype_to_pftype定義了點雲消息中數據結構和numpy中數據結構的映射關系.
唯一需要注意的就是前面提到過的ros在包裝ＰointCloud2的時候可能在ＰointField之間添加了一些額外信息,這點我們在處理的時候要注意一下.同理還有Ｐoint與Ｐoint之間也可能有額外的信息.　　代碼里的

        while offset < f.offset:
            # might be extra padding between fields
            np_dtype_list.append(('%s%d' % (DUMMY_FIELD_PREFIX, offset), np.uint8))
            offset += 1
            
            
    # might be extra padding between points
    while offset < point_step:
        np_dtype_list.append(('%s%d' % (DUMMY_FIELD_PREFIX, offset), np.uint8))
        offset += 1

就是為了處理上述問題.

復現點雲檢測模型SqueezeSeg檢測點雲數據

https://blog.csdn.net/AdamShan/article/details/83544089
原文用的py2.7，復現的時候遇到了很多問題

• conda activate env2.7
• pip install tensorflow
• pip install easydict
• pip install joblib

直接運行squeezeseg_ros_node.py的時候會報如下錯誤.

錯誤代碼的意思是出錯於讀launch文件.

npy_path = rospy.get_param('npy_path')

這一句會讀launch文件中的配置.
在執行了roslaunch squeezeseg_ros squeeze_seg_ros.launch之后,會報錯

這之后再執行python squeezeseg_ros_node.py就可以正常運行了.