Unity で ArUco 使うヤツ

ArPlane.cs

using UnityEngine;
using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.Threading;

public class ArPlane : MonoBehaviour
{
	[DllImport("opencv_sample")]
	private static extern IntPtr aruco_initialize(int width, int height, float markerSize, string cameraParamsFilePath);
	[DllImport("opencv_sample")]
	private static extern void aruco_finalize(IntPtr instance);
	[DllImport("opencv_sample")]
	private static extern void aruco_set_image(IntPtr instance, IntPtr src);
	[DllImport("opencv_sample")]
	private static extern void aruco_get_image(IntPtr instance, IntPtr dest);
	[DllImport("opencv_sample")]
	private static extern int aruco_detect(IntPtr instance);
	[DllImport("opencv_sample")]
	private static extern IntPtr aruco_get_markers(IntPtr instance);

	private WebCamTexture webcamTexture_;
	private bool isWebCamInitialized_ = false;
	private int width_;
	private int height_;

	private IntPtr aruco_ = IntPtr.Zero;
	private bool isArucoUpdated_ = false;
	public float unityMarkerSize = 1f;
	public float markerSize = 0.1f;
	public string cameraParamsFileName = "intrinsics.yml";
	private List<MarkerResult> markers_ = new List<MarkerResult>();
	private int markerNum_;

	private Thread thread_;
	private Mutex mutex_;

	public GameObject prefab;
	private Dictionary<int, GameObject> arObjects_ = new Dictionary<int, GameObject>();

	[StructLayout(LayoutKind.Sequential)]
	struct MarkerResult
	{
		[MarshalAs(UnmanagedType.I4)]
		public int id;
		[MarshalAs(UnmanagedType.ByValArray, SizeConst=3)]
		public double[] position;
		[MarshalAs(UnmanagedType.ByValArray, SizeConst=4)]
		public double[] orientation;
	}

	public float cameraFov = 38f;

	string GetFilePath(string fileName)
	{
#if UNITY_EDITOR
		return Application.streamingAssetsPath + "/" + fileName;
#elif UNITY_ANDROID
		return "/sdcard/AndroidOpenCvSample/" + fileName;
#endif
	}

	void Awake()
	{
		SetupWebCamTexture();
		InitializeAruco();

		mutex_ = new Mutex();
		thread_ = new Thread(() => {
			try {
				for (;;) {
					Thread.Sleep(0);
					if (!isArucoUpdated_) {
						var num = aruco_detect(aruco_);
						mutex_.WaitOne();
						GetMarkers(num);
						mutex_.ReleaseMutex();
						isArucoUpdated_ = true;
					}
				}
			} catch (Exception e) {
				if (!(e is ThreadAbortException)) {
					Debug.LogError("Unexpected Death: " + e.ToString());
				}
			}
		});

		thread_.Start();
	}

	void OnDestroy()
	{
		thread_.Abort();
		FinalizeAruco();
	}

	void SetupWebCamTexture()
	{
		var devices = WebCamTexture.devices;
		if (devices.Length > 0) {
			webcamTexture_ = new WebCamTexture(devices[0].name, 1280, 720);
			webcamTexture_.Play();
			GetComponent<Renderer>().material.mainTexture = webcamTexture_;
			width_  = webcamTexture_.width;
			height_ = webcamTexture_.height;
			isWebCamInitialized_ = true;
		} else {
			Debug.Log("no camera");
		}
	}

	void InitializeAruco()
	{
		if (!isWebCamInitialized_) return;

		var path = GetFilePath(cameraParamsFileName);
		aruco_ = aruco_initialize(width_, height_, markerSize, path);
	}

	void FinalizeAruco()
	{
		aruco_finalize(aruco_);
	}

	void GetMarkers(int num)
	{
		markers_.Clear();
		var ptr = aruco_get_markers(aruco_);
		var size = Marshal.SizeOf(typeof(MarkerResult));
		for (int i = 0; i < num; ++i) {
			var data = new IntPtr(ptr.ToInt64() + size * i);
			var marker = (MarkerResult)Marshal.PtrToStructure(data, typeof(MarkerResult));
			markers_.Add(marker);
		}
	}

	void OnMarkerDetected(int id, Vector3 pos, Quaternion rot)
	{
		if (arObjects_.ContainsKey(id)) {
			var obj = arObjects_[id].transform;
			obj.localPosition = pos;
			obj.localRotation = rot;
		} else {
			var obj = Instantiate(prefab) as GameObject;
			obj.transform.parent = Camera.main.transform;
			obj.transform.localPosition = pos;
			obj.transform.localRotation = rot;
			arObjects_.Add(id, obj);
		}
	}

	Vector3 GetMarkerPos(double[] p)
	{
		var unityFov = Camera.main.fieldOfView;
		// should be 1f
		var xyScaleFactor = Mathf.Tan(Mathf.Deg2Rad * unityFov) / Mathf.Tan(Mathf.Deg2Rad * cameraFov);
		var realToUnityScale = unityMarkerSize / markerSize;
		var x = -(float)p[0] * xyScaleFactor;
		var y = -(float)p[1] * xyScaleFactor;
		var z =  (float)p[2];
		return new Vector3(x, y, z) * realToUnityScale;
	}

