ROSのPointCloudで点群に色を付ける

ROSのrvizで色付き点群を表示しようとした時に,PointCloud型のメッセージで色情報を付与する際にハマったので,メモしておきます.

 

 

sensor_msgs::PointCloud

ROSでPointCloudを扱うときには,sensor_msgs::PointCloudというメッセージ型を使用します.
PointCloud型は点群の座標(x,y,z)情報に加えて,スレレオカメラで得られる色情報や,lidarから得られるレーザの反射強度intensity情報を付与することができます.
ROSのPointCloud型Messageのドキュメントを見てみましょう.

sensor_msgs/PointCloud Message

# This message holds a collection of 3d points, plus optional additional
# information about each point.

# Time of sensor data acquisition, coordinate frame ID.
Header header

# Array of 3d points. Each Point32 should be interpreted as a 3d point
# in the frame given in the header.
geometry_msgs/Point32[] points

# Each channel should have the same number of elements as points array,
# and the data in each channel should correspond 1:1 with each point.
# Channel names in common practice are listed in ChannelFloat32.msg.
ChannelFloat32[] channels

PointCloud型は,データの座標系やタイムスタンプ等を格納するheader,点群の座標の配列を格納するpoints,点の色や反射強度等の付属の情報の配列を格納するchannelsという要素から成り立っています.
このchannelsに色情報を格納することで色付き点群としてrviz上に表示することができます.
ChannelFloat32型について詳しく見ていきましょう.

 

 

sensor_msgs/ChannelFloat32 Message

ChannelFloat32型はPointCloudの点群に色々な情報を付与でき,予め指定されたフォーマットに従うと,rviz上でもそれにならってビジュアライズしてくれます.

sensor_msgs/PointCloud Message

# This message is used by the PointCloud message to hold optional data
# associated with each point in the cloud. The length of the values
# array should be the same as the length of the points array in the
# PointCloud, and each value should be associated with the corresponding
# point.

# Channel names in existing practice include:
#   "u", "v" - row and column (respectively) in the left stereo image.
#              This is opposite to usual conventions but remains for
#              historical reasons. The newer PointCloud2 message has no
#              such problem.
#   "rgb" - For point clouds produced by color stereo cameras. uint8
#           (R,G,B) values packed into the least significant 24 bits,
#           in order.
#   "intensity" - laser or pixel intensity.
#   "distance"

# The channel name should give semantics of the channel (e.g.
# "intensity" instead of "value").
string name

# The values array should be 1-1 with the elements of the associated
# PointCloud.
float32[] values

ChannelFloat32型のメッセージはそのデータが何であるかを示すnameと,その情報を格納した配列であるfloat32型の配列であるvaluesからなります.
ここでnameに”rgb”を指定して,valuesに正しい値を入れてあげることで点群をカラー表示することができます.
“rgb”の部分の説明を見ると,unsigned_int8型で記述された値を(R,G,B)の順番で下位24ビットに順番に入れよ.となっています.が,
valuesの型は32ビットfloatとなっています.

 

 

解決策

reintrepret_castを使って,uint32型を強制的にfloat型にキャストすることで解決しました.
以下にサンプルコード載せておきます.

sensor_msgs::ChannelFloat32 colors;
colors.name = "rgb";
uint32_t r = red_0_255, g = green_0_255, b = blue_0_255;
uint32_t rgb = (r << 16 | g << 8 | b);   //8bit毎に色情報を付与
colors.values.push_back(*reinterpret_cast<float*>(&rgb));   //uint32_tをfloatにキャスト

channelsの情報でintensity等の浮動小数の値も格納したかったためにfloat型を採用したのだとは思いますが,そこに整数型の値を突っ込むわけですから,混乱を招くわけです...
まあ,解決出来たので良しとしましょう.