2019-04-04

MATLABでnumpyで保存したcsvファイルを開く

MATLAB python

結論
numpy arrayの保存
- こんなデータになる
MATLABでcsvを開く
- csvread：’，’をdelimiterにしないとおかしくなる．
- readtable：有能
Reference

結論

書きはじめの時は，numpyで保存したcsvがmatlabに変な感じで認識されるのを不満に思っていたのですが，途中から自分の過ちに気づきました．

要点は以下の２つです．

csvread()は区切りがカンマでなくては行けない
そうでない場合はreadtable()を使う

はい．帰ってよし．

numpy arrayの保存

自分はもっぱらsavetxtを用います．

numpy.savetxt("K.csv", K, delimiter =',',fmt="%0.14f")

loadする時，そのままnumpy arrayとしてloadでき，他のソフトからも開きやすいからです．（Excelはダメ）

こんなデータになる

~~糞長くなるので人間にわかりやすいデータではないですが，一応こんな感じにsaveされます．~~ 嘘です．delimiter =','という引数を加えないと区切りがスペースになってしまいます．其の結果が以下の通りです．

1.554141360341085672e+09 8.172370370370369983e+02 4.427456790123457040e+02 -1.855009145168301770e-03 -4.460917310385187178e-03
1.554141360408607960e+09 8.172024691358025166e+02 4.427777777777777715e+02 -1.670469061790032239e-03 -4.653048558273605202e-03
1.554141360473935604e+09 8.172383292383292428e+02 4.427518427518427302e+02 -1.816789199264026364e-03 -4.465982169869531920e-03
1.554141360534764767e+09 8.171901234567901611e+02 4.427753086419753004e+02 -1.689939994465252445e-03 -4.698456650358831185e-03

MATLABでcsvを開く

最も直感的なのはドラッグアンドドロップしてuiopenとして開くことです．直感的だけど多くのcsvを読む用途には適しません．

csvread：’，’をdelimiterにしないとおかしくなる．

一応，csvread(filename)とする手法がありますが，先ほどのcsvを読むと4行5列のarrayのはずなのに4行1列として表示されます．

ans =

   1.0e+09 *

    1.5541
    0.0000
    0.0000
   -0.0000

カンマで区切ってないからですね．

readtable：有能

readtableなら区切り文字がほかの文字でもうまく検出することができます．一方で返り値はtableなのでtable2array()関数で変換する必要があります．

table2array(readtable(flename))

という感じです．

Reference

みんなもちゃんとリファレンス読みましょう．

How can I import data from .csv file with numeric values and texts (with column headers) into MATLAB Workspace? - MATLAB Answers - MATLAB Central

テキストファイルのインポート - MATLAB & Simulink - MathWorks 日本

2019-04-03

ROSBagから画像を抽出する（image_viewを使うべきではない気がする）

Python ROS

問題設定
image_viewを用いて画像をエクスポート
PythonのrosbagAPIを叩く方法（推奨）
- rosbagファイルから特定のメッセージを抽出
- おまけ：比較用ビデオ生成コード

問題設定

実験などのデータをrosbag形式で保存して静的な環境で検証をしたいということが多々有るかと思います．

image_viewを用いて画像をエクスポート

image_viewのexport imageを使う（非推奨）手法があります．

公式？のDocumentを元に進めればOKです．

以下のexport.launchと言う名前のlaunchfileを作って，実行せよとのことです．

<launch>
  <node pkg="rosbag" type="play" name="rosbag" args="-d 2 $(find image_view)/test.bag"/>
  <node name="extract" pkg="image_view" type="extract_images" respawn="false" output="screen" cwd="ROS_HOME">
    <remap from="image" to="/camera/image_raw"/>
  </node>
</launch>

$(find image_view)/test.bagのところを対象のbagファイルに変更すればOKです．

roslaunch export.launch

いい点

launchファイルを叩くだけで簡単にできる
多分変数をいじればもっと細かい指定もできそう．

悪い点

timestampの情報を拾うことができない（多分）
PCのスペックなどにより，拾い落としが起きる場合がある

後の手法を用いた方が余すことなくデータを取ることが出きます．

やっていることのメモ

