へっぽこ元ロボコニスト

ロボコンに燃え尽きた自分が日々の出来事を書くだけのブログです

2019/6/4(estimatePoseSingleMarkersのtvecとrvecからカメラ姿勢を計算する)

ココロオークションが最近の自分の流行
www.youtube.com


Arucoを使ってマーカの位置を取得しようとしています
Unityでマーカ位置にオブジェクトを置きたいのでちゃんとした位置結果を計算しないといけません
ネットで検索しても,detectMarkers→drawDetectedMarkersで描画するだけや,detectMarkers→tvecの値から直接位置計算をやっていて正解にたどり着けませんでした

OpencvにestimatePoseSingleMarkersと同様の結果を出すsolvePnPという関数があり,これで検索すれば出てくるかなと思って検索して見つけました
stackoverflow.com
これによって自分が出来たのはマーカを原点としてカメラの位置・姿勢がどこにあるかということです.

  • 環境

Unity 2018.2.21f1
OpenCV for Unity v2.3.2

  • プログラム
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using OpenCVForUnity;
using UnityEngine.SceneManagement;

public class Opencv_test : MonoBehaviour {

    //カメラの番号
    private int camera_number = 0;

    private VideoCapture cam;
    public Mat cam_frame;

    private Texture2D cam_Texture;

    //Arucoの設定
    public ArUcoDictionary dictionaryId = ArUcoDictionary.DICT_4X4_50;
    private Dictionary ar_dict;

    //検出アルゴリズムの設定
    private DetectorParameters detect_param;

    //マーカの位置
    private Mat rvecs;
    private Mat tvecs;
    //送信するマーカ位置
    private Vector3 position;

    public GameObject camera;
    public GameObject marker;



    // Use this for initialization
    void Start () {
        cam = new VideoCapture(camera_number);
        //cam.Set(CaptureProperty.ConvertRgb, 3);
        cam.set(16, 3);
        cam_frame = new Mat();

        ar_dict = Aruco.getPredefinedDictionary((int)dictionaryId);
        detect_param = DetectorParameters.create();

        //plopIdはopenCVの2系時に名前で定義されていたが廃止された.3系以降は番号でこれを取得する
        cam_Texture = new Texture2D((int)cam.get(3), (int)cam.get(4), TextureFormat.RGB24, false);

        this.GetComponent<Renderer>().material.mainTexture = cam_Texture;

        tvecs = new Mat();
        rvecs = new Mat();
        position = new Vector3(0, 0, 0);
    }

    // Update is called once per frame
    void Update () {
        List<Mat> marker_corner = new List<Mat>(OpenCVForUnity.CvType.CV_8UC1);
        Mat marker_ids = new Mat();
        List<Mat> reject_points = new List<Mat>(OpenCVForUnity.CvType.CV_8UC1);

        cam.read(cam_frame);

        Aruco.detectMarkers(cam_frame, ar_dict, marker_corner, marker_ids, detect_param, reject_points);

        //Debug.Log(marker_ids.size());
        if (marker_ids.total() > 0)
        {
            //マーカの描画
            Aruco.drawDetectedMarkers(cam_frame, marker_corner, marker_ids, new Scalar(0, 255, 0));

            //キャリブレーションデータの設定
            Mat camMatrix = new Mat(3, 3, CvType.CV_64FC1);
            camMatrix.put(0, 0, 548.82998077504431);
            camMatrix.put(0, 1, 0);
            camMatrix.put(0, 2, 332.15854329853039);
            camMatrix.put(1, 0, 0);
            camMatrix.put(1, 1, 549.82809802204554);
            camMatrix.put(1, 2, 236.24689239842121);
            camMatrix.put(2, 0, 0);
            camMatrix.put(2, 1, 0);
            camMatrix.put(2, 2, 1.0f);

            MatOfDouble distCoeffs = new MatOfDouble(-0.088668381409835462, 0.4802348866900254, -0.0026481695882058297, 0.0019686217165377452, -0.82389338772252729);



            Aruco.estimatePoseSingleMarkers(marker_corner, (float)0.125, camMatrix, distCoeffs, rvecs, tvecs);


            
            Debug.Log("x=" + tvecs.get(0, 0)[0]+ "y=" + tvecs.get(0, 0)[1] + "z=" + tvecs.get(0, 0)[2]);
            
            
            Debug.Log("roll=" + (rvecs.get(0, 0)[0] * 180 / Mathf.PI) + "pitch=" + rvecs.get(0, 0)[1] * 180 / Mathf.PI + "yaw=" + rvecs.get(0, 0)[2] * 180 / Mathf.PI);

            

            marker.transform.position = new Vector3(0, 0, 0);
            Mat Rt = new Mat();

            OpenCVForUnity.Calib3d.Rodrigues(rvecs,Rt);
            

            Mat T = new Mat(3, 1, CvType.CV_64FC1);
            T.put(0, 0, tvecs.get(0, 0)[0]);
            T.put(1, 0, tvecs.get(0, 0)[1]);
            T.put(2, 0, tvecs.get(0, 0)[2]);

            Mat R = Rt.t();

            Mat camera_position = Rt.t() * (-T);

            Debug.Log(camera_position);

            camera.transform.position = new Vector3((float)camera_position.get(0,0)[0]*10, (float)camera_position.get(2, 0)[0]*10, (float)camera_position.get(1, 0)[0]*10);

            float roll = Mathf.Atan2((float)-R.get(2, 1)[0], (float)R.get(2, 2)[0]) * 180 / Mathf.PI;
            float pitch = Mathf.Asin((float)R.get(2, 0)[0]) * 180 / Mathf.PI;
            float yaw = Mathf.Atan2((float)-R.get(1, 0)[0], (float)R.get(0, 0)[0]) * 180 / Mathf.PI;

            camera.transform.rotation = Quaternion.Euler(roll, yaw, pitch);


        }

        Utils.matToTexture2D(cam_frame, cam_Texture);
    }

