概要
ROSのカメラ画像処理に関して、いろいろ細かなテクニックがあったので、まとめてみました。
Depth 画像の Registration
RealSense などの RGBD カメラを利用する場合、RGB画像とDepth画像の座標系を揃えるRegistrationの下処理を行うのが一般的です。
- depth_image_proc/register
- rgbd_launch/processing.launch.xml
- Realense の場合: realsense_camera/r200_nodelet_rgbd.launch
r200_nodelet_rgbd.launch は内部的に rgbd_launch を include していて、rgbd_launch は内部的に image_proc や
depth_image_proc のノードを利用しています。
Registration の仕組み
- Depth 画像を Rectify (歪み補正)
- RGB 画像を Rectify (歪み補正)
- Depth 画像を3次元空間にマッピング (deprojection)
- 3次元空間上の点を RGB カメラの座標系にマッピング (projection)
このステップを経ることで、Depth と RGB の各画素で1対1の対応関係が取れるようになります。最終的に得られる Depth 画像の座標系・解像度は、入力の座標系・解像度に関わらず、RGB 画像と同じものになります。
画像のフィルタ処理
画像系のトピックのフィルタ処理は、原則として下記の作法に従うのが良い。(image_pipelineの実装が参考になります)
- Nodelet を使う (zero copy で画像転送できる)
- 通常の Publisher, Subscriber の代わりに image_transport の Publisher, Subscriber, CameraSubscriber を利用する
- できれば、subscriber がいるときだけ処理を行うようにする
-
image_transport::SubscriberStatusCallbackが利用できる
-
このようにすることで、ノード間連携のオーバーヘッドを最小化しつつ、CameraInfo との同期等がスムーズに行なえます。
PointCloud の Nodelet の負荷
PointCloud 配信の Nodelet を動かすと負荷が大きくなるのでは、と懸念したのですが、 depth_image_proc/point_cloud_xyzrgb は前述の作法にのっとって実装されているので、実は Nodelet を動かしても subscribe しなければ負荷は少ないです。rviz でのデバッグ目的とかでも、気軽に PointCloud の Nodelet を動かしてしまって問題ないです。
トピック名のルール
sensor_msgs/CameraInfo を見ると、下記のように書かれています。
# image_raw - raw data from the camera driver, possibly Bayer encoded
# image - monochrome, distorted
# image_color - color, distorted
# image_rect - monochrome, rectified
# image_rect_color - color, rectified
自前で Image を配信する場合にも、なるべくこの作法にのっとった方が良さそうです。
CameraInfo と Image の同期
rtabmap_ros/data_throttle を利用する際にハマりました。一部の Node は、中で synchronize 処理を行っており、入力の CameraInfo と Image の stamp が合っていないと処理が行われないので注意が必要です。画像のフィルタ処理に image_transport を利用していれば問題ないのですが、そうでない場合にハマります。
CameraInfo を用いた画像処理
image_geometry を使うと、CameraInfo を使って簡単に projection や rectify が行えます。
from image_geometry import PinholeCameraModel
def get_direction(camera_info, px, py):
u""" 画像上の座標を入力として、カメラ座標における方向を返す """
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
vector = camera_model.projectPixelTo3dRay((px, py))
例えば、こんな感じ。