rosbagのplayの後に-d 2という引数がついていますが rosbagのrefに拠ると，2秒開始するのを待つということのようです．（ノードに接続するため）

PythonのrosbagAPIを叩く方法（推奨）

先ほどのは本番環境を模擬して試験する文には良いのですが，dataのConnection Lossが起きうるので記録したデータを解析したいという目的には不向きだと思います．

rosbagファイルから特定のメッセージを抽出

rosbagをインポートします．今回はたまたま画像の解析をしたいので必要なパッケージは以下のようになります．

import rosbag
import cv2
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image

以下のようにBagモジュールのread_messages()というメソッドを用いてトピック名，メッセージ本体，タイムスタンプを入手できます．

timestamps = [];
images = [];
for topic, msg, t in  rosbag.Bag('HOGEFUGA.bag').read_messages():
    if topic == '/zed/left/image_raw_color':
        timestamps.append(t.to_sec())
        images.append( CvBridge().imgmsg_to_cv2(msg, "8UC3") )

timestampはros.Timeという形式なので to_sec()で変換する
メッセージの変換などは通常のrospyファイルと同様

おまけ：比較用ビデオ生成コード

ビデオにすると先ほどの手法と比較してどの画像が落ちていそうか見ることができます．

## Create Video for Evaluation
size = (images[0].shape[1],images[0].shape[0])
out = cv2.VideoWriter('bagvideo.avi',cv2.VideoWriter_fourcc(*'DIVX'), 15, size)
for i in range(len(images)):
    out.write(images[i])
out.release()

2019-03-20

Python Gmail API を用いてメールの締切を自動でリストアップする①

python 自動化

あらまし
- 目指したい理想像
Gmail APIを用いたメール抽出
提案アルゴリズム
今後やりたいこと（誰かにやってほしいこと）

あらまし

最近謎にものづくりのモチベがあります。昨日メール見ていて締切多すぎぃ！ってなったのでメールから自動で締切をリストアップしてくれる手法がなんかないかなと思ってGmail APIのドキュメントとにらめっこしてきました。

目指したい理想像

Gmail に届いているメールから「締切」に関係するワードを抽出
同メールから「日付」情報を抽出

Gmail APIを用いたメール抽出

まずはQuickStartのガイドから入っていきます。

developers.google.com

ここではまず，Googleの情報にアクセスする認証キー的なものをダウンロードできます。キーの仕様についてはこっちを参照すると良いでしょう。

APIの立ち上げ

上の手順でquickstart.pyというサンプルコードを実行すると思います。実質的なサービスの立ち上げは，以下の箇所にあるようです。

service = build('gmail', 'v1', credentials=creds)

ここでcredentialsというのが初回の登録でDLしたcredential.jsonというAPIの認証キーです。 build関数の仕様に依るとgmailのサービスを'v1'のバージョンで起動するということみたいですね。

Queryとマッチングするメッセージを検索

メールの検索ですがAPIがきちんと準備されています。

developers.google.com

必要なものはだいたい以下の通りです。

先程立ち上げたservice
ユーザID ：メールアドレス or 'me'（自分自身のこと）
検索クエリ：検索文字列。ここでは「締切」などとする
maxResults：検索上限。時間を気にするなら入れておくといい。

messagesに帰ってくるのはメール情報のidの配列です。

response = service.users().messages().list(userId=user_id,
                                           q=query,maxResults=maxlistnum).execute()
messages = []
if 'messages' in response:
  messages.extend(response['messages'])

サンプルコードでは nextPageTokenなるものを使ってますが，これは構造体や配列を辿る時の現在地を表すポインタ的なやつです。

idからメッセージの本文などを抽出

先程の検索ではメッセージのidを入手したのでget関数を用いてメッセージの中身を取得します。

developers.google.com

ここのサンプルコードの困った点はマルチバイト文字が含まれているときにエンコーディングの問題が発生するということです。

APIの記述的にはMIMEという形式の方を推していそうですが，メールに依って文字化けをしたりしなかったりして半日では解決できなかったので今回は採用を見送りました。

