Error Getting Head Pose using OpenCVForUnity.UnityUtils.ARUtils.ConvertRvecTvecToPoseData

[mattycorbett]

Unity 2022.2.19f1 OpenCVForUnity 2.5.4

I am trying to create a head post estimation from a MediaPipe face landmark list from the device's camera. After getting the landmarks, I use the code below to estimate the pose of the face

` var camera_matrix = new Mat(3, 3, CvType.CV_32F); var dist_coeffs = new MatOfDouble(1, 4, CvType.CV_32F); Mat Rvec = new Mat(); Mat Tvec = new Mat(); Mat rvec = new Mat(1, 3, CvType.CV_64FC1); Mat tvec = new Mat(1, 3, CvType.CV_64FC1);

            camera_matrix.put(0, 0, 600, 0, 400, 2, 600, 300, 0, 0, 0);
            dist_coeffs.put(0, 0, 600, 0, 400, 2);

            OpenCVForUnity.CoreModule.Point[] imagePoints = new OpenCVForUnity.CoreModule.Point[6];
            imagePoints[0] = new OpenCVForUnity.CoreModule.Point(landmarks.Landmark[1].X, landmarks.Landmark[1].Y);
            imagePoints[1] = new OpenCVForUnity.CoreModule.Point(landmarks.Landmark[52].X, landmarks.Landmark[52].Y);
            imagePoints[2] = new OpenCVForUnity.CoreModule.Point(landmarks.Landmark[226].X, landmarks.Landmark[226].Y);
            imagePoints[3] = new OpenCVForUnity.CoreModule.Point(landmarks.Landmark[446].X, landmarks.Landmark[446].Y);
            imagePoints[4] = new OpenCVForUnity.CoreModule.Point(landmarks.Landmark[57].X, landmarks.Landmark[57].Y);
            imagePoints[5] = new OpenCVForUnity.CoreModule.Point(landmarks.Landmark[287].X, landmarks.Landmark[287].Y);
            var image_points = new MatOfPoint2f(imagePoints);

            OpenCVForUnity.CoreModule.Point3[] objectPoints = new OpenCVForUnity.CoreModule.Point3[6];
            objectPoints[0] = new OpenCVForUnity.CoreModule.Point3(rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[0].x, rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[0].y,rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[0].z);
            objectPoints[1] = new OpenCVForUnity.CoreModule.Point3(rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[1].x, rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[1].y, rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[1].z);
            objectPoints[2] = new OpenCVForUnity.CoreModule.Point3(rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[2].x, rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[2].y, rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[2].z);
            objectPoints[3] = new OpenCVForUnity.CoreModule.Point3(rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[3].x, rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[3].y, rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[3].z);
            objectPoints[4] = new OpenCVForUnity.CoreModule.Point3(rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[4].x, rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[4].y, rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[4].z);
            objectPoints[5] = new OpenCVForUnity.CoreModule.Point3(rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[5].x, rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[5].y, rearFaceCube.GetComponent<MeshFilter>().mesh.vertices[5].z);
            MatOfPoint3f object_points = new MatOfPoint3f(objectPoints);

            Calib3d.solvePnP(object_points, image_points, camera_matrix, dist_coeffs, rvec, tvec);

            Mat rotMat = new Mat(3, 3, CvType.CV_64FC1);

            Calib3d.Rodrigues(Rvec, rotMat);

            rvec.convertTo(Rvec, CvType.CV_64FC1);
            tvec.convertTo(Tvec, CvType.CV_64FC1);

            List<double> rvecList = new List<double>();
            List<double> tvecList = new List<double>();

            OpenCVForUnity.UtilsModule.Converters.Mat_to_vector_double(Rvec.reshape(1, 1), rvecList);
            OpenCVForUnity.UtilsModule.Converters.Mat_to_vector_double(Tvec.reshape(1, 1), tvecList);

            var poseData = OpenCVForUnity.UnityUtils.ARUtils.ConvertRvecTvecToPoseData(rvecList, tvecList);
            var rotationMatrix = OpenCVForUnity.UnityUtils.ARUtils.ConvertPoseDataToMatrix(ref poseData);
            var outQuat = OpenCVForUnity.UnityUtils.ARUtils.ExtractRotationFromMatrix(ref rotationMatrix);

            rearFaceCube.transform.rotation = outQuat;`

However, when I run this code, I get the error "CvException: CvType.CV_64FC1 != m.type() || m.cols()!=1 Mat [ 13CV_64FC1, isCont=True, isSubmat=False, nativeObj=0x2131137118096, dataAddr=0x2131137119232 ] OpenCVForUnity.UtilsModule.Converters.Mat_to_vector_double (OpenCVForUnity.CoreModule.Mat m, System.Collections.Generic.List`1[T] ds) (at Assets/OpenCVForUnity/org/opencv/utils/Converters.cs:824) Mediapipe.Unity.CameraCapture.Update () (at Assets/Scripts/CameraCapture.cs:236)"

This happens at the var poseData = OpenCVForUnity.UnityUtils.ARUtils.ConvertRvecTvecToPoseData(rvecList, tvecList) line. When I try to use the Rvec and Tvec without conversion, and am told that it cant convert a Mat to a List<> and wont run. When I DO convert it, the List has 3 columns, and requires one. Has anyone done this? Where am I going wrong?

[EnoxSoftware]

The error that occurs in the Converters.Mat_to_vector_double function can be resolved by modifying the code as follows

OpenCVForUnity.UtilsModule.Converters.Mat_to_vector_double(Rvec.reshape(1, 3), rvecList);
OpenCVForUnity.UtilsModule.Converters.Mat_to_vector_double(Tvec.reshape(1, 3), tvecList);

The simple code to convert rvec and tvec obtained by the Calib3d.solvePnP function to poseData is as follows.

 // Convert to unity pose data.
double[] rvecArr = new double[3];
rvec.get(0, 0, rvecArr);
double[] tvecArr = new double[3];
tvec.get(0, 0, tvecArr);
PoseData poseData = ARUtils.ConvertRvecTvecToPoseData(rvecArr, tvecArr);

var outQuat = poseData.rot;

[mattycorbett]

This worked. Thank you