← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sw/ext/opencv_bebop/opencv/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/PnPProblem.cpp
 
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
 
 
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
 
 
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
 
 
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
 
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
 
 
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
 
 
86
PnPProblem::~PnPProblem()
 
87
{
 
88
  // TODO Auto-generated destructor stub
 
89
}
 
90
 
 
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>(2,2) = R_matrix.at<double>(2,2);
 
103
  _P_matrix.at<double>(0,3) = t_matrix.at<double>(0);
 
104
  _P_matrix.at<double>(1,3) = t_matrix.at<double>(1);
 
105
  _P_matrix.at<double>(2,3) = t_matrix.at<double>(2);
 
106
}
 
107
 
 
108
 
 
109
// Estimate the pose given a list of 2D/3D correspondences and the method to use
 
110
bool PnPProblem::estimatePose( const std::vector<cv::Point3f> &list_points3d,
 
111
                               const std::vector<cv::Point2f> &list_points2d,
 
112
                               int flags)
 
113
{
 
114
  cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);
 
115
  cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);
 
116
  cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);
 
117
 
 
118
  bool useExtrinsicGuess = false;
 
119
 
 
120
  // Pose estimation
 
121
  bool correspondence = cv::solvePnP( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
 
122
                                      useExtrinsicGuess, flags);
 
123
 
 
124
  // Transforms Rotation Vector to Matrix
 
125
  Rodrigues(rvec,_R_matrix);
 
126
  _t_matrix = tvec;
 
127
 
 
128
  // Set projection matrix
 
129
  this->set_P_matrix(_R_matrix, _t_matrix);
 
130
 
 
131
  return correspondence;
 
132
}
 
133
 
 
134
// Estimate the pose given a list of 2D/3D correspondences with RANSAC and the method to use
 
135
 
 
136
void PnPProblem::estimatePoseRANSAC( const std::vector<cv::Point3f> &list_points3d, // list with model 3D coordinates
 
137
                                     const std::vector<cv::Point2f> &list_points2d,     // list with scene 2D coordinates
 
138
                                     int flags, cv::Mat &inliers, int iterationsCount,  // PnP method; inliers container
 
139
                                     float reprojectionError, double confidence )    // Ransac parameters
 
140
{
 
141
  cv::Mat distCoeffs = cv::Mat::zeros(4, 1, CV_64FC1);  // vector of distortion coefficients
 
142
  cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);          // output rotation vector
 
143
  cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);    // output translation vector
 
144
 
 
145
  bool useExtrinsicGuess = false;   // if true the function uses the provided rvec and tvec values as
 
146
            // initial approximations of the rotation and translation vectors
 
147
 
 
148
  cv::solvePnPRansac( list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec,
 
149
                useExtrinsicGuess, iterationsCount, reprojectionError, confidence,
 
150
                inliers, flags );
 
151
 
 
152
  Rodrigues(rvec,_R_matrix);      // converts Rotation Vector to Matrix
 
153
  _t_matrix = tvec;       // set translation matrix
 
154
 
 
155
  this->set_P_matrix(_R_matrix, _t_matrix); // set rotation-translation matrix
 
156
 
 
157
}
 
158
 
 
159
// Given the mesh, backproject the 3D points to 2D to verify the pose estimation
 
160
std::vector<cv::Point2f> PnPProblem::verify_points(Mesh *mesh)
 
161
{
 
162
  std::vector<cv::Point2f> verified_points_2d;
 
163
  for( int i = 0; i < mesh->getNumVertices(); i++)
 
164
  {
 
165
    cv::Point3f point3d = mesh->getVertex(i);
 
166
    cv::Point2f point2d = this->backproject3DPoint(point3d);
 
167
    verified_points_2d.push_back(point2d);
 
168
  }
 
169
 
 
170
  return verified_points_2d;
 
171
}
 
172
 
 
173
 
 
174
// Backproject a 3D point to 2D using the estimated pose parameters
 
175
 
 
176
cv::Point2f PnPProblem::backproject3DPoint(const cv::Point3f &point3d)
 
177
{
 
178
  // 3D point vector [x y z 1]'
 
179
  cv::Mat point3d_vec = cv::Mat(4, 1, CV_64FC1);
 
180
  point3d_vec.at<double>(0) = point3d.x;
 
181
  point3d_vec.at<double>(1) = point3d.y;
 
182
  point3d_vec.at<double>(2) = point3d.z;
 
183
  point3d_vec.at<double>(3) = 1;
 
184
 
 
185
  // 2D point vector [u v 1]'
 
186
  cv::Mat point2d_vec = cv::Mat(4, 1, CV_64FC1);
 
187
  point2d_vec = _A_matrix * _P_matrix * point3d_vec;
 
188
 
 
189
  // Normalization of [u v]'
 
190
  cv::Point2f point2d;
 
191
  point2d.x = (float)(point2d_vec.at<double>(0) / point2d_vec.at<double>(2));
 
192
  point2d.y = (float)(point2d_vec.at<double>(1) / point2d_vec.at<double>(2));
 
193
 
 
194
  return point2d;
 
195
}
 
196
 
 
197
// Back project a 2D point to 3D and returns if it's on the object surface
 
198
bool PnPProblem::backproject2DPoint(const Mesh *mesh, const cv::Point2f &point2d, cv::Point3f &point3d)
 
