視覚センサの利用

masutani · August 17, 2018, 6:22am

カメラや深度センサやレンジセンサのデータをRTミドルウェアのポートを介して外部へ取り出すにはどうしたらいいのでしょうか？バージョン１．５の頃にBodyRTCの設定ファイルに
out-port = camera:CAMERA_IMAGE
などと書いていた内容に対応する機能です．
sample/OpenRTM/OpenRTM-TankVisionSensors.cnoid
がそれに対応する例だと思うのですが，考え方や仕様がよくわからずに困っています．
マニュアルは
http://choreonoid.org/ja/manuals/latest/openrtm/tank-joystick-vision-sensors.html
でしょうか？まだ空です．
他に見るべきドキュメントがありましたら教えてください．よろしくお願いいたします．

nakaoka · August 17, 2018, 8:43am

従来のBoryRTCItemのやり方もできなくはないですが、今後はBodyIoRTCItemに移行したく思っています。

視覚センサについては、とりあえずsample/OpenRTMに入っている、VisionSensorIoRTCを使ってセンサデータを出力することができます。

BodyIoRTCItemを作成し、対象のBodyItemの小アイテムとして配置し、VisionSensorIoRTCを使うよう設定します。すると、対象のモデルが有している視覚センサに対応する出力ポートが自動で生成されます。

ただしまだ一点問題がありまして、RGB-Dカメラの深度画像について、出力できるようになっていません。これはVisionSensorIoRTC.cppのソースコードを見て頂ければ分かるように、対応する部分がコメントアウトされています。

これに関して、これまでOpenRTMの公式ではない古いIDLを使っていたのですが、それは使わずにOpenRTMに付属のIDLを使った方がよいということが分かったため、そちらに移行する必要があるのですが、その作業がまだ完了しておりませんでした。こちらについても順次対応を進めていきます。

一方で、カメラ画像、レンジセンサ画像については、これで動作するかと思います。

RGB-Dカメラの深度画像については、自前で適当なIDLに対応するか、BodyRTCItemを使うか、上記の対応を待っていただく、ということになるかと思います。

いろいろと不備がありまして申し訳ありませんが、よろしくお願いいたします。

masutani · August 18, 2018, 8:34am

回答ありがとうございます。
色々試してみて、VisionSensorIoRTCの大まかな仕様については理解しました。

しかし、データポートでImg::TimedCameraImageを扱うRTCの運用に手こずっています。
おそらく、2017年11月16日にopenrtm-users@openrtm.orgでRTShell vs. RT System Editorというスレッドで話題になった問題が解消されていないのではないかと思います。

そのスレッドの中の原様のメイルを一部引用します。

産総研の原です。
少し気になってChoreonoidのIDLファイルを見てみました。
Choreonoidでは、CameraImage.idlというファイルがあったのですが、この中で定義されている
enum ColorFormat の定義がCameraCommonInterface.idlと異なっています。

何故かChoreonoidでは、勝手に変更されており、この部分が原因だと思われます。
＃両者ともに module Img ですので、名前空間が同じですので。

RTSystemEditorのidlファイルは調べていませんが、少なくともidlファイル内で同じ名前の型の定義が異なるのでは、正常に動作しないと思います。

現在使っているシステムの
/usr/include/openrtm-1.1/rtm/ext/CameraCommonInterface.idl
とChoreonoidのソースに含まれる
https://github.com/s-nakaoka/choreonoid/blob/master/src/OpenRTMPlugin/deprecated/corba/CameraImage.idl
は異なっています。

masutani · August 18, 2018, 9:24am

RTミドルウェアでポイントクラウドを扱うことこれまで試してきましたので、少し意見を言わせてください。

「OpenRTMに付属のIDL」とは、
/usr/include/openrtm-1.1/rtm/idl/InterfaceDataTypes.idl
に含まれるRTC::PointCloudのことですね？これは標準には含まれていますが、私はこれを使った実装を見たことがありません。それから、1点に48byte（double×6）使っているため、データが大変大きくなってしまします。

