環境

この記事は以下の環境で動いています。

項目	値
CPU	Core i5-8250U
Ubuntu	20.04
ROS	Noetic
Gazebo	11.9.0
OpenCV	4.2

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

image_geometryはカメラ画像上の位置ピクセル位置と3D空間上の位置を変換するAPIです。今回は以下の処理をしてみます。

/gazebo/model_stateから物体位置を取得して、カメラの位置と物体位置をrostopicに出す
上記の物体位置をimage_geometryを用いてカメラ画像に重畳する

ソースコード(位置変換ノード)

cam_lecture/src/cam_model_objects.cpp

ソースコード(投影ノード)

cam_lecture/src/cam_display_objects.cpp

#include <ros/ros.h>
#include <image_transport/image_transport.h>

#include <cv_bridge/cv_bridge.h>
#include <image_geometry/pinhole_camera_model.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/image_encodings.h>
#include <vision_msgs/Detection3DArray.h>

class FrameDrawer{
public:
  FrameDrawer() : it_(nh_)
  {
    camera_sub_ = it_.subscribeCamera("/head_camera/image_raw", 1, &FrameDrawer::imageCb, this);
    camera_pub_ = it_.advertise("output_image", 1);
    object_sub_ = nh_.subscribe("objects", 1, &FrameDrawer::objectCb, this);
  }

  void objectCb(const vision_msgs::Detection3DArray object_msg){
    last_objects_=object_msg;
  }

  void imageCb(const sensor_msgs::ImageConstPtr& image_msg,
               const sensor_msgs::CameraInfoConstPtr& info_msg)
  {
    cv::Mat image;
    cv_bridge::CvImagePtr input_bridge;
    try {
      input_bridge = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
      image = input_bridge->image;
    }
    catch (cv_bridge::Exception& ex){
      ROS_ERROR("[draw_frames] Failed to convert image");
      return;
    }

    image_geometry::PinholeCameraModel cam_model;
    cam_model.fromCameraInfo(info_msg);
    for(int i=0;i<last_objects_.detections.size();i++){
      geometry_msgs::PoseStamped source_pose;
      source_pose.header = last_objects_.detections[i].header;
      source_pose.pose = last_objects_.detections[i].bbox.center;
      geometry_msgs::PoseStamped target_pose;
      try{
        tf_listener_.waitForTransform(cam_model.tfFrame(), source_pose.header.frame_id, ros::Time(0), ros::Duration(1.0));
        tf_listener_.transformPose(cam_model.tfFrame(),ros::Time(0),source_pose,source_pose.header.frame_id,target_pose);
      }
      catch(...){
        ROS_INFO("tf error %s->%s", cam_model.tfFrame().c_str(), source_pose.header.frame_id.c_str());
      }

      if(0.1<target_pose.pose.position.z && target_pose.pose.position.z<10){
        cv::Point3d pt_cv1(target_pose.pose.position.x, target_pose.pose.position.y, target_pose.pose.position.z);
        cv::Point2d uv1 = cam_model.project3dToPixel(pt_cv1);
        cv::Point2d uv2 = cam_model.unrectifyPoint(uv1); // position on rectify image -> on unrectify image
        cv::circle(image, cv::Point(uv2.x,uv2.y), 30, CV_RGB(255,0,0), 5);
      }
    }
    camera_pub_.publish(input_bridge->toImageMsg());
  }

  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::CameraSubscriber camera_sub_;
  ros::Subscriber object_sub_;
  image_transport::Publisher camera_pub_;
  tf::TransformListener tf_listener_;
  vision_msgs::Detection3DArray last_objects_;
};

int main(int argc, char** argv){
  ros::init(argc, argv, "cam_display_objects");
  FrameDrawer drawer;
  ros::spin();
}

インクルードは#include <image_geometry/pinhole_camera_model.h>と行います。
image_geometry::PinholeCameraModel cam_model_;でインスタンスを宣言します。
cam_model_.fromCameraInfo(info_msg);でカメラパラメーターを読み込みます。
cv::Point2d uv1 = cam_model_.project3dToPixel(pt_cv1);が一番メインの関数で、空間上の3次元位置[cv::Point3d]からカメラ上のピクセルの位置[cv::Point2d]に変換します。
逆の変換はprojectPixelTo3dRay()で行います。ただし一意に位置は決まらないので候補になる線を表すベクトルが出てきます。

