目的
ロボットの自律移動に必要な外界の情報を獲得するために様々なセンサが搭載されます．ここでは，センサの一つであるカメラを使用して，ROSで画像処理するための方法を学びます．なお，本内容はROSによる画像処理(Kinect版)のアレンジです．今回はKinectを使用する代わりにロボットシミュレータから取得できる画像を利用します．
画像トピックの確認
以前の講義で使用したロボットシミュレータStageを利用します．よりリアルなシミュレーションが必要な場合にはGAZEBOがお勧めです．Stageでは簡単なセンサシミュレーションしかできませんが，スペックが低いPCでも動作します．stage_rosでは幾つかのモデルが用意されていますが，カメラが搭載されているロボットを使用しましょう．
$ rosrun stage_ros stageros $(rospack find stage_ros)/world/willow-four-erratics-multisensor.world
起動するとウィンドウが立ち上がり，マップ上部に移動すると下記のような正方形の図形があらわれます．4つの青い正方形がロボットを表しています．ロボット1体につき，2機の測域センサ，2機のカメラが搭載されています．なお，カメラからはカラー画像の他に，カメラから物体までの距離を表わす距離画像を取得可能です．なお，デフォルトのカメラのスペックは，解像度が100×100画素，FOV(Field of View)が横70°，縦40°，距離の測定範囲は0.2~8.0mです．
それでは，ロボットに搭載されているカメラからの画像を確認しましょう．ROSでは幾つかの画像ビューアーが用意されています．以前の講義で学んだrvizでも画像を表示できますが，今回はより簡単な方法で画像を確認しましょう．
$ rosrun rqt_image_view rqt_image_view
正常に起動するとウィンドウが立ちがるので，左上のプルダウンから画像のトピックを選択しましょう．下記は/robot_0/image_0と/robot_0/depth_0を選択した際の画像です．
ROSよる画像処理
それでは，画像処理するノードを作成しましょう．画像処理にはopencvを利用します．ただし，ROSとopencvでは，画像を扱う形式が異なるため，opencvで扱える形式に変換するcv_bridgeを利用します．
1. パッケージの作成
ROSで画像処理を行う場合には，依存パッケージとして下記を指定してください．なお，本ページではパッケージの名前をdepth_estimaterとして話を進めます．
cv_bridge roscpp sensor_msgs std_msgs
2. プログラミング
それでは，カメラから得られたRGB画像とD画像を処理するプログラムを書きます．今回は，RGB画像の注目する領域(ROI領域)を赤く表示し，そのROI領域の平均距離を計算するプログラムを作成します．
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#define WIDTH 20
#define HEIGHT 15
class depth_estimater{
public:
depth_estimater();
~depth_estimater();
void rgbImageCallback(const sensor_msgs::ImageConstPtr& msg);
void depthImageCallback(const sensor_msgs::ImageConstPtr& msg);
private:
ros::NodeHandle nh;
ros::Subscriber sub_rgb, sub_depth;
};
depth_estimater::depth_estimater(){
sub_rgb = nh.subscribe<sensor_msgs::Image>("/robot_0/image_0", 1, &depth_estimater::rgbImageCallback, this);
sub_depth = nh.subscribe<sensor_msgs::Image>("/robot_0/depth_0", 1, &depth_estimater::depthImageCallback, this);
}
depth_estimater::~depth_estimater(){
}
void depth_estimater::rgbImageCallback(const sensor_msgs::ImageConstPtr& msg){
int i, j;
int x1, x2, y1, y2;
int width = WIDTH;
int height = HEIGHT;
cv_bridge::CvImagePtr cv_ptr;
try{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}catch (cv_bridge::Exception& ex){
ROS_ERROR("error");
exit(-1);
}
cv::Mat &mat = cv_ptr->image;
x1 = int(mat.cols / 2) - width;
x2 = int(mat.cols / 2) + width;
y1 = int(mat.rows / 2) - height;
y2 = int(mat.rows / 2) + height;
for(i = y1; i < y2; i++){
for(j = x1; j < x2; j++){
// 0 : blue, 1 : green, 2 : red.
mat.data[i * mat.step + j * mat.elemSize() + 0] = 0;
mat.data[i * mat.step + j * mat.elemSize() + 1] = 0;
//mat.data[i * mat.step + j * mat.elemSize() + 2] = 0;
}
}
cv::rectangle(cv_ptr->image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 0, 255), 3, 4);
cv::imshow("RGB image", cv_ptr->image);
cv::waitKey(10);
}
void depth_estimater::depthImageCallback(const sensor_msgs::ImageConstPtr& msg){
int x1, x2, y1, y2;
int i, j, k;
int width = WIDTH;
int height = HEIGHT;
double sum = 0.0;
double ave;
cv_bridge::CvImagePtr cv_ptr;
try{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
}catch (cv_bridge::Exception& ex){
ROS_ERROR("error");
exit(-1);
}
cv::Mat depth(cv_ptr->image.rows, cv_ptr->image.cols, CV_32FC1);
cv::Mat img(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1);
x1 = int(depth.cols / 2) - width;
x2 = int(depth.cols / 2) + width;
y1 = int(depth.rows / 2) - height;
y2 = int(depth.rows / 2) + height;
for(i = 0; i < cv_ptr->image.rows;i++){
float* Dimage = cv_ptr->image.ptr<float>(i);
float* Iimage = depth.ptr<float>(i);
char* Ivimage = img.ptr<char>(i);
for(j = 0 ; j < cv_ptr->image.cols; j++){
if(Dimage[j] > 0.0){
Iimage[j] = Dimage[j];
Ivimage[j] = (char)(255*(Dimage[j]/8.0));
}else{
}
if(i > y1 && i < y2){ if(j > x1 && j < x2){ if(Dimage[j] > 0.0){
sum += Dimage[j];
}
}
}
}
}
ave = sum / ((width * 2) * (height * 2));
ROS_INFO("depth : %f [m]", ave);
cv::imshow("DEPTH image", img);
cv::waitKey(10);
}
int main(int argc, char **argv){
ros::init(argc, argv, "depth_estimater");
depth_estimater depth_estimater;
ros::spin();
return 0;
}
3. ビルド
画像処理ライブラリであるopencvを使用しているため，CMakeLists.txtにその旨を記述する必要があります．2箇所のハイライト部分を追加しましょう．
find_package(catkin REQUIRED COMPONENTS cv_bridge roscpp sensor_msgs std_msgs ) find_package(OpenCV REQUIRED)
add_executable(depth_estimater src/depth_estimater.cpp)
target_link_libraries(depth_estimater ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
4. 実行
作成したプログラムを実行します．
次に，rosrunにより作成したノードを起動しましょう．下記に作成したノードを実行した例を示します．
5. プログラムの内容
それでは，プログラムの内容を確認しましょう．
#include <ros/ros.h> #include <sensor_msgs/Image.h> #include <cv_bridge/cv_bridge.h> #include <opencv2/opencv.hpp>
必要なヘッダファイルを読み込んでいます．Open CVを使用する場合には，多くのヘッダファイルが必要になるので，全て記述しましょう．
#define WIDTH 20 #define HEIGHT 15
赤枠の大きさです．実際には，(WIDTH*2)x(HEIGHT*2)の画素数となるので注意してください．
class depth_estimater{
public:
depth_estimater();
~depth_estimater();
void rgbImageCallback(const sensor_msgs::ImageConstPtr& msg);
void depthImageCallback(const sensor_msgs::ImageConstPtr& msg);
private:
ros::NodeHandle nh;
ros::Subscriber sub_rgb, sub_depth;
};
depth_estimaterというクラスを定義し，幾つかのクラスメンバを宣言しています．カラー画像を購読した際にはrgbImageCallback関数，距離画像を購読した際にはdepthImageCallback関数が実行されます．
void depth_estimater::rgbImageCallback(const sensor_msgs::ImageConstPtr& msg){
int i, j;
int x1, x2, y1, y2;
int width = WIDTH;
int height = HEIGHT;
cv_bridge::CvImagePtr cv_ptr;
try{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}catch (cv_bridge::Exception& ex){
ROS_ERROR("error");
exit(-1);
}
cv::Mat &mat = cv_ptr->image;
x1 = int(mat.cols / 2) - width;
x2 = int(mat.cols / 2) + width;
y1 = int(mat.rows / 2) - height;
y2 = int(mat.rows / 2) + height;
for(i = y1; i < y2; i++){
for(j = x1; j < x2; j++){
// 0 : blue, 1 : green, 2 : red.
mat.data[i * mat.step + j * mat.elemSize() + 0] = 0;
mat.data[i * mat.step + j * mat.elemSize() + 1] = 0;
//mat.data[i * mat.step + j * mat.elemSize() + 2] = 0;
}
}
cv::rectangle(cv_ptr->image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 0, 255), 3, 4);
cv::imshow("RGB image", cv_ptr->image);
cv::waitKey(10);
}
カラー画像を購読した際には上記の関数が実行されます．詳細を見ましょう．
int i, j;
int x1, x2, y1, y2;
int width = WIDTH;
int height = HEIGHT;
cv_bridge::CvImagePtr cv_ptr;
変数を宣言しています．cv_ptrは画像を扱うためのものです．
try{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}catch (cv_bridge::Exception& ex){
ROS_ERROR("error");
exit(-1);
}
cv::Mat &mat = cv_ptr->image;
画像を購読し，BGR8形式(1チャンネル(青，緑，赤のどれか1つ)につき8ビット(0~255階調))のカラー画像に変換し，cv_ptrに代入しています．ここで注意すべき点は，チャンネルの順番がBGR，すなわち青，緑，赤の順になっている点です．また，画像を購読できない場合には，エラーを出力します．そして，cv_ptrをmatに代入しています．
x1 = int(mat.cols / 2) - width;
x2 = int(mat.cols / 2) + width;
y1 = int(mat.rows / 2) - height;
y2 = int(mat.rows / 2) + height;
赤枠の大きさを設定しています．
for(i = y1; i < y2; i++){
for(j = x1; j < x2; j++){
// 0 : blue, 1 : green, 2 : red.
mat.data[i * mat.step + j * mat.elemSize() + 0] = 0;
mat.data[i * mat.step + j * mat.elemSize() + 1] = 0;
//mat.data[i * mat.step + j * mat.elemSize() + 2] = 0;
}
}
赤枠内の画素にアクセスし，青と緑のチャンネルに0を代入しています．チャンネルの順番は，青，緑，赤です．画素にアクセスしているプログラムは3行ありますが，先ほどのチャンネルの順番となっているので注意しましょう．画素にアクセスする方法は幾つかあります．アクセス方法によっては，時間がかかる場合があるので注意が必要です．アクセスの仕方についてはこちらのページを参考にしましょう．
cv::rectangle(cv_ptr->image, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(0, 0, 255), 3, 4);
cv::imshow("RGB image", cv_ptr->image);
cv::waitKey(10);
最後に，rectangle関数で枠の外側を赤色で囲っています．そして，imshow関数により画像を表示しています．waitKey関数はキーが押されるまで待機する関数です．今回はキー入力を必要としませんが，画像を表示する場合には必要な関数なので記述しましょう．
void depth_estimater::depthImageCallback(const sensor_msgs::ImageConstPtr& msg){
int x1, x2, y1, y2;
int i, j, k;
int width = WIDTH;
int height = HEIGHT;
double sum = 0.0;
double ave;
cv_bridge::CvImagePtr cv_ptr;
try{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
}catch (cv_bridge::Exception& ex){
ROS_ERROR("error");
exit(-1);
}
cv::Mat depth(cv_ptr->image.rows, cv_ptr->image.cols, CV_32FC1);
cv::Mat img(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1);
x1 = int(depth.cols / 2) - width;
x2 = int(depth.cols / 2) + width;
y1 = int(depth.rows / 2) - height;
y2 = int(depth.rows / 2) + height;
for(i = 0; i < cv_ptr->image.rows;i++){
float* Dimage = cv_ptr->image.ptr<float>(i);
float* Iimage = depth.ptr<float>(i);
char* Ivimage = img.ptr<char>(i);
for(j = 0 ; j < cv_ptr->image.cols; j++){
if(Dimage[j] > 0.0){
Iimage[j] = Dimage[j];
Ivimage[j] = (char)(255*(Dimage[j]/8.0));
}else{
}
if(i > y1 && i < y2){ if(j > x1 && j < x2){ if(Dimage[j] > 0.0){
sum += Dimage[j];
}
}
}
}
}
ave = sum / ((width * 2) * (height * 2));
ROS_INFO("depth : %f [m]", ave);
cv::imshow("DEPTH image", img);
cv::waitKey(10);
}
距離画像を購読した際には上記の関数が実行されます．もう少し詳細を見ましょう．
int x1, x2, y1, y2;
int i, j, k;
int width = WIDTH;
int height = HEIGHT;
double sum = 0.0;
double ave;
cv_bridge::CvImagePtr cv_ptr;
カラー画像と同様に変数を宣言しています．
try{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
}catch (cv_bridge::Exception& ex){
ROS_ERROR("error");
exit(-1);
}
上記もカラー画像とほぼ同じ内容です．異なる点は，TYPE_32FC1形式に変換している点です．TYPE_32FC1は，1チャンネル浮動小数点数(1つの画素に32ビット)を表しています．
cv::Mat depth(cv_ptr->image.rows, cv_ptr->image.cols, CV_32FC1);
cv::Mat img(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC1);
指定した大きさの配列を確保しています．CV_8UC1は，先ほど出てきたBGR8と同じです．
x1 = int(depth.cols / 2) - width;
x2 = int(depth.cols / 2) + width;
y1 = int(depth.rows / 2) - height;
y2 = int(depth.rows / 2) + height;
枠の大きさを設定しています．
for(i = 0; i < cv_ptr->image.rows;i++){
float* Dimage = cv_ptr->image.ptr<float>(i);
float* Iimage = depth.ptr<float>(i);
char* Ivimage = img.ptr<char>(i);
for(j = 0 ; j < cv_ptr->image.cols; j++){
if(Dimage[j] > 0.0){
Iimage[j] = Dimage[j];
Ivimage[j] = (char)(255*(Dimage[j]/8.0));
}else{
}
if(i > y1 && i < y2){ if(j > x1 && j < x2){ if(Dimage[j] > 0.0){
sum += Dimage[j];
}
}
}
}
}
ave = sum / ((width * 2) * (height * 2));
ROS_INFO("depth : %f [m]", ave);
枠内の距離値の合計を計算しています．その際，もし距離が0[m]以下であれば，除外しております．先ほどとは画像のアクセスの仕方が異なります．
CV_32FC1型では，dataメソッドを使用して画素にアクセスすることができないため，ptrメソッドを使用しています．また，距離画像を可視化するために，100行目では距離値を取得できる最大距離で割り，255を掛けています．255は8ビットで表現できる最大の数です．最後に，114行目で枠内の画素数で割り，計算した平均距離を次の行で出力しています．
cv::imshow("DEPTH image", img);
cv::waitKey(10);
カラー画像を購読する関数と同様に，可視化した距離画像を表示しています．
int main(int argc, char **argv){
ros::init(argc, argv, "depth_estimater");
depth_estimater depth_estimater;
ros::spin();
return 0;
}
以前と変わらない処理なので説明を省略します．
課題
- ロボットが前進している際に，ロボットと前方の障害物までの距離が1.0m以下になったら停止しなさい．
- ROI領域の大きさを40×30画素から60×40画素に変更しなさい．
- ROI領域の位置を画面中央から画面下部に変更しなさい．具体的な位置については指示しないが，課題1. を実施する際に適切だと考えられる位置に設定すること．
- ロボットが前進している際に，ロボットと前方の障害物までの距離が1.0m以下になったら迂回しなさい．例えば，前方の障害物の右，及び左の空間に対して障害物判定を行い，空いている空間を進むことで迂回を実装できます．ロボットの並進運動の他に回転運動を取り入れてください．
- ROI領域の数を3つに増やしなさい．なお，ROI領域の大きさ，位置については指示しないが，課題4.を実施する際に適切だと考えられる大きさ，位置に設定すること．なお，効率の良い実装ができない場合には画像処理の計算が追いつかないので注意すること．