    public enum ArUcoDictionary
    {
        DICT_4X4_50 = Aruco.DICT_4X4_50,
        DICT_4X4_100 = Aruco.DICT_4X4_100,
        DICT_4X4_250 = Aruco.DICT_4X4_250,
        DICT_4X4_1000 = Aruco.DICT_4X4_1000,
        DICT_5X5_50 = Aruco.DICT_5X5_50,
        DICT_5X5_100 = Aruco.DICT_5X5_100,
        DICT_5X5_250 = Aruco.DICT_5X5_250,
        DICT_5X5_1000 = Aruco.DICT_5X5_1000,
        DICT_6X6_50 = Aruco.DICT_6X6_50,
        DICT_6X6_100 = Aruco.DICT_6X6_100,
        DICT_6X6_250 = Aruco.DICT_6X6_250,
        DICT_6X6_1000 = Aruco.DICT_6X6_1000,
        DICT_7X7_50 = Aruco.DICT_7X7_50,
        DICT_7X7_100 = Aruco.DICT_7X7_100,
        DICT_7X7_250 = Aruco.DICT_7X7_250,
        DICT_7X7_1000 = Aruco.DICT_7X7_1000,
        DICT_ARUCO_ORIGINAL = Aruco.DICT_ARUCO_ORIGINAL,
    }

    void OnDestroy()
    {
        if (cam_frame != null)
            cam_frame.Dispose();
    }


}
  • ARマーカの認識
Aruco.detectMarkers(cam_frame, ar_dict, marker_corner, marker_ids, detect_param, reject_points);

cam_frameがカメラ画像でこれを入力することで,マーカの位置のmarker_cornerを取得します

  • ARマーカの位置の取得
Aruco.estimatePoseSingleMarkers(marker_corner, (float)0.125, camMatrix, distCoeffs, rvecs, tvecs);

マーカ位置のmarker_cornerとカメラパラメータのcamMatrixとdistCoeffsを入力します
カメラパラメータは事前に取得しておきます
mozyanari.hatenablog.com
(float)0.125はARマーカの実際の大きさをはかって入力しています
こうすることで,ARマーカとの相対的な位置関係を表す,tvecとrvecを取得出来ます

  • rvecとtvecからカメラ位置を計算

冒頭の参考文献をそのまま使っています
ロドリゲス変換

OpenCVForUnity.Calib3d.Rodrigues(rvecs,Rt);

カメラ位置の計算

Mat camera_position = Rt.t() * (-T);

カメラの回転角度計算

float roll = Mathf.Atan2((float)-R.get(2, 1)[0], (float)R.get(2, 2)[0]) * 180 / Mathf.PI;
float pitch = Mathf.Asin((float)R.get(2, 0)[0]) * 180 / Mathf.PI;
float yaw = Mathf.Atan2((float)-R.get(1, 0)[0], (float)R.get(0, 0)[0]) * 180 / Mathf.PI;

Unityに適した形で入力

camera.transform.rotation = Quaternion.Euler(roll, yaw, pitch);


これによってマーカの位置を原点として,カメラの位置・姿勢を取得できると思います

追記
マーカ位置を計算するVerも書きました
mozyanari.hatenablog.com