ココロオークションが最近の自分の流行
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