それで代わりに書いた関数がこれ。（クラスの形式になってるけど直すの面倒なので各自解釈してください）

    def GetMessageSubjectAndBody(self, msg_id):
        """Get a Message and use it to create a MIME Message.

        Args:

            msg_id: The ID of the Message required.

        Returns:
            A MIME Message, consisting of data from Message.
        """
        try:
            message = self.service.users().messages().get(userId=self.user_id, id=msg_id,
                                                    ).execute()
            body = ""
            subject = ""

            if message['payload']['body']['size']:
                msg_str = base64.urlsafe_b64decode(message['payload']['body']['data'])
                body = msg_str.decode('utf-8')

            if 'headers' in message['payload']:
                for header in message['payload']['headers']:
                    if header['name'] == 'Subject':
                        subject = header['value']

            return subject, body
        except errors.HttpError as error:
            print('An error occurred: %s' % error)

基本的にはmessage['payload']['body']['data']の中に本文がありますが，稀にこのデータが空の場合があります。今の所そういうのは真面目なメールではなかったので例外処理をして回避しています。

また，同様にmessage['payload']['header']の中に件名を保管している箇所があるのでそこも掘り起こしています。気になる方は一度Jupyterなどで中身を見てみると良いでしょう。

提案アルゴリズム

上記の手段でメールの本文を検索して見ることができるようになりました。これをどのように使っていくかについて書いていきます。

締切に関する語句の検索（メール検索時）

締切と言っても様々な表記があります。その全てを検索クエリに含める必要があるのですが，その時の書き方は以下のようにORでつなぐことで実現します。

"締め切り OR 〆切 OR 締切 OR 締切り OR deadline OR DEADLINE"

もちろん，普段gmailの検索に用いるクエリも使用可能で，ここにあるように，重要なメールであれば ”AND is:important”とかつけて検索することもできます。

締切の日時の検索（メール内検索）

メール本文を抽出した後，締切に関する日時を検索します。日時というのは意外とメールの中で自然に使われているので"締切"などのワードの近くを検索したいです。

従って，

締切に関連する語句の付近の文章抽出（行単位or文字数で）
抽出した文章から日付の抽出

という手順を取りました。

日付の抽出には非常によく整備されたツールがあるのですが，残念ながら日本語対応しておらず，２バイト文字に対して誤作動的なのを起こすので今回は採用をやめました。もし使うなら２バイト文字があるかどうか判別してから用いればよいかと思います。海外の方と多く連絡するのであれば導入も考えます。（今のところはそんなにない） github.com

代わりに以下の正規表現を用意しreモジュールを用いて検索をかけています。（正規表現初めて使ったので深く理解していない）

['(\d{1,2})/(\d{1,2})', '(\d{1,2})月(\d{1,2})日']

この表現を用いることで「3/4」や「７月４日」といった表現をキャッチできます。半角全角にかかわらずキャッチできるので偉い。

まとめてデータ化

以上の手順で，締切に関する語句を含んだメールから締切の日時と件名を抽出できました。

これをまとめてデータ化してGoogleカレンダーにぶち込むなどしたいですが，ちょっと息切れしたので辞書型に詰めてひとまず完成とします。

