1. はじめに

最近，OpenCVを使った実装をしていて座標変換の演算で落とし穴にハマったのですが，わりと素で「え，これってこう書いちゃダメなの？」みたいな感じでした．この罠にはまると，原因に気付くまでけっこう苦労する可能性があるというか，私は（問題のショボさに比して）めっちゃ苦労しました．
ということで，かなりショボいゆるふわネタなのですが，せっかくの機会なので紹介してみようと思います．

2. 問題設定

2.1. 3次元の座標変換

OpenCVで座標変換のコードを書いた経験のある方も多いと思います．その際，cv::Mat同士の演算では素直に書けると思いますが，座標をcv::Pointやcv::Vecなどで持っていた場合にどう書くか，ということが本稿の主題です．

とりあえず今回は3x3の回転行列$\mathbf{R}$と3次元座標$\mathbf{x}$，3次元ベクトル$\mathbf{t}$が与えられた状況で

$\mathbf{y} = \mathbf{R} ~ \mathbf{x} + \mathbf{t}$

の$\mathbf{y}$を計算する，という3次元の座標変換を扱うものとします．言うまでもなく，適当に読み替えることで2D座標でも同じような話になります．

2.2. cv::Matについて

いきなり脱線しますが，OpenCVで画像や行列を扱うクラスとして普通はcv::Matを使っていると思います．UMatとかoclMatとかのマニアックなクラスは知りません．さすがにIplImageとか言ってる人は既に絶滅していますよね？
ここで，私は実際にはcv::Matではなくcv::Mat1fやcv::Mat1dなどを好んで使っています．なぜかというと，例えばfloat型の3x3行列を設定する場合に，cv::Matを使うと

cv::Mat mat(3, 3, CV_32FC1);
mat.at<float>(0, 0) = 1.0;

のようなスタイルになりますが，配列確保はまだしも，要素アクセスの度に毎回"<float>"などと型を指定しなければならないのが面倒だからです．さらに言えば，要素アクセスで型指定をうっかり間違えて

cv::Mat mat(3, 3, CV_32FC1);
mat.at<double>(0, 0) = 1.0;

とかやってしまった場合にもそのままコンパイルが通ってしまいますし，実行時にもdouble型の値がメモリに書き込まれて（多くの場合）誤動作をしながらコードとしては動いてしまいます．一方，cv::Mat1fを使えば

cv::Mat1f mat(3, 3);
mat(0, 0) = 1.0;

となって，とてもスッキリしますし，前記のような誤動作のリスクも大幅に減ります．
double型の場合はcv::Mat1d，unsigned char型の場合はcv::Mat1b，intの場合はcv::Mat1i，shortの場合はcv::Mat1s，unsigned shortの場合はcv::Mat1wを使います．また，カラー画像（3チャンネルunsigned char）の場合はcv::Mat3bを使います．
もちろん，要素の型だけでなくサイズも固定して良い場合は，例えば3x3行列であればcv::Matx33fやcv::Matx33dを使うという手もあると思います．

ただし，これらは便利さの代償として柔軟性が失われます．cv::Matはチャンネル数や要素の型を変更することが可能ですが，例えばcv::Mat1fはfloatで１チャンネルの配列としてしか使えません．また，例えばcv::Matでは

cv::Mat mat(height, width, CV_8UC3);
mat.at<cv::Vec3b>(y, x) = cv::Vec3b(255,192,128);

のように画素(3チャンネル)単位でアクセスすることもできれば，

cv::Mat mat(height, width, CV_8UC3);
mat.at<uchar>(y, x*3+0) = 255;
mat.at<uchar>(y, x*3+1) = 192;
mat.at<uchar>(y, x*3+2) = 128;

のように画素のチャンネルそれぞれにアクセスすることもできます．一方，cv::Mat3bでは基本的に画素単位(つまりcv::Vec3bが１単位)でしかアクセスはできません．

なお，

cv::Mat mat(height, width, CV_8UC3);
uchar *ptr = mat.ptr<uchar>(y);
ptr[x*3+0] = 255;

