[ROS] Pythonで色付き点群をpublish

本文について

ROSでPointCloud2 msgを作るとき、色付けない場合
sensor_msgs.point_cloud2 中の関数で利用すれば簡単に

import sensor_msgs.point_cloud2 as pcd2
msg = pcd2.create_cloud_xyz32(header, points)

でもPythonのsensor_msgs.point_cloud2中は直接で色付き点群、つまりRGB PointCloud Messageを作る関数がない。
本文では　PythonでこのRGB PointCloud Messageの作る方法を説明
本文のコード

データを用意

この部分は本文で使った色付き点群データの用意方法について説明する
もう色付き点群データがある方ではパスしてもよい

本文で使ったデータは前の文章

で処理して取ったデータ

RGB画像:

デプス画像:

そしてデプス画像を前々回の文章

中のコードをもう一度点群に変換

    import numpy as np
    import cv2

    #depth2pcd 
    depth2pcd =test_depth_to_pcd(1)

    #load image and depth image
    bgr = cv2.imread("./data/0000000150.png")
    depth = cv2.imread("./data/depth.png")

    depth = depth[:,:,0]
    rgb = cv2.cvtColor(bgr,cv2.COLOR_BGR2RGB)

    #get pointcloud　(N x 3 ) and rgb data (N x 3 )
    pointcloud = depth2pcd.get_pcd(depth)
    rgb_list = rgb.reshape(-1,3)

すると、色付き点群必要の　点群と色は手に入れる

まだ、このデプス画像は低密度なので、画像で一部分だけデプス値があるため、無効の点を削除

    depth = depth.reshape(-1,1)
    filter = np.where(depth!=0)

    pointcloud = pointcloud[filter[0],:]
    rgb_list = rgb_list[filter[0],:]

RGB PointCloud Message

1:RGBF32

sensor_msgs.point_cloud2中のcreate_cloud_xyz32関数を参考しよう

def create_cloud_xyz32(header, points):
    """
    Create a L{sensor_msgs.msg.PointCloud2} message with 3 float32 fields (x, y, z).

    @param header: The point cloud header.
    @type  header: L{std_msgs.msg.Header}
    @param points: The point cloud points.
    @type  points: iterable
    @return: The point cloud.
    @rtype:  L{sensor_msgs.msg.PointCloud2}
    """
    fields = [PointField('x', 0, PointField.FLOAT32, 1),
              PointField('y', 4, PointField.FLOAT32, 1),
              PointField('z', 8, PointField.FLOAT32, 1)]
    return create_cloud(header, fields, points)

とても短くな関数
要するには点群のfieldsを定義して、それで点群データ、headerと一緒にcreate_cloud関数に入れる
点群fieldsではPointFieldの組み合わせ
PointFieldでは

PointField(name , offset, datatype, count)
"""
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
"""

nameとdatatypeがわかりやすい、countも1をすればよい
offsetではバイト位置

先のXYZ32関数では、float32を使い
float32は4バイト
だからバイト位置としては0 4 8

それをわかる以上、xyzrgbのfieldsを作る

    from sensor_msgs.msg import PointCloud2,PointField
    ##RGBF32
    fields = [  PointField("x",0,PointField.FLOAT32,1),
                    PointField("y",4,PointField.FLOAT32,1),
                    PointField("z",8,PointField.FLOAT32,1),
                    PointField("r",12,PointField.FLOAT32,1),
                    PointField("g",16,PointField.FLOAT32,1),
                    PointField("b",20,PointField.FLOAT32,1)]

画像でrgb値を保存するdate typeでは uint8 [0~255]
でもここでr,g,bデータはfloat32定義するので、さき画像からのデータを float32 [0~1]に変換

color = (rgb_list/255).astype(np.float32)

そして点群データと色データを組み合わせ　色付き点群を作る、一緒にcreate_cloud関数に入れすれば
RGB POINTCLOUD MESSAGEが手に入れる

    from sensor_msgs.msg import PointCloud2,PointField
    from std_msgs.msg import Header
    import sensor_msgs.point_cloud2 as pcd2

    data = np.hstack((pointcloud,color)).astype(np.float32)
    header = Header()
    header.frame_id = "map"
    msg = pcd2.create_cloud(header=header,fields=fields,points=data )

ROSにpublish

    import rospy
    
    rospy.init_node("rgbpcd")
    pcd_publish = rospy.Publisher("test_pcd",PointCloud2,queue_size=1)
    rate =rospy.Rate(20)


    while not rospy.is_shutdown():
        msg = pcd2.create_cloud(header=header,fields=fields,points=data )
        pcd_publish.publish(msg)
        rate.sleep()

最後はでrviz中で　点群の color transformerをRGBF32を選択すれば、点群は色付きます

2.RGB8

先で色付き点群をpublishしましたが
まだ改良点がある

画像からのRGBデータでは元々 uint8 [0~255]　の形
publishするため　float32 [0~1]　に変換した

uint8データサイズは　1バイト　　8ビット
float32 は　4バイト　32ビット

一つ組のRGBデータをfloat32に変換すると　サイズは本来の　3バイトから12バイトに変化する
9バイトを増加
データのバイト量が増加したら、publishするとき使ったband widthも増加する

先RGBF32 , RATE = 20 Hz場合のbandwidthをチェックしよう

bandwidthは8.5MB/s

これを改良するため、RGBデータを圧縮する
圧縮方法は以下の文章と動画を参考

簡単に言うと、元々のRGBデータ　3つの uint8 3*8=24ビット　変換じゃなく
そのまま一つfloat32の32ビットに入れる

流れは
uint8 r = R R R R R R R R
uint8 g = G G G G G G G G
uint8 b = B B B B B B B B

uint32 rgb = 0 0 0 0 0 0 0 0 R R R R R R R R G G G G G G G G B B B B B B B B
float32 rgb =0 0 0 0 0 0 0 0 R R R R R R R R G G G G G G G G B B B B B B B B

    r,g,b = rgb_list[:,0].astype(np.uint32), rgb_list[:,1].astype(np.uint32), rgb_list[:,2].astype(np.uint32)
    rgb = np.array((r << 16) | (g << 8 ) | (b << 0),dtype=np.uint32)
    color = rgb.reshape(-1,1)
    color.dtype = np.float32

そしてr,g,bデータを　一つのfloat32に圧縮した
点群の定義　fieldsを以下のように変更

    fields = [  PointField("x",0,PointField.FLOAT32,1),
                PointField("y",4,PointField.FLOAT32,1),
                PointField("z",8,PointField.FLOAT32,1),
                PointField("rgb",12,PointField.FLOAT32,1)]

他の部分は先のRGBF32と同じでもよい、ROSにpublishする

すると、rvizで点群のcolortransformerはRGB8に変更した

も一回点群のbandwidthをチェックしよう

5.6MB/sになった

参考資料