使用来自相机和 opencv 库的图像估计欧拉天使（相机姿势）

Question

我正在开发一个 android 应用程序，我需要使用来自相机和 opencv 库的图像来估计 3D 平面中的在线相机旋转。我喜欢计算欧拉角。

我已经阅读了this 和this page，我可以估计像here 这样的单应矩阵。

我的第一个问题是，我真的应该从相机校准中知道相机内在矩阵还是单应矩阵（相机外在）足以估计欧拉角（俯仰、滚动、偏航）？

如果单应矩阵足够了，我该怎么做呢？

对不起，我真的是opencv的初学者，不能像here描述的那样将单应性的“Mat”分解为旋转矩阵和平移矩阵。如何在android中实现欧拉角？

你可以看到我的代码使用solvePnPRansac()和decomposeProjectionMatrix来计算欧拉角。

但它只返回一个空向量作为 double[] eulerArray = {0,0,0}！！！有人可以帮帮我吗？！那里有什么问题？ 非常感谢您的任何回复！

public double[] findEulerAngles(MatOfKeyPoint keypoints1, MatOfKeyPoint keypoints2, MatOfDMatch matches){

    KeyPoint[] k1 = keypoints1.toArray();
    KeyPoint[] k2 = keypoints2.toArray();


    List<DMatch> matchesList = matches.toList();
    List<KeyPoint> referenceKeypointsList = keypoints2.toList();
    List<KeyPoint> sceneKeypointsList = keypoints1.toList();
    // Calculate the max and min distances between keypoints.
    double maxDist = 0.0;
    double minDist = Double.MAX_VALUE;
    for(DMatch match : matchesList) {
        double dist = match.distance;
        if (dist < minDist) {
            minDist = dist;
        }
        if (dist > maxDist) {
            maxDist = dist;
        }
    }

    // Identify "good" keypoints based on match distance.
    List<Point3> goodReferencePointsList = new ArrayList<Point3>();
    ArrayList<Point> goodScenePointsList = new ArrayList<Point>();
    double maxGoodMatchDist = 1.75 * minDist;
    for(DMatch match : matchesList) {
        if (match.distance < maxGoodMatchDist) {
            Point kk2 = k2[match.queryIdx].pt;
            Point kk1 = k1[match.trainIdx].pt;

            Point3 point3 = new Point3(kk1.x, kk1.y, 0.0);
            goodReferencePointsList.add(point3);
            goodScenePointsList.add( kk2);
            sceneKeypointsList.get(match.queryIdx).pt);
        }
    }


    if (goodReferencePointsList.size() < 4 || goodScenePointsList.size() < 4) {
        // There are too few good points to find the pose.
        return;
    }

    MatOfPoint3f goodReferencePoints = new MatOfPoint3f();
    goodReferencePoints.fromList(goodReferencePointsList);
    MatOfPoint2f goodScenePoints = new MatOfPoint2f();
    goodScenePoints.fromList(goodScenePointsList);

    MatOfDouble mRMat = new MatOfDouble(3, 3, CvType.CV_32F);
    MatOfDouble mTVec = new MatOfDouble(3, 1, CvType.CV_32F);

    //TODO: solve camera intrinsic matrix
    Mat intrinsics = Mat.eye(3, 3, CvType.CV_32F); // dummy camera matrix
    intrinsics.put(0, 0, 400);
    intrinsics.put(1, 1, 400);
    intrinsics.put(0, 2, 640 / 2);
    intrinsics.put(1, 2, 480 / 2);
    Calib3d.solvePnPRansac(goodReferencePoints, goodScenePoints, intrinsics, new MatOfDouble(), mRMat, mTVec);
    MatOfDouble rotCameraMatrix1 = new MatOfDouble(3, 1, CvType.CV_32F);
    double[] rVecArray = mRMat.toArray();
    // Calib3d.Rodrigues(mRMat, rotCameraMatrix1);
    double[] tVecArray = mTVec.toArray();

    MatOfDouble projMatrix = new MatOfDouble(3, 4, CvType.CV_32F); //projMatrix 3x4 input projection matrix P.
    projMatrix.put(0, 0, rVecArray[0]);
    projMatrix.put(0, 1, rVecArray[1]);
    projMatrix.put(0, 2, rVecArray[2]);
    projMatrix.put(0, 3, 0);
    projMatrix.put(1, 0, rVecArray[3]);
    projMatrix.put(1, 1, rVecArray[4]);
    projMatrix.put(1, 2, rVecArray[5]);
    projMatrix.put(1, 3, 0);
    projMatrix.put(2, 0, rVecArray[6]);
    projMatrix.put(2, 1, rVecArray[7]);
    projMatrix.put(2, 2, rVecArray[8]);
    projMatrix.put(2, 3, 0);

    MatOfDouble cameraMatrix = new MatOfDouble(3, 3, CvType.CV_32F); //cameraMatrix Output 3x3 camera matrix K.
    MatOfDouble rotMatrix = new MatOfDouble(3, 3, CvType.CV_32F); //rotMatrix Output 3x3 external rotation matrix R.
    MatOfDouble transVect = new MatOfDouble(4, 1, CvType.CV_32F); //transVect Output 4x1 translation vector T.
    MatOfDouble rotMatrixX = new MatOfDouble(3, 3, CvType.CV_32F); //rotMatrixX a rotMatrixX
    MatOfDouble rotMatrixY = new MatOfDouble(3, 3, CvType.CV_32F); //rotMatrixY a rotMatrixY
    MatOfDouble rotMatrixZ = new MatOfDouble(3, 3, CvType.CV_32F); //rotMatrixZ a rotMatrixZ
    MatOfDouble eulerAngles = new MatOfDouble(3, 1, CvType.CV_32F); //eulerAngles Optional three-element vector containing three Euler angles of rotation in degrees.

    Calib3d.decomposeProjectionMatrix( projMatrix,
            cameraMatrix,
            rotMatrix,
            transVect,
            rotMatrixX,
            rotMatrixY,
            rotMatrixZ,
            eulerAngles);

    double[] eulerArray = eulerAngles.toArray();

    return eulerArray;
}

