private double[][] estimatePose(List<Mat> corners){
float marker_length = (float) 0.05;//0.05
double camera_matrix[] = {344.173397, 0.000000, 630.793795,
0.000000, 344.277922, 487.033834,
0.000000, 0.000000, 1.000000};
double distortion_coefficients[] = {-0.152963, 0.017530, -0.001107,
-0.000210, 0.000000};
Mat camera_matrix_mat = new Mat(3,3,CV_64F);
Mat distortion_coefficients_mat = new Mat(1,5,CV_64F);
camera_matrix_mat.put(0,0,camera_matrix[0]);
camera_matrix_mat.put(0,1,camera_matrix[1]);
camera_matrix_mat.put(0,2,camera_matrix[2]);
camera_matrix_mat.put(1,0,camera_matrix[3]);
camera_matrix_mat.put(1,1,camera_matrix[4]);
camera_matrix_mat.put(1,2,camera_matrix[5]);
camera_matrix_mat.put(2,0,camera_matrix[6]);
camera_matrix_mat.put(2,1,camera_matrix[7]);
camera_matrix_mat.put(2,2,camera_matrix[8]);
distortion_coefficients_mat.put(0,0,distortion_coefficients[0]);
distortion_coefficients_mat.put(0,1,distortion_coefficients[1]);
distortion_coefficients_mat.put(0,2,distortion_coefficients[2]);
distortion_coefficients_mat.put(0,3,distortion_coefficients[3]);
distortion_coefficients_mat.put(0,4,distortion_coefficients[4]);
Mat rvecs = new Mat();
Mat tvecs = new Mat();
Aruco.estimatePoseSingleMarkers(corners,marker_length,camera_matrix_mat,distortion_coefficients_mat,rvecs,tvecs);
Log.d("MY_STRING", "rvecs[0,0] =" + Arrays.toString(rvecs.get(0, 0)));
Log.d("MY_STRING", "tvecs[0,0] = "+ Arrays.toString(tvecs.get(0, 0)));
double[][] ans = new double[2][];
ans[0] = rvecs.get(0,0);
ans[1] = tvecs.get(0,0);
return ans;
}
private boolean readARmarker(Bitmap bitmap, boolean needSend) {
int width = bitmap.getWidth();
int height = bitmap.getHeight();
bitmap = Bitmap.createScaledBitmap(bitmap,(int)(width*0.3),(int)(height*0.3),true);
Mat inputImage = new Mat();
bitmapToMat(bitmap,inputImage,false);
Dictionary dictionary = Aruco.getPredefinedDictionary(Aruco.DICT_5X5_250);
List<Mat> corners = new ArrayList<>();
Mat markerIds = new Mat();
DetectorParameters parameters = DetectorParameters.create();
Imgproc.cvtColor(inputImage, inputImage, Imgproc.COLOR_BGR2GRAY);
Aruco.detectMarkers(inputImage, dictionary, corners, markerIds, parameters);
String markerIDfromArr = Arrays.toString(markerIds.get(0, 0));
if (markerIDfromArr.equals("null")) {
return false;
} else {
String markerID = String.valueOf((int)(markerIds.get(0, 0)[0]));
double[][] vecs = estimatePose(corners);
//vecs[][]はrvecとtvecが入った二次元配列です。以下にそれぞれの要素を示します。
Log.d("MY_STRING", "rvec_1 = " +vecs[0][0]);
Log.d("MY_STRING", "rvec_2 = " +vecs[0][1]);
Log.d("MY_STRING", "rvec_3 = " +vecs[0][2]);
Log.d("MY_STRING", "tvec_1 = " +vecs[1][0]);
Log.d("MY_STRING", "tvec_2 = " +vecs[1][1]);
Log.d("MY_STRING", "tvec_3 = " +vecs[1][2]);
if(needSend){
api.judgeSendDiscoveredAR(markerID);
}
return true;
}
}