ちょっと見せるとしうかつだらけですね。なおIDは加工しました。検索できないよ。見学会系はちょっとたくさん締切を発行してきますね。他の用途にも使えそうなので今回遊んでみてまぁまぁよかったんじゃないかなと思ってます。

          {'deadlines': [[3, 18]],
           'message_id': '1697f3bcab',
           'subject': '【３月１８日(月）午前９時締切】ソニー・インタラクティブエンタテインメント\u3000'
                      '技術系・事務系エントリーシート受付中'},
          {'deadlines': [[3, 8], [4, 3], [3, 8], [5, 13]],
           'message_id': '1695b47b1',
           'subject': '【日本精工（株）】ログインのお願い：【総合職】応募受付開始しました。'},
          {'deadlines': [[3, 1]],
           'message_id': '16933597b7',
           'subject': '【重要なお知らせ】3月1日0時より『まとめてプレエントリー』機能をご利用いただけます'},
          {'deadlines': [[3, 1]],
           'message_id': '16923774f',
           'subject': '【重要】2/27（水）～2/28（木）プレエントリー受付開始準備による一部機能停止のご案内'},
          {'deadlines': [[1, 31]],
           'message_id': '168944a8b6',
           'subject': ' 【特別号】有効期限が近づいているマイルやe JALポイントはございませんか？ / 【先得】 '
                      'JMB会員先行予約サービス 受付間もなく開始！'},
          {'deadlines': [[1, 31]],
           'message_id': '16886a4406',
           'subject': '【UTAS】(周知)【1/31まで deadline Jan '
                      '31】情報セキュリティ教育について/e-learning(Information Security '
                      'Education)'},
          {'deadlines': [[1, 23]],
           'message_id': '1686095a6',
           'subject': '【ご好評につき2期募集開催！】マーケティング、商品企画などコースに特化した短期インターンプログラム"Business '
                      'Master Program"'},
          {'deadlines': [[1, 31]],
           'message_id': '1683af6025',
           'subject': '【2019年1月31日締切】 JAL旅行積立 ボーナスマイルキャンペーン実施中！'},
          {'deadlines': [[1, 8], [1, 8]],
           'message_id': '16827b89',
           'subject': '【締切1/8(火)10:00】（事務系・技術系）ソニー インターンシッププログラム エントリー受付中'},

今後やりたいこと（誰かにやってほしいこと）

今後の課題は，

メールのid，件名，抽出した締切を含んだ適切なデータ構造
件名や締切で検索をかけることができ，APIを叩いて本文全文を参照できるようにする
Google Calendarなどに見やすく一覧でマッピングする
重要な締切かどうかまで判別してくれるとめちゃ助かる

といったところでしょうか。コード公開したら誰か引き継いでくれたりしないだろうか。

2019-03-19

Google CalendarとGoogle Keepを連携してPCで賢く予定管理したい

雑記メモ

まえがき
Google Keep と Google Calendar
- Google KeepのCalendarリマインダ機能
- 追記スペース
PC版をデスクトップアプリ風に使う
- １．ショートカットの作成
- ２．タスクバーのアイコンがChromeのものになる問題の解決
参考になりそうな文献達

まえがき

古来より人間は道具を発明し，自らの能力の延長として使ってきました。道具は能力を拡張するだけではなく補完的な意味合いも強くあり，例えば「眼鏡」があることにより近眼という致命的な能力欠如を持った個体でも社会においてそれほど不自由せずに生活を営むことができるようになりました。

眼鏡バンザイ。

f:id:ossyaritoori:20190319050922p:plain — 我々の救世主

ということは現代の情報社会の技術を使えば 「私生活がちゃんとしていない系の人」も「健常な人々」と同等な生活を営めるはずなのです。

締切や予定の把握・管理が苦手でもなんかうまくやる方法あるでしょ！助けてGoogle先生！ということです。

f:id:ossyaritoori:20190319051117p:plain — 我らが大先生（https://www.google.com/doodles/doraemon-2009?hl=ja より引用）

Google Keep と Google Calendar

今回の秘密道具はこちら。

Google Keep：万能メモ帳
Google Calendar：万能カレンダー

特に説明することはないですがGoogle Calendarは是非スマホにアプリを入れて使ってください。他の人との予定共有とかめっちゃ捗ります。というかGoogleﾎﾆｬﾗﾗは情報共有において最強と勝手に思っております。Google Photoをみんな使おう。

Google KeepのCalendarリマインダ機能

Google Keep は普通に使えばただのメモ帳です。しかし，メモをリマインダとして登録すればGoogle Calendarにも表示させることが可能です。 Calendarからは時刻などの情報を編集可能なようです。

詳しくは以下のサイトをば。 loumo.jp

健常者の人々には当たり前かもしれませんが，予定などのちょっとしたメモ書きというのは一般的にすぐにどっかいきがちです。 PCとスマホを持っている場合はその都度適当なファイルに書いて後々ファイル検索をかけて探すなんてこともよくあるわけです。

そういう片付けできない系ダメな奴がダメな理由は行動に一貫性・ルールがないからです。メモはGoogle Keepだけを使う。少なくとも何か情報を紛失するリスクは減ります。

追記スペース