のように，「俺は常にポインタを直接使って要素アクセスするぜ」という異端的な思想の人は，ここは気にせずスルーしてください．

3. 検証

では，本題に入ります．以下では具体的なコードで検証していきます．

3.1. cv::Matだけで書いたコード

まずはcv::Matだけで問題設定に従ったコードを書いてみましょう．使っているパラメータは適当に設定しただけで意味はありません．

#include <opencv2/opencv.hpp>

int main(){

    // 回転行列の生成
    cv::Mat1d R;
    cv::Rodrigues(cv::Vec3d(1.2,-2.6,0.5), R);

    // 並進ベクトルの生成
    cv::Mat1d t(3,1);
    t(0) = 0.7;
    t(1) = -0.9;
    t(2) = 0.2;

    // 射影元の座標
    cv::Mat1d x(3,1);
    x(0) = 1;
    x(1) = 2;
    x(2) = -3;

    std::cout << "R * x + t = " << std::endl << R * x + t << std::endl;

    return 0;
}

コンパイルして実行すると，結果は

R * x + t =
[-1.269288893651164;
0.8210574252565116;
2.875791956096662]

となりました．
以降では，この$\mathbf{x}$や$\mathbf{t}$をcv::Matではなくcv::Vecやcv::Pointで持っていた場合について検証してきます．

3.2. cv::Pointを使った場合

#include <opencv2/opencv.hpp>

int main(){

    // 回転行列の生成
    cv::Mat1d R;
    cv::Rodrigues(cv::Vec3d(1.2,-2.6,0.5), R);

    // 並進ベクトルの生成
    cv::Point3d t(0.7, -0.9, 0.2);

    // 射影元の座標
    cv::Point3d x(1, 2, -3);

    std::cout << "R * x + t = " << std::endl << R * x + t << std::endl;

    return 0;

}

このコードはコンパイルが通りません．cv::Matとcv::Pointの演算はできないからです．ではどうすれば良いか？cv::Pointからcv::Matに変換すれば良いのです．といっても，「3x1行列のcv::Matを作って，cv::Pointの要素を１つづつコピーする」なんていうコードをチマチマと書くわけではありませんよ?（すみません，わりと最近までそういうクソなコードを書いていました．）
以下のように，行列演算する時点でcv::Pointをcv::MatにキャストすればOKです．

std::cout << "R * x + t = " << std::endl << R * cv::Mat1d(x) + cv::Mat1d(t) << std::endl;

これをコンパイル，実行すれば先程と同じ結果が得られることが確認できるはずです．なお，上記コードでは私の宗教観に従ってcv::Mat1dへキャストしていますが，cv::Matへキャストしても（xの型に合わせて）doubleの行列になるので問題ありません．

さて，この計算結果をcv::Point3dに保存する場合はどうすれば良いでしょうか？結論としては，以下のようなコードになります．以下のように「全体をcv::Matでキャスト」「cv::Matからcv::Pointへキャスト」という手順を踏まないと，コンパイルエラーになります．

    cv::Point3d y = (cv::Point3d)cv::Mat1d(R * cv::Mat1d(x) + cv::Mat1d(t));
    std::cout << "R * x + t = " << y << std::endl;

書き忘れていましたが，今回のサンプルは全てcygwin上のg++で試しています．Visual Studioだと以下のコードでも大丈夫だったような気がしなくもないです（演算結果をcv::Mat1dではなくcv::Matでキャストしていることに注意）が，g++では以下のコードもエラーになりました．

    cv::Point3d y = cv::Mat(R * cv::Mat1d(x) + cv::Mat1d(t));

ま，とにかくこのようにすれば，めでたく「cv::Pointからcv::Pointへの座標変換」がわりとスッキリ書けるわけです．

3.3. cv::Vecを使った場合

cv::Vecもcv::Pointと同じで，そのまま演算しようとするとコンパイルエラーになります．なので，キャストするわけですが，以下のコードでコンパイルが通ります．

#include <opencv2/opencv.hpp>