一方、「OpenRTMの公式ではない古いIDL」とは、RTC:PCLに含まれるPointCloudTypes::PointCoudと同じものだと思います。

github.com

gbiggs/rtcpcl/blob/master/pc_type/pointcloud.idl

/* RTC:OpenNI
 *
 * Copyright (C) 2011
 *     Geoffrey Biggs
 *     RT-Synthesis Research Group
 *     Intelligent Systems Research Institute,
 *     National Institute of Advanced Industrial Science and Technology (AIST),
 *     Japan
 *     All rights reserved.
 * Licensed under the Eclipse Public License -v 1.0 (EPL)
 * http://www.opensource.org/licenses/eclipse-1.0.txt
 *
 * Point cloud data type.
 */


#ifndef POINTCLOUD_IDL__
#define POINTCLOUD_IDL__

module PointCloudTypes

This file has been truncated. show original

標準ではないといえ、これにはいくつが実装があり、1点のデータは16byteです。私は、引き続きこちらのデータ型を使った方がいいのではないかと思います。

ちなみに、私もPointCloudTypes::PointCoudを使うRTCをいくつか作りました。

masutani · August 18, 2018, 9:42am

申し訳ありません．誤解でした．
choreonoid/sample/OpenRTM/VisionSensorIoRTC.cpp
では，
#include <rtm/ext/CameraCommonInterface.hh>
しています．

とはいえ，以前と同じような問題は出ています．
VisionSensorIoRTCを使っていると，
rtls /localhost
などとすると，
rtls: CORBA.MARSHAL(omniORB.MARSHAL_InvalidEnumValue, CORBA.COMPLETED_YES)
というエラーが出て，RTShellを使った運用ができずにいます．

masutani · August 18, 2018, 9:59am

連投失礼します．今わかっていることを書いておきます．

VisionSensorIoRTCのインスタンス名が短く，ポートの数が少ないと，問題が発生しないことは確認しています．例えば，

bin/choreonoid sample/OpenRTM/OpenRTM-AizuSpiderSS-Remote.cnoid

を実行したあとに，他の端末で

rtls /localhost

を実行すると，

rtls: CORBA.MARSHAL(omniORB.MARSHAL_InvalidEnumValue, CORBA.COMPLETED_YES)

になります．

一方，同じ環境で，

bin/choreonoid sample/OpenRTM/OpenRTM-TankVisionSensors.cnoid

を実行したあとに，他の端末で

rtls /localhost

を実行すると，

VisionSensorSubscriber.rtc  Tank-VisionSensorIoRTC.rtc     MessageView
CameraImageView.rtc         TankJoystickControllerRTC.rtc  JoystickRTC.rtc
Tank-TankIoRTC.rtc

と正常に動作します．

どれぐらい一般性があるかわかりませんが，bodyファイルの先頭のname:で名前を短くし，カメラを4個々以下にするとエラーが出なくなりました．

masutani · August 18, 2018, 12:03pm

さらに連投失礼します．

Img::TimedCameraImage の実装例は，大原先生の作られた

だと思っています．ですが，これにVisionSensorIoRTCの出力をImageViewerにつなぐと色が変です．

コードの中の

github.com

rsdlab/ImageViewer/blob/master/src/ImageViewer.cpp#L230


      
          		else if( m_Image.data.image.format == Img::CF_JPEG || m_Image.data.image.format == Img::CF_PNG )
          		{
          			std::vector<uchar> compressed_image = std::vector<uchar>(data_length);
          			memcpy(&compressed_image[0], &m_Image.data.image.raw_data[0], sizeof(unsigned char) * data_length);
          
          			//Decode received compressed image
          			cv::Mat decoded_image;
          			if(channels == 3)
          			{
          			  decoded_image = cv::imdecode(cv::Mat(compressed_image), cv::IMREAD_COLOR);
          				//cv::cvtColor(decoded_image, image, CV_RGB2BGR);
          				image = decoded_image;
          			}
          			else
          			{
          			  decoded_image = cv::imdecode(cv::Mat(compressed_image), cv::IMREAD_GRAYSCALE);
          				image = decoded_image;
          			}
          		}
            }

