【ROS】pythonでPointCloudを高速に処理する

始まりました！製の６時間！

クリスマスイルミネーション、きれいですね
なんか...点群に見えてきますね

それではrvizに自分だけのイルミネーションを作りに行きましょう

ほんへ

真面目な動機ですが、rosで点群処理をしようと思うと、pclライブラリで扱えるオブジェクトにブリッジをした後、pclライブラリで処理していくことが一般的なようです。
しかし、私はC++が書けないよわよわなので、どうしてもPythonで処理がしたかったのです。
（OpenCVのブリッジがあるんだからOpen3Dのブリッジがあってもいいのに）

実はrosのsensor_msgs.point_cloud2モジュールの中に、create_point()という関数があるのですが、重いらしいのでstructモジュールを使って、パブリッシュしてみます。
create_point() とざっくり比較も載せておきました

先人の知恵

create_point()関数を使って点群処理をしています。PointFieldの定義の仕方が割と重要です。

開発項目

画像をロードして、画像をもとに整列した点群を色々動かしながらパブリッシュします。

sensor_msgs/PointCloud2について

create_point()関数は結局の所、自分が作成したndarrayなどの点群情報を、PointCloud2のmsg型に変形しているだけです。
そこで、msg型に合わせて突っ込んでやろうという感じです。

msgの中身について軽く説明します
http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointCloud2.html

• Header
  先人の知恵にもありますが、これも、さらにstd_msgs.msgのHeader型を参照しながら、時間とフレームなどの情報を加えます。
• height
  1にしときます。
• width
  点群の総数です。
• fields
  先人の知恵の通り
• is_bigendian
  bigendianかどうかです。dataに詰め込むバイナリの作り方によります。
• point_step
  FIELDSの示す総バイト数です。
• row_step
  データの総バイト数です。
• data
  フィールドにより定義したデータを順番に羅列したbitデータです。
• is_dense
  trueにしときます。

コードの説明

下の方に全文載せてあります。
①BitPointsクラスのコンストラクタで、imgフォルダから表示用のイラストをロード

package_path = roslib.packages.get_pkg_dir('christmas')
img_path = package_path + '/img/christmas_zangyou.png'
print(img_path)
self.img = Image.open(img_path)
self.img = np.asarray(self.img)

ロードした画像を元に点群を作成

logi = np.where(self.img[:, :, 3] == 255)
logi = np.asarray(logi)
points_size = logi.shape[1]

self.points = np.zeros([points_size, 4])
counter = 0

for i in range(self.img.shape[0]):
    for j in range(self.img.shape[1]):
        if self.img[i, j, 3] == 255:
            print(counter)
            self.points[counter, 0] = j*0.1 - self.img.shape[0]*0.1/2
            self.points[counter, 2] = -i*0.1 + self.img.shape[1]*0.1
            self.points[counter, 3] = np.array((self.img[i, j, 0] << 16) | (self.img[i, j, 1] << 8 ) | (self.img[i, j, 2] << 0),dtype=np.uint32)

            counter += 1

②transformation関数で点群を変形
scipyのモジュールを使って、オイラー角を回転行列に変換しています。

def transformation(self, points, x, y, z, roll, pitch, yaw):
    points[:, 0] = points[:, 0] + x
    points[:, 0] = points[:, 0] + y
    points[:, 0] = points[:, 0] + z

    rot = Rotation.from_rotvec(np.array([roll, pitch, yaw]))
    rvecs = rot.as_dcm()

    tmp = np.copy(points)

    points[:, 0] = rvecs[0, 0]*tmp[:, 0] + rvecs[0, 1]*tmp[:, 1] + rvecs[0, 2]*tmp[:, 2]
    points[:, 1] = rvecs[1, 0]*tmp[:, 0] + rvecs[1, 1]*tmp[:, 1] + rvecs[1, 2]*tmp[:, 2]
    points[:, 2] = rvecs[2, 0]*tmp[:, 0] + rvecs[2, 1]*tmp[:, 1] + rvecs[2, 2]*tmp[:, 2]

    return points

③main関数の中で色々いじりながら配信しています。
上記したmsg型の変換です。
structモジュールのpack関数でバイナリデータに変換しています。

points = self.transformation(self.points, 0, 0, 0, 0, 0, 0.1)

if points.size != 0:
    #直接msg型の中身を書き込んでいく
    data_send = PointCloud2()
    t = rospy.Time.now()
    data_send.header.stamp.secs=int(t.to_sec())
    data_send.header.stamp.nsecs=t.to_nsec()-int(t.to_sec())*10**9
    data_send.header.frame_id = "christmas"
    data_send.height = 1
    data_send.width = points.shape[0]
    data_send.fields = FIELDS
    data_send.is_bigendian = False
    data_send.point_step = 16#FIELDSのbyte数
    data_send.row_step = 16*points.shape[0]#総byte数

    tmp = points.reshape(points.shape[0]*points.shape[1])
    data_send.data = struct.pack('fffI'*points.shape[0], *tmp)#*tmpでunpackして引数を並べる
    data_send.is_dense = True

    self.pub_points.publish(data_send)

