1 /*
2 * PnPProblem.cpp
3 *
4 * Created on: Mar 28, 2014
5 * Author: Edgar Riba
6 */
7
8 #include <iostream>
9 #include <sstream>
10
11 #include "PnPProblem.h"
12 #include "Mesh.h"
13
14 #include <opencv2/calib3d/calib3d.hpp>
15
16 /* Functions headers */
17 cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2);
18 double DOT(cv::Point3f v1, cv::Point3f v2);
19 cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2);
20 cv::Point3f get_nearest_3D_point(std::vector<cv::Point3f> &points_list, cv::Point3f origin);
21
22
23 /* Functions for Möller–Trumbore intersection algorithm */
24
CROSS(cv::Point3f v1,cv::Point3f v2)25 cv::Point3f CROSS(cv::Point3f v1, cv::Point3f v2)
26 {
27 cv::Point3f tmp_p;
28 tmp_p.x = v1.y*v2.z - v1.z*v2.y;
29 tmp_p.y = v1.z*v2.x - v1.x*v2.z;
30 tmp_p.z = v1.x*v2.y - v1.y*v2.x;
31 return tmp_p;
32 }
33
DOT(cv::Point3f v1,cv::Point3f v2)34 double DOT(cv::Point3f v1, cv::Point3f v2)
35 {
36 return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
37 }
38
SUB(cv::Point3f v1,cv::Point3f v2)39 cv::Point3f SUB(cv::Point3f v1, cv::Point3f v2)
40 {
41 cv::Point3f tmp_p;
42 tmp_p.x = v1.x - v2.x;
43 tmp_p.y = v1.y - v2.y;
44 tmp_p.z = v1.z - v2.z;
45 return tmp_p;
46 }
47
48 /* End functions for Möller–Trumbore intersection algorithm
49 * */
50
51 // Function to get the nearest 3D point to the Ray origin
get_nearest_3D_point(std::vector<cv::Point3f> & points_list,cv::Point3f origin)52 cv::Point3f get_nearest_3D_point(std::vector<cv::Point3f> &points_list, cv::Point3f origin)
53 {
54 cv::Point3f p1 = points_list[0];
55 cv::Point3f p2 = points_list[1];
56
57 double d1 = std::sqrt( std::pow(p1.x-origin.x, 2) + std::pow(p1.y-origin.y, 2) + std::pow(p1.z-origin.z, 2) );
58 double d2 = std::sqrt( std::pow(p2.x-origin.x, 2) + std::pow(p2.y-origin.y, 2) + std::pow(p2.z-origin.z, 2) );
59
60 if(d1 < d2)
61 {
62 return p1;
63 }
64 else
65 {
66 return p2;
67 }
68 }
69
70 // Custom constructor given the intrinsic camera parameters
71
PnPProblem(const double params[])72 PnPProblem::PnPProblem(const double params[])
73 {
74 _A_matrix = cv::Mat::zeros(3, 3, CV_64FC1); // intrinsic camera parameters
75 _A_matrix.at<double>(0, 0) = params[0]; // [ fx 0 cx ]
76 _A_matrix.at<double>(1, 1) = params[1]; // [ 0 fy cy ]
77 _A_matrix.at<double>(0, 2) = params[2]; // [ 0 0 1 ]
78 _A_matrix.at<double>(1, 2) = params[3];
79 _A_matrix.at<double>(2, 2) = 1;
80 _R_matrix = cv::Mat::zeros(3, 3, CV_64FC1); // rotation matrix
81 _t_matrix = cv::Mat::zeros(3, 1, CV_64FC1); // translation matrix
82 _P_matrix = cv::Mat::zeros(3, 4, CV_64FC1); // rotation-translation matrix
83
84 }
85
~PnPProblem()86 PnPProblem::~PnPProblem()
87 {
88 // TODO Auto-generated destructor stub
89 }
90
set_P_matrix(const cv::Mat & R_matrix,const cv::Mat & t_matrix)91 void PnPProblem::set_P_matrix( const cv::Mat &R_matrix, const cv::Mat &t_matrix)
92 {
93 // Rotation-Translation Matrix Definition
94 _P_matrix.at<double>(0,0) = R_matrix.at<double>(0,0);
95 _P_matrix.at<double>(0,1) = R_matrix.at<double>(0,1);
96 _P_matrix.at<double>(0,2) = R_matrix.at<double>(0,2);
97 _P_matrix.at<double>(1,0) = R_matrix.at<double>(1,0);
98 _P_matrix.at<double>(1,1) = R_matrix.at<double>(1,1);
99 _P_matrix.at<double>(1,2) = R_matrix.at<double>(1,2);
100 _P_matrix.at<double>(2,0) = R_matrix.at<double>(2,0);
101 _P_matrix.at<double>(2,1) = R_matrix.at<double>(2,1);
102 _P_matrix.at<double>(0,3) = t_matrix.at<double>(0);
103 _P_matrix.at<double>(1,3) = t_matrix.at<double>(1);
104 _P_matrix.at<double>(2,3) = t_matrix.at<double>(2);
105 }
106
107
108 // Estimate the pose given a list of 2D/3D correspondences and the method to use
estimatePose(const std::vector<cv::Point3f> & list_points3d,const std::vector<cv::Point2f> & list_points2d,int flags)109 bool PnPProblem::estimatePose( const std::vector<cv::Point3f> &list_points3d,
110 const std::vector<cv::Point2f> &list_points2d,
111 int flags)
112 {
113 cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);
114 cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
115 cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
116
117 bool useExtrinsicGuess = false;
118
119 // Pose estimation
120 bool correspondence = cv::solvePnP( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
121 useExtrinsicGuess, flags);
122
123 // Transforms Rotation Vector to Matrix
124 Rodrigues(rvec,_R_matrix);
125 _t_matrix = tvec;
126
127 // Set projection matrix
128 this->set_P_matrix(_R_matrix, _t_matrix);
129
130 return correspondence;
131 }
132
133 // Estimate the pose given a list of 2D/3D correspondences with RANSAC and the method to use
134
estimatePoseRANSAC(const std::vector<cv::Point3f> & list_points3d,const std::vector<cv::Point2f> & list_points2d,int flags,cv::Mat & inliers,int iterationsCount,float reprojectionError,double confidence)135 void PnPProblem::estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points3d, // list with model 3D coordinates
136 const std::vector<cv::Point2f> &list_points2d, // list with scene 2D coordinates
137 int flags, cv::Mat &inliers, int iterationsCount, // PnP method; inliers container
138 float reprojectionError, double confidence ) // Ransac parameters
139 {
140 cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1); // vector of distortion coefficients
141 cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1); // output rotation vector
142 cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1); // output translation vector
143
144 bool useExtrinsicGuess = false; // if true the function uses the provided rvec and tvec values as
145 // initial approximations of the rotation and translation vectors
146
147 cv::solvePnPRansac( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
148 useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
149 inliers, flags );
150
151 Rodrigues(rvec,_R_matrix); // converts Rotation Vector to Matrix
152 _t_matrix = tvec; // set translation matrix
153
154 this->set_P_matrix(_R_matrix, _t_matrix); // set rotation-translation matrix
155
156 }
157
158 // Given the mesh, backproject the 3D points to 2D to verify the pose estimation
verify_points(Mesh * mesh)159 std::vector<cv::Point2f> PnPProblem::verify_points(Mesh *mesh)
160 {
161 std::vector<cv::Point2f> verified_points_2d;
162 for( int i = 0; i < mesh->getNumVertices(); i++)
163 {
164 cv::Point3f point3d = mesh->getVertex(i);
165 cv::Point2f point2d = this->backproject3DPoint(point3d);
166 verified_points_2d.push_back(point2d);
167 }
168
169 return verified_points_2d;
170 }
171
172
173 // Backproject a 3D point to 2D using the estimated pose parameters
174
backproject3DPoint(const cv::Point3f & point3d)175 cv::Point2f PnPProblem::backproject3DPoint(const cv::Point3f &point3d)
176 {
177 // 3D point vector [x y z 1]'
178 cv::Mat point3d_vec = cv::Mat(4, 1, CV_64FC1);
179 point3d_vec.at<double>(0) = point3d.x;
180 point3d_vec.at<double>(1) = point3d.y;
181 point3d_vec.at<double>(2) = point3d.z;
182 point3d_vec.at<double>(3) = 1;
183
184 // 2D point vector [u v 1]'
185 cv::Mat point2d_vec = cv::Mat(4, 1, CV_64FC1);
186 point2d_vec = _A_matrix * _P_matrix * point3d_vec;
187
188 // Normalization of [u v]'
189 cv::Point2f point2d;
190 point2d.x = (float)(point2d_vec.at<double>(0) / point2d_vec.at<double>(2));
191 point2d.y = (float)(point2d_vec.at<double>(1) / point2d_vec.at<double>(2));
192
193 return point2d;
194 }
195
196 // Back project a 2D point to 3D and returns if it's on the object surface
backproject2DPoint(const Mesh * mesh,const cv::Point2f & point2d,cv::Point3f & point3d)197 bool PnPProblem::backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d, cv::Point3f &point3d)
198 {
199 // Triangles list of the object mesh
200 std::vector<std::vector<int> > triangles_list = mesh->getTrianglesList();
201
202 double lambda = 8;
203 double u = point2d.x;
204 double v = point2d.y;
205
206 // Point in vector form
207 cv::Mat point2d_vec = cv::Mat::ones(3, 1, CV_64F); // 3x1
208 point2d_vec.at<double>(0) = u * lambda;
209 point2d_vec.at<double>(1) = v * lambda;
210 point2d_vec.at<double>(2) = lambda;
211
212 // Point in camera coordinates
213 cv::Mat X_c = _A_matrix.inv() * point2d_vec ; // 3x1
214
215 // Point in world coordinates
216 cv::Mat X_w = _R_matrix.inv() * ( X_c - _t_matrix ); // 3x1
217
218 // Center of projection
219 cv::Mat C_op = cv::Mat(_R_matrix.inv()).mul(-1) * _t_matrix; // 3x1
220
221 // Ray direction vector
222 cv::Mat ray = X_w - C_op; // 3x1
223 ray = ray / cv::norm(ray); // 3x1
224
225 // Set up Ray
226 Ray R((cv::Point3f)C_op, (cv::Point3f)ray);
227
228 // A vector to store the intersections found
229 std::vector<cv::Point3f> intersections_list;
230
231 // Loop for all the triangles and check the intersection
232 for (unsigned int i = 0; i < triangles_list.size(); i++)
233 {
234 cv::Point3f V0 = mesh->getVertex(triangles_list[i][0]);
235 cv::Point3f V1 = mesh->getVertex(triangles_list[i][1]);
236 cv::Point3f V2 = mesh->getVertex(triangles_list[i][2]);
237
238 Triangle T(i, V0, V1, V2);
239
240 double out;
241 if(this->intersect_MollerTrumbore(R, T, &out))
242 {
243 cv::Point3f tmp_pt = R.getP0() + out*R.getP1(); // P = O + t*D
244 intersections_list.push_back(tmp_pt);
245 }
246 }
247
248 // If there are intersection, find the nearest one
249 if (!intersections_list.empty())
250 {
251 point3d = get_nearest_3D_point(intersections_list, R.getP0());
252 return true;
253 }
254 else
255 {
256 return false;
257 }
258 }
259
260 // Möller–Trumbore intersection algorithm
intersect_MollerTrumbore(Ray & Ray,Triangle & Triangle,double * out)261 bool PnPProblem::intersect_MollerTrumbore(Ray &Ray, Triangle &Triangle, double *out)
262 {
263 const double EPSILON = 0.000001;
264
265 cv::Point3f e1, e2;
266 cv::Point3f P, Q, T;
267 double det, inv_det, u, v;
268 double t;
269
270 cv::Point3f V1 = Triangle.getV0(); // Triangle vertices
271 cv::Point3f V2 = Triangle.getV1();
272 cv::Point3f V3 = Triangle.getV2();
273
274 cv::Point3f O = Ray.getP0(); // Ray origin
275 cv::Point3f D = Ray.getP1(); // Ray direction
276
277 //Find vectors for two edges sharing V1
278 e1 = SUB(V2, V1);
279 e2 = SUB(V3, V1);
280
281 // Begin calculation determinant - also used to calculate U parameter
282 P = CROSS(D, e2);
283
284 // If determinant is near zero, ray lie in plane of triangle
285 det = DOT(e1, P);
286
287 //NOT CULLING
288 if(det > -EPSILON && det < EPSILON) return false;
289 inv_det = 1.f / det;
290
291 //calculate distance from V1 to ray origin
292 T = SUB(O, V1);
293
294 //Calculate u parameter and test bound
295 u = DOT(T, P) * inv_det;
296
297 //The intersection lies outside of the triangle
298 if(u < 0.f || u > 1.f) return false;
299
300 //Prepare to test v parameter
301 Q = CROSS(T, e1);
302
303 //Calculate V parameter and test bound
304 v = DOT(D, Q) * inv_det;
305
306 //The intersection lies outside of the triangle
307 if(v < 0.f || u + v > 1.f) return false;
308
309 t = DOT(e2, Q) * inv_det;
310
311 if(t > EPSILON) { //ray intersection
312 *out = t;
313 return true;
314 }
315
316 // No hit, no win
317 return false;
318 }
319