	Quaternion GetMarkerRot(double[] q)
	{
		var x = -(float)q[2];
		var y =  (float)q[1];
		var z =  (float)q[0];
		var w = -(float)q[3];
		return new Quaternion(x, y, z, w);
	}

	void FullScreen()
	{
		var fov = Camera.main.fieldOfView;
		var farPos = 1000f;
		var scaleY = farPos * Mathf.Tan(Mathf.Deg2Rad * fov / 2) * 2 * 0.1f;
		var scaleX = scaleY * width_ / height_;
		transform.localPosition = Vector3.forward * farPos;
		transform.localScale = new Vector3(scaleX, 0f, scaleY);
	}

	void Update()
	{
		if (isWebCamInitialized_ && webcamTexture_.didUpdateThisFrame) return;

		if (isArucoUpdated_) {
			var pixels = webcamTexture_.GetPixels32();
			var handle = GCHandle.Alloc(pixels, GCHandleType.Pinned);
			var ptr	= handle.AddrOfPinnedObject();
			aruco_set_image(aruco_, ptr);
			handle.Free();

			isArucoUpdated_ = false;
		}

		mutex_.WaitOne();
		foreach (var marker in markers_) {
			var pos = GetMarkerPos(marker.position);
			var rot = GetMarkerRot(marker.orientation);
			OnMarkerDetected(marker.id, pos, rot);
			//Debug.Log("pos: " + pos + "\nrot: " + rot.eulerAngles);
		}
		mutex_.ReleaseMutex();

		FullScreen();
	}
}

aruco.cpp

#include <opencv2/opencv.hpp>
#include <aruco.h>
#include <cstdio>
#include "log.h"


class aruco_manager
{
public:
	struct marker_result
	{
		int id;
		double position[3];
		double orientation[4];
	};

	aruco_manager()
	{
	}

	void set_image_size(int width, int height)
	{
		width_ = width;
		height_ = height;
		input_ = cv::Mat(height, width, CV_8UC4);
	}

	void set_marker_size(float marker_size)
	{
		marker_size_ = marker_size;
	}

	void set_params(const char* camera_params_file_path)
	{
		params_.readFromXMLFile(camera_params_file_path);
		params_.resize(cv::Size(width_, height_));
	}

	void set_image(unsigned char* src)
	{
		if (src != nullptr) {
			input_ = cv::Mat(height_, width_, CV_8UC4, src).clone();
		}
	}

	void get_image(unsigned char* dest) const
	{
		if (dest != nullptr) {
			memcpy(dest, output_.data, output_.total() * output_.elemSize());
		}
	}

	size_t detect()
	{
		if (input_.empty()) return -1;

		cv::Mat bgr;
		cv::cvtColor(input_, bgr, cv::COLOR_BGRA2BGR);

		std::vector<aruco::Marker> markers;
		detector_.detect(bgr, markers, params_, marker_size_, true);

		markers_.clear();

		for (auto&& marker : markers) {
			marker.draw(bgr, cv::Scalar(0, 0, 255), 2);
			aruco::CvDrawingUtils::draw3dCube(bgr, marker, params_);

			marker_result result;
			result.id = marker.id;
			marker.OgreGetPoseParameters(result.position, result.orientation);

			markers_.push_back(result);
		}

		cv::cvtColor(bgr, output_, cv::COLOR_BGR2BGRA);
		input_.release();

		return markers_.size();
	}

	void* get_markers()
	{
		return &markers_[0];
	}

private:
	aruco::CameraParameters params_;
	aruco::MarkerDetector detector_;
	cv::Mat input_;
	cv::Mat output_;
	int width_  = 0;
	int height_ = 0;
	float marker_size_ = 0.1f;
	int marker_num_ = 10;
	std::vector<marker_result> markers_;
};


extern "C"
{

void* aruco_initialize(
	int width,
	int height,
	float marker_size,
	const char* camera_params_file_path)
{
	auto manager = new aruco_manager();
	manager->set_image_size(width, height);
	manager->set_marker_size(marker_size);
	manager->set_params(camera_params_file_path);
	return manager;
}

void aruco_finalize(void* instance)
{
	auto manager = static_cast<aruco_manager*>(instance);
	delete manager;
	instance = nullptr;
}

void aruco_set_image(void* instance, unsigned char* src)
{
	auto manager = static_cast<aruco_manager*>(instance);
	if (manager == nullptr) return;
	manager->set_image(src);
}

void aruco_get_image(void* instance, unsigned char* dest)
{
	auto manager = static_cast<aruco_manager*>(instance);
	if (manager == nullptr) return;
	manager->get_image(dest);
}

int aruco_detect(void* instance)
{
	auto manager = static_cast<aruco_manager*>(instance);
	if (manager == nullptr) return -1;
	return static_cast<int>(manager->detect());
}

void* aruco_get_markers(void* instance)
{
	auto manager = static_cast<aruco_manager*>(instance);
	if (manager == nullptr) return nullptr;
	return manager->get_markers();
}

}