cv::cvtColor(decoded_image, image, CV_RGB2BGR);
を
image = decoded_image
に置き換えると，色が正常になります．

VisionSensorIoRTCから送り出す色の順序がBGRになっていませんか？

masutani · August 19, 2018, 2:02am

昨日の続きです．

RTShellのrtls /localhostでエラーになると書きましたが，RTShell全体に問題があるのではなく，VisionSensorIoRTCからプロフィールを得るところに問題があるようです．

Pythonのバックトレースの最後の部分

File "/usr/local/lib/python2.7/dist-packages/rtctree/rtc/RTC_idl.py", line 1344, in get_component_profile
  return _omnipy.invoke(self, "get_component_profile", _0_RTC.RTObject._d_get_component_profile, args)

問題が起こらない（起こりにくい？）こともあります．
インスタンス名が短かったり，ポートの数が少なかったりする場合は問題が起こりにくいです．
Chorenoidでシミュレーションを一旦実行すると問題が起こらなくなります．RTCのActivateだけではだめなようです．

事例1：問題が起こらない場合
端末1で以下を実行．

bin/choreonoid sample/OpenRTM/OpenRTM-TankVisionSensors.cnoid

端末2の実行状況

$ rtls /localhost
VisionSensorSubscriber.rtc  Tank-VisionSensorIoRTC.rtc
MessageView                 CameraImageView.rtc
ubuntu.host_cxt/             TankJoystickControllerRTC.rtc
JoystickRTC.rtc             Tank-TankIoRTC.rtc
$ rtls /localhost/Tank-VisionSensorIoRTC.rtc
Tank-VisionSensorIoRTC.rtc
$ rtcat /localhost/Tank-VisionSensorIoRTC.rtc
Tank-VisionSensorIoRTC.rtc  Inactive
  Category       Generic
  Description    Vision sensor I/O
  Instance name  Tank-VisionSensorIoRTC
  Type name      VisionSensorIoRTC
  Vendor         Choreonoid
  Version        1.0.0
  Parent         
  Type           Monolithic
 +Extra properties
+Execution Context 0
+DataOutPort: Kinect
+DataOutPort: Theta
+DataOutPort: VLP-16

事例2：問題が起こる場合
端末1で以下を実行．

bin/choreonoid sample/OpenRTM/OpenRTM-AizuSpiderSS-Remote.cnoid

端末2の実行状況

$ rtls /localhost
rtls: CORBA.MARSHAL(omniORB.MARSHAL_InvalidEnumValue, CORBA.COMPLETED_YES)
$ rtls /localhost/AizuSpider-JoystickInput.rtc
AizuSpider-JoystickInput.rtc
$ rtcat /localhost/AizuSpider-JoystickInput.rtc
AizuSpider-JoystickInput.rtc  Inactive
  Category       HumanInterfaceDevice
  Description    RTC for inputting joystick state via RTC
  Instance name  AizuSpider-JoystickInput
  Type name      JoystickInput
  Vendor         AIST
  Version        1.0.0
  Parent         
  Type           Monolithic
 +Extra properties
+Execution Context 0
+DataInPort: axes
+DataInPort: buttons
$ rtls /localhost/AizuSpider-VisionSensorIoRTC.rtc
rtls: CORBA.MARSHAL(omniORB.MARSHAL_InvalidEnumValue, CORBA.COMPLETED_YES)
$ rtcat /localhost/AizuSpider-VisionSensorIoRTC.rtc
rtcat: CORBA.MARSHAL(omniORB.MARSHAL_InvalidEnumValue, CORBA.COMPLETED_YES)

ここで，Choreonoidでシミューレションを開始して，停止する．

その後の端末2の実行状況

