| Line 28... |
Line 28... |
| 28 |
cv::Vec3f rot_mark;
|
28 |
cv::Vec3f rot_mark;
|
| 29 |
cv::Rodrigues(R[i], rot);
|
29 |
cv::Rodrigues(R[i], rot);
|
| 30 |
cv::Rodrigues(R_mark[i], rot_mark);
|
30 |
cv::Rodrigues(R_mark[i], rot_mark);
|
| 31 |
|
31 |
|
| 32 |
cv::Vec3f P = 2.0*sin(cv::norm(rot)/2.0)*cv::normalize(rot);
|
32 |
cv::Vec3f P = 2.0*sin(cv::norm(rot)/2.0)*cv::normalize(rot);
|
| - |
|
33 |
//std::cout << "P: " << std::endl << P << std::endl;
|
| 33 |
cv::Vec3f P_mark = 2.0*sin(cv::norm(rot_mark)/2.0)*cv::normalize(rot_mark);
|
34 |
cv::Vec3f P_mark = 2.0*sin(cv::norm(rot_mark)/2.0)*cv::normalize(rot_mark);
|
| 34 |
|
- |
|
| - |
|
35 |
//std::cout << "P_mark: " << std::endl << P_mark << std::endl;
|
| 35 |
cv::Vec3f sum = P+P_mark;
|
36 |
cv::Vec3f sum = P+P_mark;
|
| 36 |
cv::Mat crossProduct = (cv::Mat_<float>(3,3) << 0.0, -sum(2), sum(1), sum(2), 0.0, -sum(0), -sum(1), sum(0), 0.0);
|
37 |
cv::Mat crossProduct = (cv::Mat_<float>(3,3) << 0.0, -sum(2), sum(1), sum(2), 0.0, -sum(0), -sum(1), sum(0), 0.0);
|
| 37 |
|
- |
|
| - |
|
38 |
//std::cout << "crossProduct: " << std::endl << crossProduct << std::endl;
|
| 38 |
crossProduct.copyTo(A.rowRange(i*3, i*3+3));
|
39 |
crossProduct.copyTo(A.rowRange(i*3, i*3+3));
|
| 39 |
|
40 |
|
| 40 |
cv::Mat(P-P_mark).copyTo(b.rowRange(i*3, i*3+3));
|
41 |
cv::Mat(P-P_mark).copyTo(b.rowRange(i*3, i*3+3));
|
| 41 |
}
|
42 |
}
|
| 42 |
|
43 |
|
| 43 |
// solve for rotation
|
44 |
// solve for rotation
|
| 44 |
cv::Vec<float, 9> P_prime;
|
45 |
cv::Vec3f P_prime;
|
| 45 |
cv::solve(A, b, P_prime);
|
46 |
cv::solve(A, b, P_prime, cv::DECOMP_SVD);
|
| 46 |
cv::Vec<float, 9> P = 2.0*P_prime/(cv::sqrt(1.0 + cv::norm(P_prime)*cv::norm(P_prime)));
|
47 |
cv::Vec3f P = 2.0*P_prime/(cv::sqrt(1.0 + cv::norm(P_prime)*cv::norm(P_prime)));
|
| 47 |
float nP = cv::norm(P);
|
48 |
float nP = cv::norm(P);
|
| 48 |
cv::Mat crossProduct = (cv::Mat_<float>(3,3) << 0.0, -P(2), P(1), P(2), 0.0, -P(0), -P(1), P(0), 0.0);
|
49 |
cv::Mat crossProduct = (cv::Mat_<float>(3,3) << 0.0, -P(2), P(1), P(2), 0.0, -P(0), -P(1), P(0), 0.0);
|
| 49 |
cv::Mat OmegaMat = (1.0-nP*nP/2.0)*cv::Mat::eye(3,3,CV_32F) + 0.5*(cv::Mat(P)*cv::Mat(P).t() + cv::sqrt(4.0 - nP*nP)*crossProduct);
|
50 |
cv::Mat OmegaMat = (1.0-nP*nP/2.0)*cv::Mat::eye(3,3,CV_32F) + 0.5*(cv::Mat(P)*cv::Mat(P).t() + cv::sqrt(4.0 - nP*nP)*crossProduct);
|
| 50 |
Omega = cv::Matx33f(OmegaMat);
|
51 |
Omega = cv::Matx33f(OmegaMat);
|
| 51 |
|
52 |
|
| 52 |
// construct equations for translation
|
53 |
// construct equations for translation
|
| 53 |
A.setTo(0.0);
|
54 |
A.setTo(0.0);
|
| 54 |
b.setTo(0.0);
|
55 |
b.setTo(0.0);
|
| 55 |
for(int i=0; i<N; i++){
|
56 |
for(int i=0; i<N; i++){
|
| 56 |
|
57 |
|
| 57 |
cv::Mat diff = cv::Mat(R[i]) - cv::Mat::eye(3, 3, CV_32F);
|
58 |
cv::Mat diff = cv::Mat(R_mark[i]) - cv::Mat::eye(3, 3, CV_32F);
|
| 58 |
diff.copyTo(A.rowRange(i*3, i*3+3));
|
59 |
diff.copyTo(A.rowRange(i*3, i*3+3));
|
| 59 |
|
60 |
|
| 60 |
cv::Mat diff_mark = cv::Mat(Omega*R_mark[i] - R[i]);
|
61 |
cv::Mat diff_mark = cv::Mat(Omega*t[i] - t_mark[i]);
|
| 61 |
diff_mark.copyTo(b.rowRange(i*3, i*3+3));
|
62 |
diff_mark.copyTo(b.rowRange(i*3, i*3+3));
|
| 62 |
}
|
63 |
}
|
| 63 |
|
64 |
|
| 64 |
// solve for translation
|
65 |
// solve for translation
|
| 65 |
cv::solve(A, b, tau);
|
66 |
cv::solve(A, b, tau, cv::DECOMP_SVD);
|
| 66 |
}
|
67 |
}
|
| 67 |
|
68 |
|
| 68 |
// Function to fit two sets of corresponding pose data.
|
69 |
// Function to fit two sets of corresponding pose data.
|
| 69 |
// Finds [Omega | tau], to minimize ||[R_mark | t_mark] - [Omega | tau][R | t]||^2
|
70 |
// Finds [Omega | tau], to minimize ||[R_mark | t_mark] - [Omega | tau][R | t]||^2
|
| 70 |
// Algorithm and notation according to Mili Shah, Comparing two sets of corresponding six degree of freedom data, CVIU 2011.
|
71 |
// Algorithm and notation according to Mili Shah, Comparing two sets of corresponding six degree of freedom data, CVIU 2011.
|