int main(){

    // 回転行列の生成
    cv::Mat1d R;
    cv::Rodrigues(cv::Vec3d(1.2,-2.6,0.5), R);

    // 並進ベクトルの生成
    cv::Vec3d t(0.7, -0.9, 0.2);

    // 射影元の座標
    cv::Vec3d x(1, 2, -3);

    // 射影後の座標
    cv::Vec3d y = (cv::Vec3d)cv::Mat(R * cv::Mat(x) + t);

    std::cout << "R * x + t = " << y << std::endl;

    return 0;

}

念のため，結果を確認してみましょう．

R * x + t = [-1.26929, 2.42106, 3.37579]

ファッ!? 結果間違っとるやんけ!!! ...そう，これが私のハマった落とし穴です．

結果を見比べると，１つ目の要素だけは正しいようです．3x1の行列"mat"と，3次元のベクトル"vec"があった場合に，"mat + vec"というコードを書いたら，常識的には我々の期待としては

result(0) = mat(0) + vec[0]
result(1) = mat(1) + vec[1]
result(2) = mat(2) + vec[2]

なわけですが，実際には

result(0) = mat(0) + vec[0]
result(1) = mat(1) + vec[0]
result(2) = mat(2) + vec[0]

となってしまうようです．では，どうすれば直るかというと，ちゃんとcv::Vecをcv::Matに加算する前に忘れずにキャストしておけば大丈夫です．

cv::Vec3d y = (cv::Vec3d)cv::Mat1d(R * cv::Mat1d(x) + cv::Mat1d(t));

...はい，これだけの一発ネタでした．
私の場合はこれをやってしまったのがけっこう奥深くの関数だったのと，まさかこんな挙動が存在するとは思っていなかったので，この問題を発見するのに半日を費やしました．

4. おまけ

「回転行列はdoubleの配列なんだけど，座標はfloatで持っている」みたいなことってありませんか？私はよくあります．そういう時の計算方法についても，要素に直接アクセスしてキャストするのではなく，スッキリと解決したいですよね．

ダメな例1

cv::Mat1d R(3,3);
cv::Vec3f x;
　中略
cv::Mat y = R * cv::Mat1d(x);

コンパイル時に，cv::Vec3fからcv::Mat1dへのキャストで「型が合わん」とエラーになります．

ダメな例2

cv::Mat1d R(3,3);
cv::Vec3f x;
　中略
cv::Mat y = R * cv::Mat1f(x);

コンパイルは通りますが，実行時にcv::Mat1dとcv::Mat1fの掛け算で「型が合わん」とエラーになります．

OKな例1

cv::Mat1d R(3,3);
cv::Vec3f x, t;
　中略
cv::Vec3f y = cv::Mat1d(R * cv::Mat1d((cv::Vec3d)x) + cv::Mat1d((cv::Vec3d)t));

cv::Vec3fからcv::Vec3dへはキャストが可能でした．また，cv::Matからcv::Vecへ代入(コピー)するとdoubleからfloatへ自動的にキャストしてくれるようです．

OKな例2

cv::Mat1d R(3,3);
cv::Point3f x, t;
　中略
cv::Point3f y = (cv::Point3f)cv::Mat1d(R * cv::Mat1d(x) + cv::Mat1d(t));

cv::Pointからcv::Mat1dへのキャストではfloatからdoubleへ自動で変換してくれるようです．一方，cv::Matからcv::Pointへの代入（コピー）は明示的なキャストが必要でした．しかし，キャストさえすれば，doubleからfloatへ自動変換してくれるようです．

5. 最後に

本稿は，配列要素を直接触らずにコードをすっきり書くことだけを主眼にして書いたものです．このようにキャストを多用した場合の実行速度への影響は何も調べていませんし，そもそも実は筆者はC++がよくわかっていませ(ry

ポインタ原理主義思想に染まっている方々や，あるいは「OpenCVの行列演算なんか使わず，行列演算は自分で書くもんね」というような歪んだ価値観の方々，はたまた「ベクトルだろうが座標だろうが全部cv::Matで保持してるぜ」という過度にストイックな方々などは，本稿のことは気にせずスルーしていただければと思います．