$ rtcat /localhost/AizuSpider-VisionSensorIoRTC.rtc
AizuSpider-VisionSensorIoRTC.rtc  Inactive
  Category       Generic
  Description    Vision sensor I/O
  Instance name  AizuSpider-VisionSensorIoRTC
  Type name      VisionSensorIoRTC
  Vendor         Choreonoid
  Version        1.0.0
  Parent         
  Type           Monolithic
 +Extra properties
+Execution Context 0
+DataOutPort: LRF
+DataOutPort: FRONT_CAMERA
+DataOutPort: LEFT_CAMERA
+DataOutPort: RIGHT_CAMERA
+DataOutPort: BACK_CAMERA
+DataOutPort: ARM_CAMERA

長くて申し訳ありません．今わかっているのはここまでです．
RTShellを使いたいので，とても困っています．

masutani · August 19, 2018, 7:51am

BodyIoRTCの利用をあきらめ，BodyRTCを使おうとしていますが，結局同じ問題にぶつかっています．原因が同じかどうかはわかりません．

モデルは，DoubleArmV7S.bodyを使っています．
BodyRTCの設定ファイルに，

out-port = FRAME_FRONT_CAMERA:FRAME_FRONT_CAMERA:CAMERA_IMAGE

と書くとカメラ画像はうまく取り出せています．しかし，カメラの出力ポートを2個以上にすると，例えば，

out-port = FRAME_FRONT_CAMERA:FRAME_FRONT_CAMERA:CAMERA_IMAGE
out-port = WORK_RIGHT_VIEW:WORK_RIGHT_VIEW:CAMERA_IMAGE

として，RTShellで扱おうとすると，

CORBA.MARSHAL(omniORB.MARSHAL_InvalidEnumValue, CORBA.COMPLETED_YES)

を喰らいます．

ところで，上記で出力したImg::TimedCameraImageの画像データは色の順番がRGBになっているようです．

nakaoka · August 20, 2018, 8:03am

詳細なご報告をありがとうございます。
まず問題を整理したいと思います。

カメラ画像のIDLについて

これについては、ご指摘のようにOpenRTM標準のを使っていないという問題がありましたので、最新版では標準のIDLを使用しています。OpenRTMにインストールされているスタブをリンクしていますので、完全にOpenRTM準拠になっているはずです。

升谷先生の指摘された「Choreonoidのソースに含まれる」古いIDLは、互換性のために入れているものです。これはCMakeで “USE_BUILTIN_CAMERA_IMAGE_IDL” を ONにすると一部で使用されますが、デフォルトではOFFとしています。（ONにすると標準のIDLと競合するので、特に理由がなければOFFにしておきます。）

「データポートでImg::TimedCameraImageを扱うRTCの運用に手こずっています」とのことですが、これはRTShellでうまく扱えないということですね。ただ、IDLの違いで問題が起きているわけではないように思います。

ポイントクラウドのIDLについて

ポイントクラウド用のIDLとしては、OpenRTM標準のものではなく、「RTC:PCLに含まれるPointCloudTypes::PointCoud」を使うようにしたい、ということですね。

これについては、実は産総研のOpenRTM開発チーム（該当IDL作者のGeoff氏もおられる）と打ち合わせをした際に、そのIDLを使うのは止めたほうがよいと言われた経緯があります。これを標準に取り込むという気もなさそうな感じでした。それで、標準のIDLに移行すべきなのだと思い込んいた次第です。

貴重なコメントありがとうございます。「これを使った実装を見たことがない」という実態も、把握しておりませんでした。

これについては、引き続き古いIDLも使えるようにすることを検討します。これについてはネームスペースが異なるので、特に競合するということもないですよね。

カメラ画像出力のRGBの並びが間違っている

ご指摘ありがとうございます。これについては確認して修正します。

RTShellでうまく扱えないときがある

