[英]Homography matrix decomposition into rotation matrix and translation vector
我正在使用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]);
非常感謝您的回復
聲明:本站的技術帖子網頁,遵循CC BY-SA 4.0協議,如果您需要轉載,請注明本站網址或者原文地址。任何問題請咨詢:yoyou2525@163.com.