就活終わったー
後は研究して卒業するだけや
前回の記事です
mozyanari.hatenablog.com
カメラ位置を計算することは出来ていましたが，自分が欲しいのはマーカの位置です
カメラ位置を計算できるということは，マーカ位置も取得できるはずなので検索しましたが全く記事が出てこなかったため色んな記事を参照しました
stackoverflow.com
www.learnopencv.com
www.learnopencv.com
最終的には回転行列からマーカの回転角度を取得するということで動きました
- プログラム
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); Aruco.drawAxis(cam_frame, camMatrix, distCoeffs, rvecs, tvecs, (float)0.1); Debug.Log("x=" + tvecs.get(0, 0)[0] + "y=" + tvecs.get(0, 0)[1] + "z=" + tvecs.get(0, 0)[2]); //Debug.Log("y=" + tvecs.get(0, 0)[1]); //Debug.Log("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]); //Debug.Log(R); Mat R = Rt.t(); Mat camera_position = Rt.t() * (-T); Debug.Log(camera_position); //回転角度の計算 float roll = 0; float pitch = 0; float yaw = 0; float a = Mathf.Sqrt((float)(R.get(0, 0)[0] * R.get(0, 0)[0] + R.get(1, 0)[0] * R.get(1, 0)[0])); if (a > 1e-6) { roll = Mathf.Atan2((float)R.get(2, 1)[0], (float)R.get(2, 2)[0]) * 180 / Mathf.PI; pitch = Mathf.Atan2((-(float)R.get(2, 0)[0]) , a) * 180 / Mathf.PI; yaw = Mathf.Atan2((float)R.get(1, 0)[0], (float)R.get(0, 0)[0]) * 180 / Mathf.PI; } else { roll = Mathf.Atan2(-(float)R.get(1, 2)[0], (float)R.get(1, 1)[0]) * 180 / Mathf.PI; pitch = Mathf.Atan2((-(float)R.get(2, 0)[0]) , a) * 180 / Mathf.PI; yaw = 0; } camera.transform.position = new Vector3((float)tvecs.get(0, 0)[0] * 10, -(float)tvecs.get(0, 0)[1] * 10, (float)tvecs.get(0, 0)[2] * 10); camera.transform.rotation = Quaternion.Euler(roll, pitch, yaw); } 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(); } }
試行錯誤の跡が残ってるのでこのままコピペしても動かないので適宜参照してください
- マーカ位置の計算
tvecとrvecを取得するまでは前回の記事と同じ流れです
camera.transform.position = new Vector3((float)tvecs.get(0, 0)[0] * 10, -(float)tvecs.get(0, 0)[1] * 10, (float)tvecs.get(0, 0)[2] * 10);
10倍してるのはデバックするときに動きが見やすいためにしています
単位はmです
前回と異なり，tvecsをそのまま使っています
- マーカの角度計算
float roll = 0; float pitch = 0; float yaw = 0; float a = Mathf.Sqrt((float)(R.get(0, 0)[0] * R.get(0, 0)[0] + R.get(1, 0)[0] * R.get(1, 0)[0])); if (a > 1e-6) { roll = Mathf.Atan2((float)R.get(2, 1)[0], (float)R.get(2, 2)[0]) * 180 / Mathf.PI; pitch = Mathf.Atan2((-(float)R.get(2, 0)[0]) , a) * 180 / Mathf.PI; yaw = Mathf.Atan2((float)R.get(1, 0)[0], (float)R.get(0, 0)[0]) * 180 / Mathf.PI; }else{ roll = Mathf.Atan2(-(float)R.get(1, 2)[0], (float)R.get(1, 1)[0]) * 180 / Mathf.PI; pitch = Mathf.Atan2((-(float)R.get(2, 0)[0]) , a) * 180 / Mathf.PI; yaw = 0; }
この部分が最も時間がかかりました
rvecsをロドリゲス変換して転置した後の行列の中身を参考にしたページをもとに計算するとなんかうまいこと動くようになりました
なので，なぜこれが上手く動くか理解していません
動けば勝ちなのでこれでいいかなと思っています