状況は概ね把握いたしました。
これについては、まだ理由などは分かっていません。Choreonoidの問題というよりは、OpenRTMとRTShellの問題のような気もしますが・・・。
「インスタンス名が長く、ポートの数が多い」と問題が発生しやすいようですが、これはChoreonoidとは独立してRTCやRTShellを動作させた場合でも同様でしょうか？あるいは、Choreonoidでしか発生しない問題でしょうか？

エラーとしては、omniORB.MARSHAL_InvalidEnumValueなのですね。インスタンス名やポートの数でEnumの値が変わるということはなさそうなので、とても不可解な症状ではありますね。Choreonoidで一旦シミュレーションをすると直ることがあると言われると、Choreonoidが悪さをしている気もします。RTCの初期化が十分でないのかも？

すみません、このあたりはOpenRTM開発チームに相談した方がよさそうです。（ただ、メーリングリストが今はとまっている？）とりあえず開発メンバーにここを見てもらうよう連絡してみます。

masutani · August 20, 2018, 8:51am

「データポートでImg::TimedCameraImageを扱うRTCの運用に手こずっています」と書いてしまったのは，以前に経験した「2017年11月16日にopenrtm-users@openrtm.orgでRTShell vs. RT System Editorというスレッド」や VisionSensorだけのBodyRTCがrtshellで扱えない（omniORB.MARSHAL_InvalidEnumValue） · Issue #173 · s-nakaoka/choreonoid · GitHub に引きずられていたからです．

ただ，同じBoryIoRTCでも，TimedDoubleSeqのポートだけのものでは問題が起きていません．

rtls /localhost/AizuSpider-JoystickInput.rtc

は正常で，

rtls /localhost/AizuSpider-VisionSensorIoRTC.rtc

で

rtls: CORBA.MARSHAL(omniORB.MARSHAL_InvalidEnumValue, CORBA.COMPLETED_YES)

になります．

gbiggs · August 21, 2018, 7:41am

omniORB.MARSHAL_InvalidEnumValueであれば、問題は多分Choreonoid側だと思います。RTShellが利用しているIDLに定義されていない値をどこかのenumに入れているということです。

nakaoka · August 23, 2018, 9:25am

コメントありがとうございます。OpenRTMに標準搭載のIDLと、Choreonoidに組み込まれたIDLの間で、型名は同じでも値の定義が異なるenumがありそうということですね。

ちょっとこのトピックが長くなってしまったので、この件に関してトピックを立て直します。

あと、ポイントクラウド用のIDLについて、升谷先生によるとジェフさん作成の古いIDLを使いたいとのことですが、OpenRTM開発チームとしては標準搭載のIDLを使って欲しいということだったかと思います。これについて、古いIDLを実際に定義した方として、見解をお知らせいただけるとうれしいです。

hattorishizuko · August 24, 2018, 4:39am

RGBが逆になっているという件です。

https://docs.opencv.org/2.4.9/modules/highgui/doc/reading_and_writing_images_and_video.html
では、
In the case of color images, the decoded images will have the channels stored in B G R order.
となっているので、RGBからBGRへの変換はいらないのではないかと思うのですが。

masutani · August 24, 2018, 8:05am

そうなんですね！
ということは，私が実装の見本にしていたImageViewerの

github.com

rsdlab/ImageViewer/blob/master/src/ImageViewer.cpp#L230


      
          		else if( m_Image.data.image.format == Img::CF_JPEG || m_Image.data.image.format == Img::CF_PNG )
          		{
          			std::vector<uchar> compressed_image = std::vector<uchar>(data_length);
          			memcpy(&compressed_image[0], &m_Image.data.image.raw_data[0], sizeof(unsigned char) * data_length);
          
          			//Decode received compressed image
          			cv::Mat decoded_image;
          			if(channels == 3)
          			{
          			  decoded_image = cv::imdecode(cv::Mat(compressed_image), cv::IMREAD_COLOR);
          				//cv::cvtColor(decoded_image, image, CV_RGB2BGR);
          				image = decoded_image;
          			}
          			else
          			{
          			  decoded_image = cv::imdecode(cv::Mat(compressed_image), cv::IMREAD_GRAYSCALE);
          				image = decoded_image;
          			}
          		}
            }