実はこの記事書いている段階ではあんまり活用法を絞れていません。 ~~思いつきで書いているので。いざ変革。~~

便利な連携・使い方等見つけたら追記します。

PC版をデスクトップアプリ風に使う

とまぁGoogle系のサービスは便利で重宝するのですが，PC版はいちいちサイトを開く必要があります。

私はタブを片付けられないのでどうしても予定などは別窓で見たいという欲求が有り，デスクトップアプリ作ってほしーなーとは思ってます。

１．ショートカットの作成

以下の記事の手順でデスクトップ上にショートカットを作成することができます。

loumo.jp

また，その後にタスクバー上で該当windowを右クリックし，「タスクバーにピン止め」をすることで速攻で起動できます。いいですね。

２．タスクバーのアイコンがChromeのものになる問題の解決

タスクバーにピンどめする時，タスクバーのアイコンがChromeのものになってしまうというバグ？を経験しました。

f:id:ossyaritoori:20190319053315p:plain — 真ん中がGoogle Keep，右側がCalendar。わかんねぇよ

こういうときは一度タスクバーからピンどめを外して，ショートカットキーを右クリックして「タスクバーにピンどめ」を選択しなおします。

f:id:ossyaritoori:20190319054522p:plain — 正しい姿。わかりやすい～。

ここまで来ることでカレンダーやメモをアプリ感覚で起動することができて非常に快適になります。

参考になりそうな文献達

拡張機能はなかなか便利そうです。いちいちブックマークするよりも探しやすいと思います。（検索できるので）

blog.thetheorier.com

tono-tax.com

2019-03-10

ホモグラフィー行列を用いた鳥瞰変換（Bird's-eye view）

鳥瞰変換とは
鳥瞰変換とホモグラフィー変換
実践編

鳥瞰変換とは

一言で言うと，ある視点からとったカメラの画像を別の視点で撮ったように見せかける手法のことです。

他サイト様の説明がわかりやすいのでこちらに載せます。 daily-tech.hatenablog.com

より一般的には「斜めから撮った画像を真上から撮った画像のように写す」という場面で使われます。

引用：Tanaka, S., Yamada, K., Ito, T. and Ohkawa, T.: Vehicle Detection Based on Perspective Transformation Using Rear-View Camera, International Journal of Vehicular Technology, Volume 2011, Article ID 279739 (2011).

鳥瞰変換とホモグラフィー変換

カメラの撮像の式

グローバル座標系にて座標 $(X,Y,Z)$ にある点をカメラの位置①から撮像して画像上の点 $(x_1,y_1)$ に写っているとします。今，仮想的なカメラの回転 $R_{12}$ を施した系②でこれが $(x_2,y_2)$ に移るとします。（この系②が上から見下ろす系になっていればいいわけですね。）

それぞれの場合でのカメラの撮像の式は以下のようになります。（Texで打ったので画像で失礼）

f:id:ossyaritoori:20190310153102p:plain — (R,t)は系①でのカメラ姿勢，太字のKはカメラ行列

ホモグラフィー変換

一方，画像変換で用いられるホモグラフィー変換はホモグラフィー行列 $G$ を用いて次のような形で表せます。

f:id:ossyaritoori:20190310153504p:plain — λはスケーリング定数。

カメラの回転とホモグラフィー変換の関係性

上のニ式を見比べてみると，回転行列 $R_{12}$ と $G$ との間には以下のような直接対応関係があることがわかります。当然ですが，系①と②の間に純粋な回転以外の並進成分などがあった場合はこんな簡単な関係にはなりません。

f:id:ossyaritoori:20190310153948p:plain — カメラ回転＝ホモグラフィー行列に変換可能！

この関係を用いることでカメラを擬似的に真下向きに回したときの画像を生成することができるというわけです。

おまけ：手ぶれ補正

先程の式は裏を返せば，微小な並進成分を無視すれば画像内のホモグラフィー変換からカメラの回転を補正することができるということを表しています。この手法を用いて手ぶれ補正をしたのが以下の記事だったりするわけです。

ossyaritoori.hatenablog.com

実践編

例のごとくOpenCVを使います。

下準備

