public void EstimatePoseSingleMarkers()
{
using var image = Image("markers_6x6_250.png", ImreadModes.Grayscale);
using var dict = CvAruco.GetPredefinedDictionary(PredefinedDictionaryName.Dict6X6_250);
var param = DetectorParameters.Create();
CvAruco.DetectMarkers(image, dict, out var corners, out _, param, out _);
using var cameraMatrix = Mat.Eye(3, 3, MatType.CV_64FC1);
using var distCoeffs = Mat.Zeros(4, 1, MatType.CV_64FC1);
using var rvec = new Mat();
using var tvec = new Mat();
using var objPoints = new Mat();
CvAruco.EstimatePoseSingleMarkers(corners, 6, cameraMatrix, distCoeffs, rvec, tvec, objPoints);
Assert.Equal(20, rvec.Total());
Assert.Equal(20, tvec.Total());
Assert.Equal(4, objPoints.Total());
}
// Update is called once per frame
void Update()
{
Point2f[][] maker_corners; //ARマーカのカドの座標
int[] maker_ids; //検出されたARマーカのID
Point2f[][] reject_points;
cam.Read(cam_frame);//フレームの更新
//ARマーカの検出
CvAruco.DetectMarkers(cam_frame, ar_dict, out maker_corners, out maker_ids, detect_param, out reject_points);
//キャリブレーションデータを設定
double[,] array3_3 = new double[3, 3] {
{ 548.8, 0.0, 332.1 }, { 0.0, 549.8, 236.2 }, { 0.0, 0.0, 1.0 }
};
Mat cameraMatrix = new Mat(3, 3, MatType.CV_32FC1, array3_3);
double[,] array1_5 = new double[1, 5] {
{ -0.0886, 0.4802, -0.0026, 0.0019, -0.8238 }
};
Mat distortionCoefficients = new Mat(1, 5, MatType.CV_32FC1, array1_5);
Mat rvec = new Mat();
Mat tvec = new Mat();
//VectorOfVec3f vector3_rvec = new VectorOfVec3f();
//IEnumerable<VectorOfVec3f> vector3_rvec = new IEnumerable<VectorOfVec3f>;
if (maker_ids.Length != 0)
{
//検出されたマーカ情報の描画
CvAruco.DrawDetectedMarkers(cam_frame, maker_corners, maker_ids, new Scalar(0, 255, 0));
//Debug.Log("Find");
/*
* Mat rvec = new Mat(3, maker_ids.Length, MatType.CV_32FC1);
* Mat tvec = new Mat(3, maker_ids.Length, MatType.CV_32FC1);
*/
/*
* Debug.Log(tvec.Rows);
* Debug.Log(tvec.Cols);
* Debug.Log(tvec.Size());
* Debug.Log(tvec.Dims());
*/
CvAruco.EstimatePoseSingleMarkers(maker_corners, (float)1.8, cameraMatrix, distortionCoefficients, rvec, tvec);
//マーカまでの距離
/*
* //1
* Debug.Log(tvec.Rows);
* //1
* Debug.Log(tvec.Cols);
* //1:1
* Debug.Log(tvec.Size());
* //2
* Debug.Log(tvec.Dims());
* //3=#define CV_16S
* Debug.Log(tvec.Channels());
*/
Vec3d vec3D = tvec.At <Vec3d>(0, 0);
double z = vec3D[2];
Debug.Log(z);
/*
* Debug.Log(tvec.At<Vec3d>(0, 0)[2]);
*/
//Debug.Log(tvec.Data[0]);
//Debug.Log(tvec.At<Vec3b>(1, 1)[2]);
//Debug.Log(tvec[new OpenCvSharp.Rect(0, 1,0,1)]);
/*
* Debug.Log(rvec.Rows);
* Debug.Log(rvec.Cols);
* Debug.Log(rvec.Size());
* Debug.Log(rvec.Dims());
*/
/*
* //Debug.Log(cameraMatrix.Size());
* //1
* Debug.Log(distortionCoefficients.Rows);
* //5
* Debug.Log(distortionCoefficients.Cols);
* Debug.Log(distortionCoefficients.Size());
*/
/*
* //1channelはdoubleできれいに取れる
* Debug.Log(distortionCoefficients.At<double>(0,0));
* //1
* Debug.Log(distortionCoefficients.Channels());
*/
//Debug.Log(tvec.)
//24
//Debug.Log(tvec.Step());
//3
//Debug.Log(tvec.Channels());
//Debug.Log(tvec.At<double>(0, 0));
//Debug.Log(tvec.At<double>(0, 1));
//Debug.Log(tvec.At<double>(0, 2));
/*
* Debug.Log(rvec.GetArray(0, 1));
* Debug.Log(rvec.GetArray(0, 2));
*/
}
cam_Texture.LoadImage(cam_frame.ImEncode());
}
void Update()
{
//Leapmotionのカメラの映像を取得
readLeftRightFrame();
Point2f[][] maker_corners;
int[] maker_ids;
Point2f[][] reject_points;
//ARマーカの検出
CvAruco.DetectMarkers(leftRight_frame[0], ar_dict, out maker_corners, out maker_ids, detect_param, out reject_points);
GameObject[] animals = GameObject.FindGameObjectsWithTag("animal");
//マーカが検出されない場合はすべてのanimalオブジェクトを削除
if (maker_ids.Length == 0)
{
foreach (GameObject animal in animals)
{
animalMaster animal_script = animal.GetComponent <animalMaster>();
animal_script.destoryThis();
Debug.Log("destroy");
}
}
//マーカが検出されなかったanimalオブジェクトを削除
else
{
foreach (GameObject animal in animals)
{
//検出されたIDと一致するゲームオブジェクトが存在しなかったら
if (DetObj.CheckSameID(animal, maker_ids) == false)
{
animalMaster animal_script = animal.GetComponent <animalMaster>();
animal_script.destoryThis();
}
}
}
if (maker_ids.Length > 0)
{
//検出されたマーカ情報の描画
CvAruco.DrawDetectedMarkers(leftRight_frame[0], maker_corners, maker_ids, new Scalar(0, 255, 0));
//マーカの姿勢推定
OutputArray tvec = new Mat(), rvec = new Mat();
CvAruco.EstimatePoseSingleMarkers(maker_corners, 0.6f, cameraMatrix, distCoefficients, rvec, tvec, null);
//検出されたIDに対する操作
for (int i = 0; i < maker_ids.Length; i++)
{
//ID辞書と一致するマーカが検出されたら
if (DetObj.CheckTargetMarker(maker_ids[i], id_dict) == true)
{
if (animals != null)
{
//すでにオブジェクトが存在していたら
if (DetObj.CheckTargetObject(maker_ids[i], animals) == true)
{
int animal_index = DetObj.GetAnimalIndex(maker_ids[i], animals);
animalMaster animal_script = animals[animal_index].GetComponent <animalMaster>();
animal_script.moveModel(i, tvec, rvec);
}
//オブジェクトが存在しない場合
else
{
GameObject newAnimal = Instantiate(animal);
animalMaster animal_script = newAnimal.GetComponent <animalMaster>();
int animal_index = DetObj.GetAnimalIndex(maker_ids[i], animals);
animal_script.setParam(maker_ids[i], i, tvec, rvec);
}
}
else
{
GameObject newAnimal = Instantiate(animal);
animalMaster animal_script = newAnimal.GetComponent <animalMaster>();
animal_script.setParam(maker_ids[i], 0, tvec, rvec);
}
}
}
}
}