比較

main関数の点群をmsg型に変換する処理にかかる時間を計測してみました。

直接msg型に変換した場合

start_time = rospy.Time.now()
start_time = start_time.to_sec()
#直接msg型の中身を書き込んでいく
data_send = PointCloud2()
t = rospy.Time.now()
data_send.header.stamp.secs=int(t.to_sec())
data_send.header.stamp.nsecs=t.to_nsec()-int(t.to_sec())*10**9
data_send.header.frame_id = "christmas"
data_send.height = 1
data_send.width = points.shape[0]
data_send.fields = FIELDS
data_send.is_bigendian = False
data_send.point_step = 16#FIELDSのbyte数
data_send.row_step = 16*points.shape[0]#総byte数

tmp = points.reshape(points.shape[0]*points.shape[1])
data_send.data = struct.pack('fffI'*points.shape[0], *tmp)#*tmpでunpackして引数を並べる
data_send.is_dense = True

end_time = rospy.Time.now()
end_time = end_time.to_sec()
print(end_time - start_time)

create_cloud()関数を使った場合

start_time = rospy.Time.now()
start_time = start_time.to_sec()

point_cloud = pc2.create_cloud(HEADER, FIELDS, points)

end_time = rospy.Time.now()
end_time = end_time.to_sec()
print(end_time - start_time)

self.pub_points.publish(point_cloud)

print(end_time - start_time)でrospyのTimeを秒に変換して時間差を表示します。

結果

直接msg型に変換した場合

0.0352971553802
0.0357060432434
0.0350620746613
0.0350420475006
0.0357580184937
0.0510640144348

create_cloud()のほう

0.0988070964813
0.0985422134399
0.098072052002
0.095803976059
0.0952179431915
0.0956950187683

という感じで、少し早くなったかなと思います。

おわりに

製の6時間に間に合わせるために（間に合わなかったけど）爆速で書きました。
ポエムを残しただけになりかけておりますお目汚し失礼しました。

コード全文

変換の方

#!/usr/bin/env python
# coding: UTF-8

from __future__ import print_function

import rospy
import roslib
from std_msgs.msg import Header
from sensor_msgs.msg import PointCloud2, PointField
import sensor_msgs.point_cloud2 as pc2

import struct
from PIL import Image
import numpy as np
from scipy.spatial.transform import Rotation


FIELDS = [
    PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
    PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
    PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
    PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1),
]


class BitPoints:
    def __init__(self):
        rospy.init_node('bit_points', anonymous=True)
        self.r = rospy.Rate(20)

        self.pub_points = rospy.Publisher("bit_points", PointCloud2, queue_size=10)

        package_path = roslib.packages.get_pkg_dir('christmas')
        img_path = package_path + '/img/christmas_zangyou.png'
        print(img_path)
        self.img = Image.open(img_path)
        self.img = np.asarray(self.img)

        logi = np.where(self.img[:, :, 3] == 255)
        logi = np.asarray(logi)
        points_size = logi.shape[1]

        self.points = np.zeros([points_size, 4])
        counter = 0

        for i in range(self.img.shape[0]):
            for j in range(self.img.shape[1]):
                if self.img[i, j, 3] == 255:
                    print(counter)
                    self.points[counter, 0] = j*0.1 - self.img.shape[0]*0.1/2
                    self.points[counter, 2] = -i*0.1 + self.img.shape[1]*0.1
                    self.points[counter, 3] = np.array((self.img[i, j, 0] << 16) | (self.img[i, j, 1] << 8 ) | (self.img[i, j, 2] << 0),dtype=np.uint32)

                    counter += 1


    def transformation(self, points, x, y, z, roll, pitch, yaw):
        points[:, 0] = points[:, 0] + x
        points[:, 0] = points[:, 0] + y
        points[:, 0] = points[:, 0] + z

        rot = Rotation.from_rotvec(np.array([roll, pitch, yaw]))
        rvecs = rot.as_dcm()

        tmp = np.copy(points)

        points[:, 0] = rvecs[0, 0]*tmp[:, 0] + rvecs[0, 1]*tmp[:, 1] + rvecs[0, 2]*tmp[:, 2]
        points[:, 1] = rvecs[1, 0]*tmp[:, 0] + rvecs[1, 1]*tmp[:, 1] + rvecs[1, 2]*tmp[:, 2]
        points[:, 2] = rvecs[2, 0]*tmp[:, 0] + rvecs[2, 1]*tmp[:, 1] + rvecs[2, 2]*tmp[:, 2]

        return points


    def main(self):
        try:
            while not rospy.is_shutdown():

                points = self.transformation(self.points, 0, 0, 0, 0, 0, 0.05)

                if points.size != 0:
                    start_time = rospy.Time.now()
                    start_time = start_time.to_sec()
                    #直接msg型の中身を書き込んでいく
                    data_send = PointCloud2()
                    t = rospy.Time.now()
                    data_send.header.stamp.secs=int(t.to_sec())
                    data_send.header.stamp.nsecs=t.to_nsec()-int(t.to_sec())*10**9
                    data_send.header.frame_id = "christmas"
                    data_send.height = 1
                    data_send.width = points.shape[0]
                    data_send.fields = FIELDS
                    data_send.is_bigendian = False
                    data_send.point_step = 16#FIELDSのbyte数
                    data_send.row_step = 16*points.shape[0]#総byte数

                    tmp = points.reshape(points.shape[0]*points.shape[1])
                    data_send.data = struct.pack('fffI'*points.shape[0], *tmp)#*tmpでunpackして引数を並べる
                    data_send.is_dense = True

                    end_time = rospy.Time.now()
                    end_time = end_time.to_sec()
                    print(end_time - start_time)

                    self.pub_points.publish(data_send)

                self.r.sleep()
        except KeyboardInterrupt:
            None


