[c++] 3次元回転確認用プログラム [OpenCV]

つくったもの

IMUデータを確認するための表示ツールがなかったので作成した。
それにトラックバーをつけてちょっとだけ遊べるようにしたものです。ロール・ピッチ・ヨーの値（これはIMUデータを確認するときようの名残）、回転行列とクォータニオンもついでに確認できるようにしています。
赤->x軸、緑->y軸、青->z軸となっています。描画用でOpenCV、回転計算用でEigenを利用しています。

全体の流れ

軸用に3点の座標 点X(1, 0, 0), 点Y(0, 1, 0), 点Z(0, 0, 1)を用意します。これをトラックバーから取得したロール・ピッチ・ヨー角の情報から回転行列を作成して、点X - 点Zを回転させています。点の回転をした後に2次元に落としこんでOpenCVのline関数で描画しています。

トラックバーの作成とデータ取得

OpenCVでのトラックバーの作成部分となります。

axis_viewer.cpp

    // 窓の作成 （window_name = "axis"）
    cv::namedWindow(window_name, cv::WINDOW_AUTOSIZE);

    // トラックバーの名前
    std::string track_name1 = "r";
    std::string track_name2 = "p";
    std::string track_name3 = "y";

    // トラックバーからのデータ格納用変数
    int r_pos, p_pos, y_pos;
    r_pos = p_pos = y_pos = 0;

    // トラックバーの作成 360 -> トラックバーの最小値は0, 最大値は360
    cv::createTrackbar(track_name1, window_name, &r_pos, 360);
    cv::createTrackbar(track_name2, window_name, &p_pos, 360);
    cv::createTrackbar(track_name3, window_name, &y_pos, 360);

これだけでトラックバーが作成できるのはとても便利だと思います。
あまりつかっている人を見ないけどもっと使われてもいいと思います。

回転行列の計算

トラックバーで取得した数値はわかる通りに度数表記°なので弧度表記radに変換します。

axis_viewer.cpp

    #define DEG2RAD(a) ((a)/180.0 * M_PI)

    Eigen::Vector3d rpy;
    rpy << DEG2RAD((double)r_pos), DEG2RAD((double)p_pos), DEG2RAD((double)y_pos);

次に回転行列の取得となります。

axis_viewer.cpp

    // _rpy.x()にr_posの値
    // _rpy.y()にp_posの値
    // _rpy.z()にy_posの値
    void calcRot(Eigen::Vector3d &_rpy){
        m_rot = Eigen::AngleAxisd(_rpy.x(), Eigen::Vector3d::UnitX())
            * Eigen::AngleAxisd(_rpy.y(), Eigen::Vector3d::UnitY())
            * Eigen::AngleAxisd(_rpy.z(), Eigen::Vector3d::UnitZ());
    }

Eigenを使うとこれだけで回転行列を得ることができるのでおすすめです。

座標の回転

無事に回転行列を取得することができました。ここから座標の回転をしていきます。
座標の回転は

\boldsymbol{x'} = \boldsymbol{R} \times \boldsymbol{x}

で表すことができます。
$\boldsymbol{x}$は変換前の点$(x, y, z)$、$\boldsymbol{x'}$は変換後の点$(x', y', z')$、$\boldsymbol{R}$は回転行列を示しています。
実装すると次のような形になります。

axis_viewer.cpp

    // pts -> 変換後の座標格納
    std::vector<Eigen::Vector3d> pts;
    // m_base_pts -> (1, 0, 0), (0, 1, 0), (0, 0, 1)の座標が格納（軸描画用）
    for(auto itr = m_base_pts.begin(); itr < m_base_pts.end(); itr++){
        // 座標変換計算と格納
        pts.emplace_back(m_rot*(*itr));
    }

Eigenによって座標の回転の計算が1行で完結します。これがEigen離れができなくなる原因でもあります。
今回は回転する座標の数が3つなので一つずつ行っていますが、複数の点を回転するときは行列にして計算しています。

画像として書き込み

OpenCVの画像として出力するための準備を行っていきます。
cv::line関数で軸の書き込みを行います。
そのために必要となるパラメータは
・始点
・終点
・色
の３つです。
出力する画像サイズは480 x 480なので、画像の中心の(240, 240)に始点をおきます。
カメラの位置は固定なので、終点は回転した座標のx, yに適当な長さを与えてあげれば良いわけです。

axis_viewer.cpp

    // cvpts -> x軸、y軸、z軸の終点座標ベクトル
    std::vector<cv::Point> cvpts;
    // origin -> 始点
    // _p[n] -> 回転後の座標にスケールをかけたもの（今回は最大100px）
    cvpts.emplace_back(cv::Point(_p[0].x(), _p[0].y()) + origin);    
    cvpts.emplace_back(cv::Point(_p[1].x(), _p[1].y()) + origin);    
    cvpts.emplace_back(cv::Point(_p[2].x(), _p[2].y()) + origin);    

軸が重なったときにどっちが上なのかわからなくなるため、座標軸の描画の順番も考慮します。

axis_viewer.hpp

struct d_idx{
    double z;     // 回転した座標の奥行き情報
    int index;    // 軸番号
    int color;    // 色
    cv::Point px; // 画像上の終点座標
};

この構造体で管理を行っていきます。

axis_viewer.cpp

    std::vector<d_idx> zids;
    int n = 0;

    // 格納
    for(auto itr = pts.begin() + 1; itr < pts.end(); itr++, n++){
        d_idx zid;
        zid.index = n;
        zid.z = itr->z();
        zid.color = n;
        zid.px = pixels[n];
        zids.emplace_back(zid);
    }

    // 奥行き情報で並び替え
    for(n = 0; n < 3; n++){
        for(int m = n + 1; m < 3; m++){
            if(zids[n].z > zids[m].z){
                d_idx tmp = zids[m];
                zids[m] = zids[n];
                zids[n] = tmp;
            }
        }
    }

ここらへんは別に良いやり方がたくさんあると思うけど、とりあえずはこれで。。。
並び替えも終わったので、奥のものから描画していくだけです。

axis_viewer.cpp

    for(auto itr = zids.begin(); itr < zids.end(); itr++){
        cv::line(axis, m_origin, itr->px, getColor((int)itr->z, itr->color), 3);
    }

回転行列の情報とか、cv::putTextで出力すればいいだけです。

まとめ

OpenCVは描画するのが簡単で良い。
あまりこういうことをやってる人がいなかったので、（理論説明とかはいっぱいあるんだけど）参考になる人がいたら嬉しい。
視点変えとか複雑なことする場合はOpenCVじゃない方がいいと思います。