将单纯形矩阵分解为旋转矩阵和平移向量

Question

我正在使用opencv 2.4.4开发一个用于Android的增强现实应用程序，并且在单应性分解方面存在一些问题。 如我们所知，单应矩阵被定义为H = A. [R t]，其中A是固有相机矩阵，R是旋转矩阵，t是平移向量。 我想估计相机使用图片的视图侧，以及相机在3d房间的方向。

我可以用opencv函数估计同形矩阵：findHomography，我觉得它有效！ 我在这里是怎么做到的：

static Mat mFindHomography(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, 10, mask); // Finds a perspective transformation between two planes. ---Calib3d.LMEDS Least-Median robust method 

    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);

    matchesFilterdByRansac = (int) mat.size().height;
    return homography;
}

之后，我想分解这个单应矩阵并计算欧拉角。 正如我们所知H = A. [R t]，我将单应矩阵乘以相机固有矩阵的逆：HA ^ { - 1} = [R t]。 所以，我想在旋转和平移中分解[R t]并从旋转矩阵计算欧拉角。 但它没有用。 有什么问题？!!

if(!homography.empty()){ // esstimate pose frome homography 
Mat intrinsics = Mat.zeros(3, 3, CvType.CV_32FC1);  // camera intrinsic matrix 
intrinsics.put(0, 0, 890);
intrinsics.put(0, 2, 400);
intrinsics.put(1, 1, 890);
intrinsics.put(1, 2, 240);
intrinsics.put(2, 2, 1);

// Inverse Matrix from Wolframalpha
double[] inverseIntrinsics = { 0.001020408, 0 , -0.408163265,
        0, 0.0011235955, -0.26966292,
        0, 0 , 1 }; 

// cross multiplication 
double[] rotationTranslation = matrixMultiply3X3(homography, inverseIntrinsics);

Mat pose = Mat.eye(3, 4, CvType.CV_32FC1);  // 3x4 matrix, the camera pose
float norm1 = (float) Core.norm(rotationTranslation.col(0));
float norm2 = (float) Core.norm(rotationTranslation.col(1));
float tnorm = (norm1 + norm2) / 2.0f;       // Normalization value  ---test: float tnorm = (float) h.get(2, 2)[0];// not worked

Mat normalizedTemp = new Mat();
Core.normalize(rotationTranslation.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(rotationTranslation.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

double[] buffer = new double[3];
rotationTranslation.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);

float[] rotationMatrix = new float[9];
rotationMatrix = getArrayFromMat(pose);

float[] eulerOrientation = new float[3];
SensorManager.getOrientation(rotationMatrix, eulerOrientation); 

// Convert radian to degree
double yaw = (double) (eulerOrientation[0]) * (180 / Math.PI));// * -57;
double pitch = (double) (eulerOrientation[1]) * (180 / Math.PI));
double roll = (double) (eulerOrientation[2]) * (180 / Math.PI));}

请注意，opencv 3.0有一个homogeraphy分解函数（ 这里 ），但我正在使用opencv 2.4.4 for android !!! 在java中有它的包装吗？

第二个问题是在欧拉天使中分解旋转矩阵。 有什么问题：

    float[] eulerOrientation = new float[3];
SensorManager.getOrientation(rotationMatrix, eulerOrientation); 

我也使用过这个链接 ，但效果不是更好！

double pitch = Math.atan2(pose.get(2, 1)[0], pose.get(2, 2)[0]);
double 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)) );
double yaw = Math.atan2(pose.get(1, 0)[0], pose.get(0, 0)[0]);

非常感谢您的回复

Answer 1

我希望这个答案能够帮助那些寻求解决方案的人。

我的回答使用c ++和opencv 2.4.9。 我从opencv 3.0复制了decomposehomographymat函数。 在计算单应性后，我使用复制的函数来分解单应性。 要过滤单应矩阵并从4个分解中选择正确的答案，请在此处查看我的答案。

要从旋转矩阵获得欧拉角，您可以参考这个 。 用这种方法我能得到很好的结果。