Answer 1

Homography 关联同一平面表面的图像，因此它只有在图像中有一个主平面并且您可以在两个图像的平面上找到足够的特征点并成功匹配它们时才有效。最少匹配数为 4，数学将在假设匹配 100% 正确的情况下起作用。借助像 RANSAC 这样的稳健估计，即使您的特征点匹配集中的某些元素明显不匹配或未放置在平面上，您也可以获得结果。

对于没有平面性假设的一组 macthed 特征的更一般情况，您需要找到一个基本矩阵。矩阵can be found here 的确切定义。简而言之，它或多或少类似于单应性——它关联了两个图像中的对应点。计算基本矩阵所需的最小匹配数是五个。要从这样一个最小样本中获得结果，您需要确保已建立的匹配是 100% 正确的。同样，如果您的对应集中存在异常值，稳健估计会有所帮助——并且通常存在自动特征检测和匹配。

OpenCV 3.0 has a function 用于基本矩阵计算，方便地与 RANSAC 稳健估计集成。本质矩阵可以分解为旋转矩阵和平移向量，如我之前链接的维基百科文章所示。 OpenCV 3.0 也有a readily available function for this。

Answer 2

现在为我工作了流动代码，我已经从单应矩阵分解了欧拉角！我有一些俯仰、滚动和偏航的值，我不知道是否正确。有谁有Idee，我该如何测试它？！

private static MatOfDMatch filterMatchesByHomography(MatOfKeyPoint keypoints1, MatOfKeyPoint keypoints2, MatOfDMatch matches){

    List<Point> lp1 = new ArrayList<Point>(500);
    List<Point> lp2 = new ArrayList<Point>(500);

    KeyPoint[] k1 = keypoints1.toArray();
    KeyPoint[] k2 = keypoints2.toArray();

    List<DMatch> matchesList = matches.toList();

    if (matchesList.size() < 4){
        MatOfDMatch mat = new MatOfDMatch();
        return mat;
    }

    // Add matches keypoints to new list to apply homography
    for(DMatch match : matchesList){
        Point kk1 = k1[match.queryIdx].pt;
        Point kk2 = k2[match.trainIdx].pt;
        lp1.add(kk1);
        lp2.add(kk2);
    }

    MatOfPoint2f srcPoints = new MatOfPoint2f(lp1.toArray(new Point[0]));
    MatOfPoint2f dstPoints  = new MatOfPoint2f(lp2.toArray(new Point[0]));

    //---------------------------------------

    Mat mask = new Mat();
    Mat homography = Calib3d.findHomography(srcPoints, dstPoints, Calib3d.RANSAC, 0.2, mask); // Finds a perspective transformation between two planes. ---Calib3d.LMEDS

    Mat pose = cameraPoseFromHomography(homography);

    //Decomposing a rotation matrix to eulerangle
    pitch = Math.atan2(pose.get(2, 1)[0], pose.get(2, 2)[0]); // arctan2(r32, r33)
    roll = Math.atan2(-1*pose.get(2, 0)[0], Math.sqrt( Math.pow(pose.get(2, 1)[0], 2) + Math.pow(pose.get(2, 2)[0], 2)) ); // arctan2(-r31, sqrt(r32^2 + r33^2))
    yaw = Math.atan2(pose.get(2, 0)[0], pose.get(0, 0)[0]);

    List<DMatch> matches_homo = new ArrayList<DMatch>();
    int size = (int) mask.size().height;
    for(int i = 0; i < size; i++){          
        if ( mask.get(i, 0)[0] == 1){
            DMatch d = matchesList.get(i);
            matches_homo.add(d);
        }
    }

    MatOfDMatch mat = new MatOfDMatch();
    mat.fromList(matches_homo);
    return mat;
}

这是我来自单应矩阵的相机姿势（参见this page）：

private static Mat cameraPoseFromHomography(Mat h) {
    //Log.d("DEBUG", "cameraPoseFromHomography: homography " + matToString(h));

    Mat pose = Mat.eye(3, 4, CvType.CV_32FC1);  // 3x4 matrix, the camera pose
    float norm1 = (float) Core.norm(h.col(0));
    float norm2 = (float) Core.norm(h.col(1));
    float tnorm = (norm1 + norm2) / 2.0f;       // Normalization value

    Mat normalizedTemp = new Mat();
    Core.normalize(h.col(0), normalizedTemp);
    normalizedTemp.convertTo(normalizedTemp, CvType.CV_32FC1);
    normalizedTemp.copyTo(pose.col(0)); // Normalize the rotation, and copies the column to pose

    Core.normalize(h.col(1), normalizedTemp);
    normalizedTemp.convertTo(normalizedTemp, CvType.CV_32FC1);    
    normalizedTemp.copyTo(pose.col(1));// Normalize the rotation and copies the column to pose

    Mat p3 = pose.col(0).cross(pose.col(1)); // Computes the cross-product of p1 and p2
    p3.copyTo(pose.col(2));// Third column is the crossproduct of columns one and two

    Mat temp = h.col(2);
    double[] buffer = new double[3];
    h.col(2).get(0, 0, buffer);
    pose.put(0, 3, buffer[0] / tnorm);  //vector t [R|t] is the last column of pose
    pose.put(1, 3, buffer[1] / tnorm);
    pose.put(2, 3, buffer[2] / tnorm);

    return pose;
}