以下が実行用のlaunchです。

cam_lecture/launch/camera_geometry.launch

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="model" default="$(find sim_env_lecture)/urdf/sensor_station.urdf" />
  <arg name="rvizconfig" default="$(find cam_lecture)/rviz/camera_sim.rviz" />
  <param name="robot_description" type="str" textfile="$(arg model)"/>

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="sensor_station_coke.world" />
  </include>

  <node name="cam_model_objects" pkg="cam_lecture" type="cam_model_objects" output="screen">
    <rosparam param="target_list">[coke0, coke1, coke2]</rosparam>
  </node>

  <node name="image_proc" pkg="image_proc" type="image_proc" ns="/head_camera" output="screen"/>

  <node name="cam_display_objects" pkg="cam_lecture" type="cam_display_objects" output="screen"/>

  <node name="sim1_odom_tf_converter" pkg="sim1_lecture" type="sim1_odom_tf_converter"/>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />   
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>

実行

各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。

roslaunch cam_lecture sim_geometry.launch

丸い円はは画像認識でなく、gazebo上で、model_states-- (cam_model_objects) -> コーラ缶の位置 -- (display_objects) -> 画像という経路で生成したものです。

hackBaselineに注意

urdfのカメラのシミュレーションの部分でhackBaselineの値が入っていると、'camera_info'の'P'項目に値が入って全体的に位置が画面から見て左にずれます。わかりにくいミスなので注意です。
カメラの座標系は右がX、下がY、奥に行く方向がZと慣習的に決まっています。これにシミュレーションや実機も合わせる必要があります。
今回はimage_rawに重ねましたが、本当は歪みを補正したimage_procに重畳させないとずれます。

光軸を合わせる

この記事にあるような3次元位置を出すためには光軸を合わせる必要があります。このずれにはカメラの取り付けずれやカメラ自体のレンズなどの歪みによる光軸ずれがあります。
このずれは外部パラメーターなので66 カメラのキャリブレーションを行うで補正することができません。
まずそもそもどのピクセルが光軸中心に当たるかですが、以下のようにcamera_infoがあった場合

補正前の画像(/image_raw)ではcamera_matrixの3つ目(400.0)がx軸、6つ目(300.0)がy軸の中心位置になります。
補正後の画像(/image_rect)ではprojection_matrixの3つ目(407.7...)がx軸、7つ目(294.4...)がy軸の中心位置になります。
またそれぞれの行列の1つ目(400程度)がx軸方向の(pixel数->角度[radian])の変換の係数になります

camera_infoの例

image_width: 800
image_height: 600
camera_name: narrow_stereo
camera_matrix:
  rows: 3
  cols: 3
  data: [383.948853, 0.000000, 400.000000, 0.000000, 385.635798, 300.000000, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.013629, -0.015113, -0.006977, 0.005697, 0.000000]
rectification_matrix:
  rows: 3
  cols: 3
  data: [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
projection_matrix:
  rows: 3
  cols: 4
  data: [383.416656, 0.000000, 407.764958, 0.000000, 0.000000, 388.115448, 294.436569, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

物理的に光軸中心をとおるような場所に目標の物を置きます。それがcameraの上どのピクセルに映っているかを確認します。これを先ほどのpixel->角度の係数で割ると何radずれているのかが分かります。これをもとにrectification_matrixを編集します。rectification_matrixはキャリブレーションをしても必ず3行3列の単位行列になります。これを上記のずれを直す回転行列に直せばよいです。以下の回転行列を生成するpythonスクリプトです。

cam_lecture/scripts/rpy_matrix.py

#!/usr/bin/env python

import sys
import numpy as np
import tf.transformations as tr

try:
    args = sys.argv
    roll = float(args[1])
    pitch = float(args[2])
    yaw = float(args[3])
except:
    print("syntax: rpy_matrix.py roll pitch yaw")
else:
    np.set_printoptions(precision=3, floatmode='fixed')
    R0 = tr.euler_matrix(roll, pitch, yaw, 'sxyz')
    R0 = R0[0:3, 0:3].reshape(-1)
    print(np.array2string(R0, precision=3, separator=',', suppress_small=True))

上のスクリプトで生成された行列をrectification_matrixのdataのところに足すことで画像をずらすことができます。

参考

image_geometry tutorial
camera_ionfoのmatrixの説明

目次ページへのリンク

ROS講座の目次へのリンク