만든 것

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은 좌표 회전 계산을 한 줄로 완료합니다. 이것이 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가 아닌 것이 좋다고 생각합니다.