先程の式を用いて鳥瞰変換を行う際には以下のパラメータが必要になります。

カメラ行列K
系①におけるカメラ姿勢（R,t）（→ 実はRだけでいい）

これは一般にキャリブレーションと言われる作業です。

Ubuntu-ROS環境が整っている方にとっては造作もないと思いますが，Windowsなどでやる場合は下記のサイト等でやるのが良いかと思います。 ossyaritoori.hatenablog.com

また，カメラ姿勢（R,t）を測るのは結構大変で，ARマーカ等を用いるのが最も楽かなぁという気がします。プログラムは適当に探してもらうとして，この辺りの記事なんかが説明が簡素でわかりやすいかも？

プログラムの流れ

カメラの姿勢Rの元となるrvecから座標系①における回転 ${}^gR_1$ を計算します。 ossyaritoori.hatenablog.com

また，変換したい座標系②での回転 ${}^gR_2$ をcv2.Rodriguesから生成しておきます。

この時，先程の $R_{12}={}^gR_2{}^gR_1^\top$ と計算できます。その後，Kをかけて作ったホモグラフィー行列を元にcv2.warpPerspectiveに投げます。以上です。

略記プログラム

コードメモですがこのままでは動きませんよ～

# calculate bird view homography
import math
theta_below = np.array([0, math.pi, 0])
Rc_, _ =  cv2.Rodrigues(theta_below)#変換先の回転
Rc,_ = cv2.Rodrigues(vm.rvecs)#オリジナル関数有


Homo = np.linalg.multi_dot([K,Rc_.T,Rc.T,np.linalg.inv(K)])#ホモグラフィー行列

nHomo = Homo/Homo[2,2]# 正規化


minx = -20000
miny = 0

# 移動量を調整しないと画面外に出ていってしまう…
nHomo[0,2] -= minx 
nHomo[1,2] -= miny 

# 2000pix四方の画面を用意しないとどっか行っちゃうので…
out=cv2.warpPerspective(img,nHomo,(2000,2000))

f:id:ossyaritoori:20190310160628p:plain — 元の画像

f:id:ossyaritoori:20190310160653p:plain — 変換後の画像（Z軸も回すべきかも）

2019-03-02

OpenCVで使われる座標系の作法メモ

Opencv Python

OpenCVの座標系
- 前提of前提
- OpenCVで使われる回転の作法
OpenCVでの座標変換計算の数式
- rvecやtvecの扱い
- 検証用投影プログラム

OpenCVの座標系

座標系というやつはどう定義されているかが非常に重要です。従って参照にすべきはQiitaでもなく，このブログでもなく公式APIリファレンスです。

これに書いてあることが全てです。でもなかなか探しづらいのでここにメモを書きます。

前提of前提

超前提知識ですが，OpenCVでは以下の前提があります。

右手系
カメラの座標系において，左上が原点，横方向がX（右が正），画像の縦方向がY（下が正），奥行き方向がZ（奥が正）

OpenCVで使われる回転の作法

回転の記述法でメジャーどころと言えば以下の3つが挙げられます。

オイラー法によるDCM（Direct Cosine Matrix）
ロドリゲスの回転公式による3変数記述
クォータニオン

OpenCVではちょくちょく回転を表す3×1のベクトルを算出しますが，これは全てロドリゲスの回転公式に基づいています。

公式のcv2.Rodrigues()のレファレンスを覗くと

f:id:ossyaritoori:20190302003143p:plain

とあります。

日本語でいうと，「ベクトルのノルムが回転角，正規化されたベクトルが回転軸」を表します。

X軸[1,0,0]まわりに45度回転させたい場合は以下のように書けばいいわけです。

import cv2
import numy as np

R, Jacob = cv2.Rodrigues(np.array([math.pi/4,0,0]))
print(R)
#R = array([[ 1.        ,  0.        ,  0.        ],
#        [ 0.        ,  0.70710678, -0.70710678],
#        [ 0.        ,  0.70710678,  0.70710678]])

なお，cv2.Rodriguesは返り値を2つ返すので注意。

従って以下のようなオイラー角のDCM変換を手で書いている人はOpenCV上では使うのをやめた方がいいです。ぱっと書きやすいので私は好きなんですが…

