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型を採用したのだとは思いますが,そこに整数型の値を突っ込むわけですから,混乱を招くわけです...
まあ,解決出来たので良しとしましょう.