ARマーカーの値の取得
1. IDの取得
ids.rows()
で検出できたIDの個数を取得。個数が0個より多いとき、各ARマーカーの座標・回転を取得
// IDの数が検出できたARマーカーの数
idsNum = ids.rows();
if (idsNum > 0)
{
// 各ARマーカーのIDを格納
idsArr = new int[idsNum];
ids.get(0, 0, idsArr); // ← ここで検出したARマーカーのIDを配列に
detected = true;
EstiamtePoseMarkers();
}
2. 各ARマーカーの座標・回転を取得
-
検出した各ARマーカーの平行移動・回転ベクトルを計算
Aruco.estimatePoseSingleMarkers(/*各ARマーカーの角の画像座標*/,
/*ARマーカー1辺の長さ(m)*/,
/*内部行列*/,
/*歪み係数*/,
/*各ARマーカーの回転ベクトル(Out)*/,
/*各ARマーカーの平行移動ベクトル(Out)*/)
で平行移動・回転ベクトルを取得。
その後、rVecs,tVecsを計算しやすい配列にし、回転ベクトルをオイラー角に変換している。
また、追従対象の情報を計算しやすいように構造体を定義している。
public struct ARMarker
{
public int ID;
public double[] tArr;
public double[] rArr;
public void Set_tArr(Mat tVec)
{
tArr = new double[3];
tVec.get(0, 0, tArr);
}
public void Set_rArr(Mat rVec)
{
rArr = new double[3];
rVec.get(0, 0, rArr);
}
}
// 省略
private void EstiamtePoseMarkers(float markerLength = 0.06F)
{
if (idsArr.Length < 1) return;
Mat rVecs = new Mat(), tVecs = new Mat();
// tVecs は 要素数が1、チャンネル数が3で得られる
// rVecs も同様に 要素数が1、チャンネル数が3で得られる
Aruco.estimatePoseSingleMarkers(corners, markerLength, cameraMat, distMat, rVecs, tVecs);
/*
* rVecs : CV_64FC3
* channels : 3
* rows : 検知したARマーカーの数
* cols : 1
*/
/*
* tVecs : CV_64FC3
* channels : 3
* rows : 検知したARマーカーの数
* cols : 1
*/
ARMarker[] markers = new ARMarker[idsArr.Length];
int i;
for (i = 0; i < idsArr.Length; i++)
{
// IDを格納
markers[i].ID = idsArr[i];
// i番目のARマーカーの位置ベクトルを格納
markers[i].Set_tArr(tVecs.row(i));
// i番目のARマーカーの位置ベクトルを取り出してベクトルの形にする
Mat _tVec = new Mat(3, 1, CvType.CV_64FC1);
_tVec.put(0, 0, markers[i].tArr);
// 一旦i番目のARマーカーの回転ベクトルを格納
markers[i].Set_rArr(rVecs.row(i));
// i番目のARマーカーの回転ベクトルを取り出してベクトルの形にする
Mat _rVec = new Mat(3, 1, CvType.CV_64FC1);
_rVec.put(0, 0, markers[i].rArr);
// 回転ベクトルから回転行列への変換を行う
// rmatrix 3×3の回転行列
Mat rMat = new Mat();
Calib3d.Rodrigues(_rVec, rMat);
/*
* rMat
* channels : 1
* rows : 3
* cols : 3
*/
// 行列の拡大
Mat projMat = new Mat();
List<Mat> src = new List<Mat> { rMat, new Mat(3,1,CvType.CV_64FC1,new Scalar(0)) };
// rMat(3×3) → projMat(3×4)へ変換
Core.hconcat(src, projMat);
// オイラー角以外で必要のない値も受け取る必要があるので宣言しておく
Mat cameraMatrix = new Mat(), rotMatrixX = new Mat(), rotMatrixY = new Mat(), rotMatrixZ = new Mat(), eulerAngles = new Mat();
/* オイラー角への変換
* projMatrix : input : 3×4 Mat
* cameraMatrix : output : 3×3 Mat
* rotMatrix : output : 3×3 Mat
* transVect : output : 4×1 Mat
* eulerAngles : output : 3×1 Mat ← この出力だけ必要
*/
Calib3d.decomposeProjectionMatrix(projMat, cameraMatrix, rMat, _tVec, rotMatrixX, rotMatrixY, rotMatrixZ, eulerAngles);
/*
* eulerAngles
* channels : 1
* rows : 3
* cols : 1
*/
// i番目のARマーカーの回転ベクトルを格納
markers[i].Set_rArr(eulerAngles);
}
CalcTargetRotation(markers);
}