の処理が要らないということですね？

nakaoka · August 25, 2018, 3:17am

そもそもCameraCommonInterface.idlで定義されているImg::ColorFormatのCF_RGBが何を意味するかという問題ですよね。
普通に考えれば、“RGB” とある以上、R、G、Bの順番にメモリに配置するものだとは思います。Choreonoidの実装ではそのようにしています。

しかしImageViewerの実装では、これをB、G、Rの順番で配置しているという前提になっているようですね。

これについて、CameraCommonInterface.idlのコメントではとくに順番に関しては書かれていませんし、以下の仕様書でも、順番に関しては書かれていませんでした。

私の勝手な予想ですが、Opencvのjpegデコード関数が、結果をB、G、Rで返すようになっているので、それを表示するためにビュアー側で順番を逆にする必要があり、ついでにCF_RGBについても同じ実装にしてしまったのではないでしょうか？両方で実装を合わせるために。

ただ、そのあたりの事情はよく分かりません。フォーマット名で RGB とあるのに、実はBGRにしなければならないとしたら、混乱する仕様ですし、そうする事情があるのであればIDLのコメント等で明記するべきだと思います。

そのあたり、OpenRTM関係者の見解としてはいかがでしょうか？やはりここは逆にするべきなのでしょうか？

それとも仕様上はRGBの順番でよいが、Choreonoidの実装が実はBGRに逆転しているだけだとしたらすみません。いちおう今回実装を確認した限りではRGBの順番になっているようでしたが、そこまで詳細を確認できているわけではないので・・・。

まずは仕様が明らかになることを期待します。

nakaoka · August 25, 2018, 7:06am

すみません、どうもよく分かっていなかったようで、升谷先生が

VisionSensorIoRTCから送り出す色の順序がBGRになっていませんか？

と聞かれているので、やはりRGBの順が正しくて、それでよいということですね？

それでChoreonoidからの出力はその逆のBGRの順になっているのですね？

だとしたらChoreonoidの問題だと思いますので、再度実装をチェックします。

nakaoka · August 25, 2018, 7:08am

ただ、最近サンプル(VisionSensorIoRTC)ではjpeg出力にも対応しました。jpegの方がデータ量がずっと少なくて済むので、遠隔操作にはよいですし、RGBの順序が問題になることもないので、そちらの利用もご検討ください。

masutani · August 26, 2018, 12:48am

セックのサイトに置かれている「カメラ機能共通インタフェース仕様書（第１.0版）2012年2月24日」は、OpenRTM-aist-1.1.2に含まれているCameraCommonInterface.idlと異なっているようです。ファイルの先頭には、/* version 2.0 */ とありますし、enum ColorFormat の要素が多数定義されています。

ただ、仰るように、画像データの中のRGBの順については規定されていないようです。

masutani · August 26, 2018, 1:00am

私の勘違いがあり混乱させてしまい申し訳ありません。

ImageViewerの実装では、非圧縮の画像データはR,G,Bの順に並んでいることを想定しています。ですから、内部でOpenCVで扱うために、RGBからBGRへ変換しています。

一方、JPEGの場合の処理は、「JPEGデコード→RGBからBGRへ変換」と処理されています。同じく大原先生の作られたWebCameraコンポーネントでは「BGRからRBGへの変換→JPEGエンコード」と処理されています。しかし、服部様から指摘があったように、利用しているOpenCVのデコード関数はBGRの画像を出力する仕様ですので、「RGBからBGRへ変換」が入っているのは明らかに間違いです。

ということで、ChoreonoidのImg::TimedCameraImageの実装は何も間違っていません。

私にとって今一番悩んでいるのは、Img::TimedCameraImageに関係あるのかわかりませんが、rtshellでエラーになってしまうことです。