199
{
 
200
  // Triangles list of the object mesh
 
201
  std::vector<std::vector<int> > triangles_list = mesh->getTrianglesList();
 
202
 
 
203
  double lambda = 8;
 
204
  double u = point2d.x;
 
205
  double v = point2d.y;
 
206
 
 
207
  // Point in vector form
 
208
  cv::Mat point2d_vec = cv::Mat::ones(3, 1, CV_64F); // 3x1
 
209
  point2d_vec.at<double>(0) = u * lambda;
 
210
  point2d_vec.at<double>(1) = v * lambda;
 
211
  point2d_vec.at<double>(2) = lambda;
 
212
 
 
213
  // Point in camera coordinates
 
214
  cv::Mat X_c = _A_matrix.inv() * point2d_vec ; // 3x1
 
215
 
 
216
  // Point in world coordinates
 
217
  cv::Mat X_w = _R_matrix.inv() * ( X_c - _t_matrix ); // 3x1
 
218
 
 
219
  // Center of projection
 
220
  cv::Mat C_op = cv::Mat(_R_matrix.inv()).mul(-1) * _t_matrix; // 3x1
 
221
 
 
222
  // Ray direction vector
 
223
  cv::Mat ray = X_w - C_op; // 3x1
 
224
  ray = ray / cv::norm(ray); // 3x1
 
225
 
 
226
  // Set up Ray
 
227
  Ray R((cv::Point3f)C_op, (cv::Point3f)ray);
 
228
 
 
229
  // A vector to store the intersections found
 
230
  std::vector<cv::Point3f> intersections_list;
 
231
 
 
232
  // Loop for all the triangles and check the intersection
 
233
  for (unsigned int i = 0; i < triangles_list.size(); i++)
 
234
  {
 
235
    cv::Point3f V0 = mesh->getVertex(triangles_list[i][0]);
 
236
    cv::Point3f V1 = mesh->getVertex(triangles_list[i][1]);
 
237
    cv::Point3f V2 = mesh->getVertex(triangles_list[i][2]);
 
238
 
 
239
    Triangle T(i, V0, V1, V2);
 
240
 
 
241
    double out;
 
242
    if(this->intersect_MollerTrumbore(R, T, &out))
 
243
    {
 
244
      cv::Point3f tmp_pt = R.getP0() + out*R.getP1(); // P = O + t*D
 
245
      intersections_list.push_back(tmp_pt);
 
246
    }
 
247
  }
 
248
 
 
249
  // If there are intersection, find the nearest one
 
250
  if (!intersections_list.empty())
 
251
  {
 
252
    point3d = get_nearest_3D_point(intersections_list, R.getP0());
 
253
    return true;
 
254
  }
 
255
  else
 
256
  {
 
257
    return false;
 
258
  }
 
259
}
 
260
 
 
261
// Möller–Trumbore intersection algorithm
 
262
bool PnPProblem::intersect_MollerTrumbore(Ray &Ray, Triangle &Triangle, double *out)
 
263
{
 
264
  const double EPSILON = 0.000001;
 
265
 
 
266
  cv::Point3f e1, e2;
 
267
  cv::Point3f P, Q, T;
 
268
  double det, inv_det, u, v;
 
269
  double t;
 
270
 
 
271
  cv::Point3f V1 = Triangle.getV0();  // Triangle vertices
 
272
  cv::Point3f V2 = Triangle.getV1();
 
273
  cv::Point3f V3 = Triangle.getV2();
 
274
 
 
275
  cv::Point3f O = Ray.getP0(); // Ray origin
 
276
  cv::Point3f D = Ray.getP1(); // Ray direction
 
277
 
 
278
  //Find vectors for two edges sharing V1
 
279
  e1 = SUB(V2, V1);
 
280
  e2 = SUB(V3, V1);
 
281
 
 
282
  // Begin calculation determinant - also used to calculate U parameter
 
283
  P = CROSS(D, e2);
 
284
 
 
285
  // If determinant is near zero, ray lie in plane of triangle
 
286
  det = DOT(e1, P);
 
287
 
 
288
  //NOT CULLING
 
289
  if(det > -EPSILON && det < EPSILON) return false;
 
290
  inv_det = 1.f / det;
 
291
 
 
292
  //calculate distance from V1 to ray origin
 
293
  T = SUB(O, V1);
 
294
 
 
295
  //Calculate u parameter and test bound
 
296
  u = DOT(T, P) * inv_det;
 
297
 
 
298
  //The intersection lies outside of the triangle
 
299
  if(u < 0.f || u > 1.f) return false;
 
300
 
 
301
  //Prepare to test v parameter
 
302
  Q = CROSS(T, e1);
 
303
 
 
304
  //Calculate V parameter and test bound
 
305
  v = DOT(D, Q) * inv_det;
 
306
 
 
307
  //The intersection lies outside of the triangle
 
308
  if(v < 0.f || u + v  > 1.f) return false;
 
309
 
 
310
  t = DOT(e2, Q) * inv_det;
 
311
 
 
312
  if(t > EPSILON) { //ray intersection
 
313
    *out = t;
 
314
    return true;
 
315
  }
 
316
 
 
317
  // No hit, no win
 
318
  return false;
 
319
}