# Calculates Rotation Matrix given euler angles.
def eulerAnglesToRotationMatrix(theta) :
     
    R_x = np.array([[1,         0,                  0                   ],
                    [0,         math.cos(theta[0]), -math.sin(theta[0]) ],
                    [0,         math.sin(theta[0]), math.cos(theta[0])  ]
                    ])
         
         
                     
    R_y = np.array([[math.cos(theta[1]),    0,      math.sin(theta[1])  ],
                    [0,                     1,      0                   ],
                    [-math.sin(theta[1]),   0,      math.cos(theta[1])  ]
                    ])
                 
    R_z = np.array([[math.cos(theta[2]),    -math.sin(theta[2]),    0],
                    [math.sin(theta[2]),    math.cos(theta[2]),     0],
                    [0,                     0,                      1]
                    ])
                     
                     
    R = np.dot(R_z, np.dot( R_y, R_x ))
 
    return R

# Checks if a matrix is a valid rotation matrix.
def isRotationMatrix(R) :
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype = R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6
 
 
# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
def rotationMatrixToEulerAngles(R) :
 
    assert(isRotationMatrix(R))
     
    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
     
    singular = sy < 1e-6
 
    if  not singular :
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], sy)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0
 
    return np.array([x, y, z])

OpenCVでの座標変換計算の数式

前節でOpenCVがどのような形で座標を扱うことを想定しているかがわかりました。次はそれがどのように使われることを想定されているかです。

例えばOpenCVではカメラの位置を図るような関数がいくつか存在します。既知の3次元点と投影された画像の点との対応からカメラの姿勢を計算するPNP問題を解く場合が特にそうです。

心当たりがあるのは

SolvePNP系統
arucoモジュールのEsitmatePose系

でしょうか。

rvecやtvecの扱い

これらでは得られたカメラの位置として返り値にrvecsとtvecsという3×1のベクトルを返してきます。気になるのはこれらは果たしてどの座標系に属しているのかということです。（都合等をよく考えたらカメラ座標系にきまっているんですが）

以前の記事でもカメラ変位をカメラtoグローバルなのかグローバルtoカメラなのかで扱いが変わることを書いたと思います。 ossyaritoori.hatenablog.com

結論から言うとrvecsやtvecsはカメラのLocal座標系から見た値で表されています。従って，

OpenCVで測ったカメラ姿勢 $R_c=Rodrigues(rvec_c)^\top$ から測った回転物の姿勢 $R_o=Rodrigues(rvec_o)^\top$ はグローバル座標系では

$^g R_{o} = R_c R_o$

と計算できるわけです。

検証用投影プログラム

自分はちょっと疑い深いので実際に計算して確かめました。

三次元の点（グローバル座標）をカメラの視点に投影するcv2.projectPoints()という関数があります。 imgpts, jac = cv2.projectPoints(3Dpoints, rvecs, tvecs, K, dist)のように使えば3次元点を2次元に投影することができます。

f:id:ossyaritoori:20190302005627p:plain — 親の顔よりみた式

この中身をDistortionを含めないで書くならこういう風になります。

tc=tvecs
Rc,_=cv2.Rodrigues(rvecs)
def camProjection(points,Rc,tc,K=np.eye(3),campose_is_global=0):
    '''
    Input: CameraPose, Camera Matrix, Object Points in Global Coordinate
    Output: Projected Points
    '''
    h,w = points.shape
    if h != 3:
        print("Input do not match points style!")
    if campose_is_global:#assume Rc,tc is from global frame
        Proj = K.dot(np.concatenate([Rc.T, -np.dot(Rc.T,tc)], axis=1))
    else:#assume Rc,tc is from camera frame( opencv default)
        Proj = K.dot(np.concatenate([Rc, tc], axis=1))        
    pts = np.vstack((points,np.ones((1,w))))
    converted_pts= Proj.dot(pts)
    return Projection(converted_pts)

def Projection(points):
    h,w = points.shape
    if h != 3:
        print("Input do not match points style!")
    Dep = points[2,:]
    Depth = np.tile(Dep,(2,1))
    return points[0:2,:]/Depth