1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51
| Mat H = findHomography(initial,points[0],CV_LMEDS); // j'ai essayé avec ransac également
Mat Hc;
Mat h1 = Mat::zeros(3, 1, CV_64F);
Mat h2 = Mat::zeros(3, 1, CV_64F);
Mat h3 = Mat::zeros(3, 1, CV_64F);
h1.at<double>(0,0) = H.at<double>(0,0);
h1.at<double>(1,0) = H.at<double>(1,0);
h1.at<double>(2,0) = H.at<double>(2,0);
h2.at<double>(0,0) = H.at<double>(0,1);
h2.at<double>(1,0) = H.at<double>(1,1);
h2.at<double>(2,0) = H.at<double>(2,1);
h3.at<double>(0,0) = H.at<double>(0,2);
h3.at<double>(1,0) = H.at<double>(1,2);
h3.at<double>(2,0) = H.at<double>(2,2);
Hc = Intrin.inv()*H;
double l1 = 1/norm(Intrin.inv()*h1);
double l2 = 1/norm(Intrin.inv()*h2);
Mat t = (l1/2+l2/2)*Intrin.inv()*h3;
cout << "t = " << endl << " " << t << endl << endl;
Mat r1 = (l1/2+l2/2)*Intrin.inv()*h1;
Mat r2 = (l1/2+l2/2)*Intrin.inv()*h2;
Mat r3 = Mat::zeros(3, 1, CV_64F);
//cvCrossProduct(r1,r2,r3);
r3=r1.cross(r2);
Mat R = Mat::zeros(3, 3, CV_64F);
R.at<double>(0,0)= r1.at<double>(0,0); R.at<double>(0,1)= r2.at<double>(0,0); R.at<double>(0,2)= r3.at<double>(0,0);
R.at<double>(1,0)= r1.at<double>(1,0); R.at<double>(1,1)= r2.at<double>(1,0); R.at<double>(1,2)= r3.at<double>(1,0);
R.at<double>(2,0)= r1.at<double>(2,0); R.at<double>(2,1)= r2.at<double>(2,0); R.at<double>(2,2)= r3.at<double>(2,0);
Mat W, U, V;
SVD::compute(R, W, U, V,SVD::FULL_UV);
Mat Rc = U*V.t();
cout << "Rc = " << endl << " " << Rc << endl << endl; |
Partager