if __name__ == '__main__':
    mypoint = BitPoints()
    try:
        mypoint.main()
    except rospy.ROSInterruptException: pass

create_cloud()のほう

#!/usr/bin/env python
# coding: UTF-8

from __future__ import print_function

import rospy
import roslib
from std_msgs.msg import Header
from sensor_msgs.msg import PointCloud2, PointField
import sensor_msgs.point_cloud2 as pc2

import struct
from PIL import Image
import numpy as np
from scipy.spatial.transform import Rotation

HEADER = Header(frame_id='christmas')

FIELDS = [
    PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
    PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
    PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
    PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1),
]


class AddPoints:
    def __init__(self):
        rospy.init_node('add_points', anonymous=True)
        self.r = rospy.Rate(20)

        self.pub_points = rospy.Publisher("add_points", PointCloud2, queue_size=10)

        package_path = roslib.packages.get_pkg_dir('christmas')
        img_path = package_path + '/img/christmas_zangyou.png'
        print(img_path)
        self.img = Image.open(img_path)
        self.img = np.asarray(self.img)

        logi = np.where(self.img[:, :, 3] == 255)
        logi = np.asarray(logi)
        points_size = logi.shape[1]

        self.points = np.zeros([points_size, 4])
        counter = 0

        for i in range(self.img.shape[0]):
            for j in range(self.img.shape[1]):
                if self.img[i, j, 3] == 255:
                    print(counter)
                    self.points[counter, 0] = j*0.1 - self.img.shape[0]*0.1/2
                    self.points[counter, 2] = -i*0.1 + self.img.shape[1]*0.1
                    self.points[counter, 3] = np.array((self.img[i, j, 0] << 16) | (self.img[i, j, 1] << 8 ) | (self.img[i, j, 2] << 0),dtype=np.uint32)

                    counter += 1


    def transformation(self, points, x, y, z, roll, pitch, yaw):
        points[:, 0] = points[:, 0] + x
        points[:, 0] = points[:, 0] + y
        points[:, 0] = points[:, 0] + z

        rot = Rotation.from_rotvec(np.array([roll, pitch, yaw]))
        rvecs = rot.as_dcm()

        tmp = np.copy(points)

        points[:, 0] = rvecs[0, 0]*tmp[:, 0] + rvecs[0, 1]*tmp[:, 1] + rvecs[0, 2]*tmp[:, 2]
        points[:, 1] = rvecs[1, 0]*tmp[:, 0] + rvecs[1, 1]*tmp[:, 1] + rvecs[1, 2]*tmp[:, 2]
        points[:, 2] = rvecs[2, 0]*tmp[:, 0] + rvecs[2, 1]*tmp[:, 1] + rvecs[2, 2]*tmp[:, 2]

        return points


    def main(self):
        try:
            while not rospy.is_shutdown():

                points = self.transformation(self.points, 0, 0, 0, 0, 0, 0.1)

                if points.size != 0:
                    start_time = rospy.Time.now()
                    start_time = start_time.to_sec()

                    point_cloud = pc2.create_cloud(HEADER, FIELDS, points)

                    end_time = rospy.Time.now()
                    end_time = end_time.to_sec()
                    print(end_time - start_time)

                    self.pub_points.publish(point_cloud)

                self.r.sleep()
        except KeyboardInterrupt:
            None


if __name__ == '__main__':
    mypoint = AddPoints()
    try:
        mypoint.main()
    except rospy.ROSInterruptException: pass