├── CMakeLists.txt
├── README.md
├── TestSolvePnp.cpp
└── epnp
    ├── Makefile
    ├── epnp.cpp
    ├── epnp.h
    └── main.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | set(CMAKE_CXX_STANDARD 11)
 3 | 
 4 | set(opencv_3_1_path add_path_to_your_opencv_specific_install_if_needed)
 5 | 
 6 | project( DisplayImage )
 7 | find_package( OpenCV REQUIRED NO_DEFAULT_PATH HINTS ${opencv_3_1_path} )
 8 | #find_package( OpenCV REQUIRED)
 9 | add_executable( TestSolvePnp TestSolvePnp.cpp epnp/epnp.cpp)
10 | target_link_libraries( TestSolvePnp ${OpenCV_LIBS} )
11 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # TestSolvePnp
 2 | 
 3 | A simple playground in order to test various pnp (3D pose estimation) methods with a given
 4 | configuration.
 5 | 
 6 | This code is related to the following conversations:
 7 | * http://answers.opencv.org/question/87546/solvepnp-fails-with-perfect-coordinates-and-cvposit-passes/
 8 | * https://github.com/Itseez/opencv/issues/6117
 9 | 
10 | Several methods are proposed
11 | ```
12 |   enum SolvePnpStrategy
13 |   {
14 |     Strategy_MySolvePnp_Epnp, //an home baked adapter of epnp using the epnp library source code
15 |     Strategy_MySolvePnpPosit, //an home baked adapter of cvPOSIT (deprecated "OpenCV 1" pose estimation method)
16 |     Strategy_solvePnp_P3p,    // opencv SOLVEPNP_P3P method
17 |     Strategy_solvePnp_Iterative_InitialGuess, // opencv SOLVEPNP_ITERATIVE method with an initial guess
18 |     Strategy_solvePnp_Epnp //opencv SOLVEPNP_EPNP method
19 |   };
20 | ```
21 | 
22 | Based on my experimentations,
23 | 
24 | 
25 | The order of the 3d points and image points *does* matter
26 | 
27 | It has to be adapted depending upon the strategy !
28 | 
29 | * With opencv's SOLVEPNP_EPNP the error can go down to 23.03 pixel.
30 |   The order of the points does matter
31 | 
32 | * With MySolvePnpEpnp (an home baked adapter of epnp using the epnp library source code),
33 |   the error is about 6.742 pixels, and the order *is* important
34 |   It is strange that this "rewrite" gives different results
35 | 
36 | * With MySolvePnpPosit, the error is about 4.911 pixels
37 |    and the order *is* important (in other cases the reprojection error is about 1278 pixels !)
38 | 
39 | * With solvePnp_P3p (cv::SOLVEPNP_P3P) the error is about 0.02961 pixels and the order does not matter much
40 | 
41 | * With solvePnp_Iterative_InitialGuess (cv::SOLVEPNP_ITERATIVE) the error can be 0 pixels
42 |   *if a good initial extrinsic guess is given* (otherwise don't hope for any convergence).
43 |   The order does not matter much with SOLVEPNP_ITERATIVE.
44 | 


--------------------------------------------------------------------------------
/TestSolvePnp.cpp:
--------------------------------------------------------------------------------
  1 | #include <opencv2/core/core.hpp>
  2 | #include <opencv2/calib3d/calib3d.hpp>
  3 | #include <iostream>
  4 | #include <iomanip>
  5 | #include "epnp/epnp.h"
  6 | 
  7 | 
  8 | //
  9 | // Utility functions
 10 | //
 11 | void printMat(const std::string & name, const cv::Mat & mat)
 12 | {
 13 |   std::cout << name << std::endl;
 14 |   const int precision = 4;
 15 |   for(int i=0; i<mat.size().height; i++)
 16 |   {
 17 |     std::cout << "[";
 18 |     for(int j=0; j<mat.size().width; j++)
 19 |     {
 20 |       std::cout << std::setprecision(precision) << mat.at<double>(i,j);
 21 |       if(j != mat.size().width-1)
 22 |         std::cout << ", ";
 23 |       else
 24 |         std::cout << "]" << std::endl;
 25 |     }
 26 |   }
 27 | }
 28 | 
 29 | cv::Point2f ImageCoordsToIdealCameraCoords(const cv::Mat_<double> & cameraIntrinsicParams, const cv::Point2f & pt)
 30 | {
 31 |   return cv::Point2f(
 32 |     ( pt.x - cameraIntrinsicParams.at<double>(0,2)  ) / cameraIntrinsicParams.at<double>(0,0),
 33 |     ( pt.y - cameraIntrinsicParams.at<double>(1,2)  ) / cameraIntrinsicParams.at<double>(1,1) );
 34 | }
 35 | 
 36 | cv::Point2f IdealCameraCoordsToImageCoords(const cv::Mat_<double> & cameraIntrinsicParams, const cv::Point2f & pt)
 37 | {
 38 |   return cv::Point2f(
 39 |     pt.x * cameraIntrinsicParams.at<double>(0,0) + cameraIntrinsicParams.at<double>(0,2),
 40 |     pt.y * cameraIntrinsicParams.at<double>(1,1) + cameraIntrinsicParams.at<double>(1,2)  );
 41 | }
 42 | 
 43 | 
 44 | //cv::undistortPoints is quite tricky by default : it takes image coords as input and return ideal camera coords !
 45 | //(the opencv documentation does not agree with the source code here...)
 46 | cv::Point2f UnDistortPoint_ImageCoords(const cv::Point2f & pt, const cv::Mat_<double> & cameraIntrinsicParams, const std::vector<double> & distCoeffs)
 47 | {
 48 |   std::vector<cv::Point2f> src, dst;
 49 |   src.push_back(pt);
 50 |   cv::fisheye::undistortPoints(src, dst, cameraIntrinsicParams, distCoeffs);
 51 |   cv::Point2f pt_undistorted = IdealCameraCoordsToImageCoords(cameraIntrinsicParams, dst[0]);
 52 |   return pt_undistorted;
 53 | }
 54 | 
 55 | std::vector<cv::Point2f> UnDistortPoints_ImageCoords(const std::vector<cv::Point2f> & points, const cv::Mat_<double> & cameraIntrinsicParams, const std::vector<double> & distCoeffs)
 56 | {
 57 |   std::vector<cv::Point2f> points_ideal_undistorted(points.size());
 58 |   cv::fisheye::undistortPoints(points, points_ideal_undistorted, cameraIntrinsicParams, distCoeffs);
 59 | 
 60 |   std::vector<cv::Point2f> points_undistorted;
 61 |   for (const auto & pt_ideal_undistorted : points_ideal_undistorted)
 62 |     points_undistorted.push_back( IdealCameraCoordsToImageCoords(cameraIntrinsicParams, pt_ideal_undistorted) );
 63 | 
 64 |   return points_undistorted;
 65 | }
 66 | 
 67 | 
 68 | 
 69 | //
 70 | //  <MySolvePnpEpnp> : tries reproduces the expected behavior of solvePnp with the original source code of the Epnp library
 71 | //
 72 | void MySolvePnpEpnp(
 73 |   const std::vector<cv::Point3f> &objectPoints,
 74 |   const std::vector<cv::Point2f> &imagePoints,
 75 |   const cv::Mat_<double> &cameraIntrinsicParams,
 76 |   const std::vector<double> &distCoeffs,
 77 |   cv::Mat_<double> &outRotationEstimated,
 78 |   cv::Mat_<double> &outTranslationEstimated)
 79 | {
 80 |   std::vector<cv::Point2f> imagePoints_undistorted = UnDistortPoints_ImageCoords(imagePoints, cameraIntrinsicParams, distCoeffs);
 81 | 
 82 |   epnp epnpCaller;
 83 | 
 84 |   epnpCaller.set_internal_parameters(
 85 |     cameraIntrinsicParams.at<double>(0, 2),
 86 |     cameraIntrinsicParams.at<double>(1, 2),
 87 |     cameraIntrinsicParams.at<double>(0, 0),
 88 |     cameraIntrinsicParams.at<double>(1, 1)
 89 |   );
 90 |   epnpCaller.set_maximum_number_of_correspondences(objectPoints.size());
 91 |   epnpCaller.reset_correspondences();
 92 | 
 93 |   for (size_t i = 0; i < objectPoints.size(); i++)
 94 |   {
 95 |     epnpCaller.add_correspondence(
 96 |       objectPoints[i].x, objectPoints[i].y, objectPoints[i].z,
 97 |       imagePoints_undistorted[i].x, imagePoints_undistorted[i].y
 98 |     );
 99 |   }
100 |   double rotationArray[3][3];
101 |   double translationArray[3];
102 | 
103 |   epnpCaller.compute_pose(rotationArray, translationArray);
104 | 
105 |   cv::Mat_<double> rotation3x3(cv::Size(3, 3));
106 |   for (int i = 0; i < 3; i++)
107 |     for (int j = 0; j < 3; j++)
108 |       rotation3x3(j,i) = rotationArray[j][i];
109 |   outRotationEstimated = cv::Mat_<double>(cv::Size(3, 1));
110 | 
111 |   outTranslationEstimated = cv::Mat_<double>(cv::Size(1, 3));
112 |   for (int j = 0; j < 3; j++)
113 |     outTranslationEstimated(j, 0) = translationArray[j];
114 | 
115 |   cv::Rodrigues(rotation3x3, outRotationEstimated);
116 | }
117 | //  </MySolvePnpEpnp>
118 | 
119 | 
120 | //
121 | //  MySolvePnpPosit : adapter for cvPOSIT with a C++ prototype close to solvePnp
122 | //
123 | 
124 | namespace
125 | {
126 |   //cvPOSIT wrapper
127 |   void Posit_IdealCameraCoords(const std::vector<cv::Point3f> & objectPoints, const std::vector<cv::Point2f> & imagePoints,
128 |                                cv::Mat_<double> &outRotationEstimated, cv::Mat_<double> & outTranslationEstimated)
129 |   {
130 | 
131 |     CvPoint2D32f * imagePoints_c = (CvPoint2D32f *) malloc(sizeof(CvPoint2D32f) * imagePoints.size());
132 |     {
133 |       for (size_t i = 0; i < imagePoints.size(); i++)
134 |         imagePoints_c[i] = cvPoint2D32f(imagePoints[i].x, imagePoints[i].y);
135 |     }
136 | 
137 |     CvPoint3D32f * objectPoints_c = (CvPoint3D32f *) malloc(sizeof(CvPoint3D32f) * objectPoints.size());
138 |     {
139 |       for (size_t i = 0; i < objectPoints.size(); i++)
140 |         objectPoints_c[i] = cvPoint3D32f(objectPoints[i].x, objectPoints[i].y, objectPoints[i].z);
141 |     }
142 | 
143 |     CvPOSITObject * positObject = cvCreatePOSITObject(objectPoints_c, objectPoints.size() );
144 | 
145 | 
146 |     CvTermCriteria criteria;
147 |     criteria.type = CV_TERMCRIT_EPS|CV_TERMCRIT_ITER;
148 |     criteria.epsilon = 0.00000000000000010;
149 |     criteria.max_iter = 30;
150 |     //criteria.epsilon = 0.10;
151 |     //criteria.max_iter = 6;
152 | 
153 |     float positTranslationArray[3];
154 |     float positRotationArray[9];
155 | 
156 |     const double idealFocal = 1.;
157 |     cvPOSIT(positObject, imagePoints_c,
158 |             idealFocal, criteria,
159 |             positRotationArray, positTranslationArray);
160 | 
161 | 
162 |     cv::Mat_<double> positRotationMat1x3;
163 |     {
164 |       cv::Mat_<double> positRotationMat3x3(cv::Size(3, 3));
165 |       {
166 |         int idx = 0;
167 |         for (int j = 0; j < 3; j++)
168 |         {
169 |           for (int i = 0; i < 3; i++)
170 |           {
171 |             positRotationMat3x3(j, i) = (double)positRotationArray[idx++];
172 |           }
173 |         }
174 |       }
175 |       cv::Rodrigues(positRotationMat3x3, positRotationMat1x3);
176 |     }
177 |     outRotationEstimated = positRotationMat1x3;
178 | 
179 |     outTranslationEstimated = cv::Mat_<double>(cv::Size(1, 3));
180 |     for (int i = 0; i < 3; i++)
181 |       outTranslationEstimated.at<double>(i, 0) = (double)positTranslationArray[i];
182 | 
183 |     cvReleasePOSITObject(&positObject);
184 |     free(imagePoints_c);
185 |     free(objectPoints_c);
186 |   }
187 | }
188 | 
189 | 
190 | 
191 | // MySolvePnpPosit implementation
192 | void MySolvePnpPosit(const std::vector<cv::Point3f> &objectPoints, const std::vector<cv::Point2f> &imagePoints,
193 |                      const cv::Mat_<double> &cameraIntrinsicParams, const std::vector<double> &distorsionCoeffs,
194 |                      cv::Mat_<double> &outRotationEstimated, cv::Mat_<double> &outTranslationEstimated)
195 | {
196 |   std::vector<cv::Point2f> imagePoints_IdealCameraCoords_undistorted;
197 |   cv::fisheye::undistortPoints(imagePoints, imagePoints_IdealCameraCoords_undistorted, cameraIntrinsicParams, distorsionCoeffs);
198 | 
199 |   Posit_IdealCameraCoords(objectPoints, imagePoints_IdealCameraCoords_undistorted, outRotationEstimated, outTranslationEstimated);
200 | }
201 | 
202 | 
203 | 
204 | 
205 | 
206 | 
207 | void TestSolvePnp()
208 | {
209 |   enum SolvePnpStrategy
210 |   {
211 |     Strategy_MySolvePnp_Epnp,
212 |     Strategy_MySolvePnpPosit,
213 |     Strategy_solvePnp_P3p,
214 |     Strategy_solvePnp_Iterative_InitialGuess,
215 |     Strategy_solvePnp_Epnp
216 |   };
217 |   SolvePnpStrategy strategy = Strategy_MySolvePnp_Epnp;
218 |   //SolvePnpStrategy strategy = Strategy_MySolvePnpPosit;
219 |   //SolvePnpStrategy strategy = Strategy_solvePnp_P3p;
220 |   //SolvePnpStrategy  strategy = Strategy_solvePnp_Iterative_InitialGuess;
221 |   //SolvePnpStrategy  strategy = Strategy_solvePnp_Epnp;
222 | 
223 | 
224 |   std::vector<cv::Point3f> objectPoints;
225 | 
226 |   // Based on my experimentations
227 |   // The order of the 3d points and image points *does* matter
228 |   // It has to be adapted depending upon the strategy !
229 | 
230 |   // With opencv's SOLVEPNP_EPNP the error can go down to 23.03 pixel with the following order.
231 |   //the order of the points does matter
232 |   if (strategy == Strategy_solvePnp_Epnp)
233 |   {
234 |     objectPoints.push_back(cv::Point3f(-62.1225319f, 15.7540569f, 0.819464564f));
235 |     objectPoints.push_back(cv::Point3f(62.3174629f, 15.7940502f, 0.819983721f));
236 |     objectPoints.push_back(cv::Point3f(-0.372639507f, 16.4230633f,  -36.5060043f));
237 |     objectPoints.push_back(cv::Point3f(0.f, 0.f, 0.f));
238 |   }
239 | 
240 |   // With MySolvePnpEpnp (an home baked adapter of epnp using the epnp library source code),
241 |   // the error is about 6.742 pixels, and the order *is* important
242 |   // It is strange that this "rewrite" gives different results
243 |   else if (strategy == Strategy_MySolvePnp_Epnp)
244 |   {
245 |     objectPoints.push_back(cv::Point3f(-62.1225319f, 15.7540569f, 0.819464564f));
246 |     objectPoints.push_back(cv::Point3f(62.3174629f, 15.7940502f, 0.819983721f));
247 |     objectPoints.push_back(cv::Point3f(0.f, 0.f, 0.f));
248 |     objectPoints.push_back(cv::Point3f(-0.372639507f, 16.4230633f,  -36.5060043f));
249 |   }
250 | 
251 |   // With MySolvePnpPosit, the error is about 4.911 pixels
252 |   // and the order *is* important (in other cases the reprojection error is about 1278 pixels !)
253 |   else if (strategy == Strategy_MySolvePnpPosit)
254 |   {
255 |     objectPoints.push_back(cv::Point3f(0.f, 0.f, 0.f));
256 |     objectPoints.push_back(cv::Point3f(-62.1225319f, 15.7540569f, 0.819464564f));
257 |     objectPoints.push_back(cv::Point3f(62.3174629f, 15.7940502f, 0.819983721f));
258 |     objectPoints.push_back(cv::Point3f(-0.372639507f, 16.4230633f,  -36.5060043f));
259 |   }
260 | 
261 |   // With solvePnp_P3p (cv::SOLVEPNP_P3P) the error is about 0.02961 pixels and the order does not matter much
262 |   else if (strategy == Strategy_solvePnp_P3p)
263 |   {
264 |     objectPoints.push_back(cv::Point3f(0.f, 0.f, 0.f));
265 |     objectPoints.push_back(cv::Point3f(-62.1225319f, 15.7540569f, 0.819464564f));
266 |     objectPoints.push_back(cv::Point3f(62.3174629f, 15.7940502f, 0.819983721f));
267 |     objectPoints.push_back(cv::Point3f(-0.372639507f, 16.4230633f,  -36.5060043f));
268 |   }
269 | 
270 |   // With solvePnp_P3p (cv::SOLVEPNP_ITERATIVE) the error can be 0 pixels
271 |   // *if a good initial extrinsic guess is given* (otherwise don't hope for any convergence)
272 |   // the order does not matter much
273 |   else if (strategy == Strategy_solvePnp_Iterative_InitialGuess)
274 |   {
275 |     objectPoints.push_back(cv::Point3f(0.f, 0.f, 0.f));
276 |     objectPoints.push_back(cv::Point3f(-62.1225319f, 15.7540569f, 0.819464564f));
277 |     objectPoints.push_back(cv::Point3f(62.3174629f, 15.7940502f, 0.819983721f));
278 |     objectPoints.push_back(cv::Point3f(-0.372639507f, 16.4230633f,  -36.5060043f));
279 |   }
280 | 
281 |   cv::Mat_<double> cameraIntrinsicParams(cv::Size(3, 3));
282 |   cameraIntrinsicParams = 0.;
283 |   cameraIntrinsicParams(0, 0) = 3844.4400000000001f;
284 |   cameraIntrinsicParams(1, 1) = 3841.0599999999999f;
285 |   cameraIntrinsicParams(0, 2) = 640.f;
286 |   cameraIntrinsicParams(1, 2) = 380.f;
287 |   cameraIntrinsicParams(2, 2) = 1.f;
288 | 
289 |   std::vector<double> distCoeffs(4);
290 |   distCoeffs[0] = -0.063500002026557922;
291 |   distCoeffs[1] = -2.5915000438690186;
292 |   distCoeffs[2] = -0.0023300000466406345;
293 |   distCoeffs[3] = 0.0008411200251430273;
294 | 
295 | 
296 |   cv::Mat_<double> rotation(cv::Size(1, 3));
297 |   rotation(0,0) = 0.07015543380659847f;
298 |   rotation(0,1) = 0.06922079477774973f;
299 |   rotation(0,2) = -0.00254676088325f;
300 | 
301 |   cv::Mat_<double> translation(cv::Size(1, 3));
302 |   translation(0,0) = -35.3236f;
303 |   translation(0,1) = -48.1699f;
304 |   translation(0,2) = 769.068f;
305 | 
306 |   std::vector<cv::Point2f> imagePoints;
307 |   cv::projectPoints(objectPoints, rotation, translation, cameraIntrinsicParams, distCoeffs, imagePoints);
308 | 
309 | 
310 |   cv::Mat_<double> rotation2(cv::Size(1, 3));
311 |   cv::Mat_<double> translation2(cv::Size(1, 3));
312 |   rotation2.setTo(0.);
313 |   translation2.setTo(0.);
314 |   translation2(2, 0) = 600.; //Hint for SOLVEPNP_ITERATIVE
315 | 
316 | 
317 |   switch(strategy)
318 |   {
319 |     case Strategy_MySolvePnp_Epnp:
320 |       MySolvePnpEpnp(
321 |         objectPoints,
322 |         imagePoints,
323 |         cameraIntrinsicParams,
324 |         distCoeffs,
325 |         rotation2,
326 |         translation2);
327 |       break;
328 |     case Strategy_MySolvePnpPosit:
329 |       MySolvePnpPosit(
330 |         objectPoints,
331 |         imagePoints,
332 |         cameraIntrinsicParams,
333 |         distCoeffs,
334 |         rotation2,
335 |         translation2);
336 |       break;
337 |     case Strategy_solvePnp_P3p:
338 |       cv::solvePnP(objectPoints, imagePoints,
339 |                    cameraIntrinsicParams, distCoeffs,
340 |                    rotation2, translation2,
341 |                    false,//useExtrinsicGuess
342 |                    cv::SOLVEPNP_P3P
343 |       );
344 |       break;
345 |     case Strategy_solvePnp_Iterative_InitialGuess:
346 |       cv::solvePnP(objectPoints, imagePoints,
347 |                    cameraIntrinsicParams, distCoeffs,
348 |                    rotation2, translation2,
349 |                    true,//useExtrinsicGuess
350 |                    cv::SOLVEPNP_ITERATIVE
351 |       );
352 |       break;
353 |     case Strategy_solvePnp_Epnp:
354 |       cv::solvePnP(objectPoints, imagePoints,
355 |                    cameraIntrinsicParams, distCoeffs,
356 |                    rotation2, translation2,
357 |                    true,//useExtrinsicGuess
358 |                    cv::SOLVEPNP_EPNP
359 |       );
360 |       break;
361 |   }
362 | 
363 | 
364 | 
365 |   std::vector<cv::Point2f> imagePoints_Reproj(3);
366 |   cv::projectPoints(objectPoints, rotation2, translation2, cameraIntrinsicParams, distCoeffs, imagePoints_Reproj);
367 | 
368 |   float sum = 0.;
369 |   for (size_t i = 0; i < imagePoints.size(); i++)
370 |     sum += cv::norm(imagePoints_Reproj[i] - imagePoints[i]);
371 | 
372 | 
373 |   printMat("rotation", rotation);
374 |   printMat("rotation2", rotation2);
375 |   printMat("translation", translation);
376 |   printMat("translation2", translation2);
377 |   std::cout << "Reproj Error=" << sum << std::endl;
378 | }
379 | 
380 | int main(int argc, char **argv)
381 | {
382 |   TestSolvePnp();
383 | }
384 | 


--------------------------------------------------------------------------------
/epnp/Makefile:
--------------------------------------------------------------------------------
  1 | EXECUTABLE_1_NAME = epnp
  2 | SOURCES_1 = main epnp
  3 | 
  4 | CC = g++-4
  5 | LINK = g++-4
  6 | 
  7 | #CC = icpc
  8 | #LINK = icpc
  9 | 
 10 | DEBUG_OPTIM = -g -O3 -ffast-math -fomit-frame-pointer
 11 | WARNINGS = -Wall -Wextra
 12 | 
 13 | BAZAR_INC = `pkg-config starter --cflags` `pkg-config garfeild --cflags`
 14 | BIZAR_INC = `pkg-config garfield --cflags`
 15 | OPENCV_INC = `pkg-config opencv --cflags`
 16 | # CLAPACK_INC = -I/System/Library/Frameworks/vecLib.framework/Versions/A/Headers/
 17 | 
 18 | ALL_LIBS_INCLUDE = $(BAZAR_INC) $(BIZAR_INC) $(OPENCV_INC) $(CLAPACK_INC)
 19 | 
 20 | BAZAR_LIB = `pkg-config starter --libs` `pkg-config garfeild --libs`
 21 | BIZAR_LIB = `pkg-config garfield --libs`
 22 | OPENCV_LIB = `pkg-config opencv --libs`
 23 | # CLAPACK_LIB = -lblas -llapack
 24 | OTHER_LIBS =
 25 | 
 26 | ALL_LIBS_LIB = $(BAZAR_LIB) $(BIZAR_LIB) $(OPENCV_LIB) $(CLAPACK_LIB) $(OTHER_LIBS)
 27 | 
 28 | CC_OPTIONS = $(WARNINGS) $(DEBUG_OPTIM) $(ALL_LIBS_INCLUDE)
 29 | LINK_OPTIONS = $(ALL_LIBS_LIB)
 30 | 
 31 | 
 32 | all: $(EXECUTABLE_1_NAME) $(EXECUTABLE_2_NAME) $(EXECUTABLE_3_NAME) $(EXECUTABLE_4_NAME) $(EXECUTABLE_5_NAME) $(EXECUTABLE_6_NAME) $(EXECUTABLE_7_NAME) $(EXECUTABLE_8_NAME) $(EXECUTABLE_9_NAME)
 33 | 
 34 | SOURCES_1_CPP = $(SOURCES_1:=.cpp)
 35 | SOURCES_1_OBJ = $(SOURCES_1:=.o)
 36 | 
 37 | SOURCES_2_CPP = $(SOURCES_2:=.cpp)
 38 | SOURCES_2_OBJ = $(SOURCES_2:=.o)
 39 | 
 40 | SOURCES_3_CPP = $(SOURCES_3:=.cpp)
 41 | SOURCES_3_OBJ = $(SOURCES_3:=.o)
 42 | 
 43 | SOURCES_4_CPP = $(SOURCES_4:=.cpp)
 44 | SOURCES_4_OBJ = $(SOURCES_4:=.o)
 45 | 
 46 | SOURCES_5_CPP = $(SOURCES_5:=.cpp)
 47 | SOURCES_5_OBJ = $(SOURCES_5:=.o)
 48 | 
 49 | SOURCES_6_CPP = $(SOURCES_6:=.cpp)
 50 | SOURCES_6_OBJ = $(SOURCES_6:=.o)
 51 | 
 52 | SOURCES_7_CPP = $(SOURCES_7:=.cpp)
 53 | SOURCES_7_OBJ = $(SOURCES_7:=.o)
 54 | 
 55 | SOURCES_8_CPP = $(SOURCES_8:=.cpp)
 56 | SOURCES_8_OBJ = $(SOURCES_8:=.o)
 57 | 
 58 | SOURCES_9_CPP = $(SOURCES_9:=.cpp)
 59 | SOURCES_9_OBJ = $(SOURCES_9:=.o)
 60 | 
 61 | 
 62 | $(EXECUTABLE_1_NAME): $(SOURCES_1_OBJ)
 63 | 	$(LINK) $(SOURCES_1_OBJ) $(LINK_OPTIONS) -o $(EXECUTABLE_1_NAME)
 64 | 
 65 | $(EXECUTABLE_2_NAME): $(SOURCES_2_OBJ)
 66 | 	$(LINK) $(SOURCES_2_OBJ) $(LINK_OPTIONS) -o $(EXECUTABLE_2_NAME)
 67 | 
 68 | $(EXECUTABLE_3_NAME): $(SOURCES_3_OBJ)
 69 | 	$(LINK) $(SOURCES_3_OBJ) $(LINK_OPTIONS) -o $(EXECUTABLE_3_NAME)
 70 | 
 71 | $(EXECUTABLE_4_NAME): $(SOURCES_4_OBJ)
 72 | 	$(LINK) $(SOURCES_4_OBJ) $(LINK_OPTIONS) -o $(EXECUTABLE_4_NAME)
 73 | 
 74 | $(EXECUTABLE_5_NAME): $(SOURCES_5_OBJ) $(GARFIELD_LIB)
 75 | 	$(LINK) $(SOURCES_5_OBJ) $(LINK_OPTIONS) -o $(EXECUTABLE_5_NAME)
 76 | 
 77 | $(EXECUTABLE_6_NAME): $(SOURCES_6_OBJ) $(GARFIELD_LIB)
 78 | 	$(LINK) $(SOURCES_6_OBJ) $(LINK_OPTIONS) -o $(EXECUTABLE_6_NAME)
 79 | 
 80 | $(EXECUTABLE_7_NAME): $(SOURCES_7_OBJ) $(GARFIELD_LIB)
 81 | 	$(LINK) $(SOURCES_7_OBJ) $(LINK_OPTIONS) -o $(EXECUTABLE_7_NAME)
 82 | 
 83 | $(EXECUTABLE_8_NAME): $(SOURCES_8_OBJ) $(GARFIELD_LIB)
 84 | 	$(LINK) $(SOURCES_8_OBJ) $(LINK_OPTIONS) -o $(EXECUTABLE_8_NAME)
 85 | 
 86 | $(EXECUTABLE_9_NAME): $(SOURCES_9_OBJ) $(GARFIELD_LIB)
 87 | 	$(LINK) $(SOURCES_9_OBJ) $(LINK_OPTIONS) -o $(EXECUTABLE_9_NAME)
 88 | 
 89 | .cpp.o:
 90 | 	$(CC) $(CC_OPTIONS) -c $*.cpp
 91 | 
 92 | dep: depend.inc
 93 | depend.inc:
 94 | 	g++ -MM $(ALL_LIBS_INCLUDE) $(SOURCES_1_CPP) $(SOURCES_2_CPP) $(SOURCES_3_CPP) $(SOURCES_4_CPP) $(SOURCES_5_CPP) $(SOURCES_6_CPP) $(SOURCES_7_CPP) $(SOURCES_8_CPP) $(SOURCES_9_CPP) > depend.inc
 95 | 
 96 | clean:
 97 | 	@rm -f *~ *.o depend.inc
 98 | 
 99 | include depend.inc
100 | 


--------------------------------------------------------------------------------
/epnp/epnp.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright (c) 2009, V. Lepetit, EPFL
  2 | // All rights reserved.
  3 | 
  4 | // Redistribution and use in source and binary forms, with or without
  5 | // modification, are permitted provided that the following conditions are met: 
  6 | 
  7 | // 1. Redistributions of source code must retain the above copyright notice, this
  8 | //    list of conditions and the following disclaimer. 
  9 | // 2. Redistributions in binary form must reproduce the above copyright notice,
 10 | //    this list of conditions and the following disclaimer in the documentation
 11 | //    and/or other materials provided with the distribution. 
 12 | 
 13 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 14 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 15 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 16 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 17 | // ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 18 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 19 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 20 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 21 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 22 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 23 | 
 24 | // The views and conclusions contained in the software and documentation are those
 25 | // of the authors and should not be interpreted as representing official policies, 
 26 | //   either expressed or implied, of the FreeBSD Project.
 27 | 
 28 | #include <iostream>
 29 | using namespace std;
 30 | 
 31 | #include "epnp.h"
 32 | 
 33 | epnp::epnp(void)
 34 | {
 35 |   maximum_number_of_correspondences = 0;
 36 |   number_of_correspondences = 0;
 37 | 
 38 |   pws = 0;
 39 |   us = 0;
 40 |   alphas = 0;
 41 |   pcs = 0;
 42 | }
 43 | 
 44 | epnp::~epnp()
 45 | {
 46 |   delete [] pws;
 47 |   delete [] us;
 48 |   delete [] alphas;
 49 |   delete [] pcs;
 50 | }
 51 | 
 52 | void epnp::set_internal_parameters(double uc, double vc, double fu, double fv)
 53 | {
 54 |   this->uc = uc;
 55 |   this->vc = vc;
 56 |   this->fu = fu;
 57 |   this->fv = fv;
 58 | }
 59 | 
 60 | void epnp::set_maximum_number_of_correspondences(int n)
 61 | {
 62 |   if (maximum_number_of_correspondences < n) {
 63 |     if (pws != 0) delete [] pws;
 64 |     if (us != 0) delete [] us;
 65 |     if (alphas != 0) delete [] alphas;
 66 |     if (pcs != 0) delete [] pcs;
 67 | 
 68 |     maximum_number_of_correspondences = n;
 69 |     pws = new double[3 * maximum_number_of_correspondences];
 70 |     us = new double[2 * maximum_number_of_correspondences];
 71 |     alphas = new double[4 * maximum_number_of_correspondences];
 72 |     pcs = new double[3 * maximum_number_of_correspondences];
 73 |   }
 74 | }
 75 | 
 76 | void epnp::reset_correspondences(void)
 77 | {
 78 |   number_of_correspondences = 0;
 79 | }
 80 | 
 81 | void epnp::add_correspondence(double X, double Y, double Z, double u, double v)
 82 | {
 83 |   pws[3 * number_of_correspondences    ] = X;
 84 |   pws[3 * number_of_correspondences + 1] = Y;
 85 |   pws[3 * number_of_correspondences + 2] = Z;
 86 | 
 87 |   us[2 * number_of_correspondences    ] = u;
 88 |   us[2 * number_of_correspondences + 1] = v;
 89 | 
 90 |   number_of_correspondences++;
 91 | }
 92 | 
 93 | void epnp::choose_control_points(void)
 94 | {
 95 |   // Take C0 as the reference points centroid:
 96 |   cws[0][0] = cws[0][1] = cws[0][2] = 0;
 97 |   for(int i = 0; i < number_of_correspondences; i++)
 98 |     for(int j = 0; j < 3; j++)
 99 |       cws[0][j] += pws[3 * i + j];
100 | 
101 |   for(int j = 0; j < 3; j++)
102 |     cws[0][j] /= number_of_correspondences;
103 | 
104 | 
105 |   // Take C1, C2, and C3 from PCA on the reference points:
106 |   CvMat * PW0 = cvCreateMat(number_of_correspondences, 3, CV_64F);
107 | 
108 |   double pw0tpw0[3 * 3], dc[3], uct[3 * 3];
109 |   CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0);
110 |   CvMat DC      = cvMat(3, 1, CV_64F, dc);
111 |   CvMat UCt     = cvMat(3, 3, CV_64F, uct);
112 | 
113 |   for(int i = 0; i < number_of_correspondences; i++)
114 |     for(int j = 0; j < 3; j++)
115 |       PW0->data.db[3 * i + j] = pws[3 * i + j] - cws[0][j];
116 | 
117 |   cvMulTransposed(PW0, &PW0tPW0, 1);
118 |   cvSVD(&PW0tPW0, &DC, &UCt, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
119 | 
120 |   cvReleaseMat(&PW0);
121 | 
122 |   for(int i = 1; i < 4; i++) {
123 |     double k = sqrt(dc[i - 1] / number_of_correspondences);
124 |     for(int j = 0; j < 3; j++)
125 |       cws[i][j] = cws[0][j] + k * uct[3 * (i - 1) + j];
126 |   }
127 | }
128 | 
129 | void epnp::compute_barycentric_coordinates(void)
130 | {
131 |   double cc[3 * 3], cc_inv[3 * 3];
132 |   CvMat CC     = cvMat(3, 3, CV_64F, cc);
133 |   CvMat CC_inv = cvMat(3, 3, CV_64F, cc_inv);
134 | 
135 |   for(int i = 0; i < 3; i++)
136 |     for(int j = 1; j < 4; j++)
137 |       cc[3 * i + j - 1] = cws[j][i] - cws[0][i];
138 | 
139 |   cvInvert(&CC, &CC_inv, CV_SVD);
140 |   double * ci = cc_inv;
141 |   for(int i = 0; i < number_of_correspondences; i++) {
142 |     double * pi = pws + 3 * i;
143 |     double * a = alphas + 4 * i;
144 | 
145 |     for(int j = 0; j < 3; j++)
146 |       a[1 + j] =
147 | 	ci[3 * j    ] * (pi[0] - cws[0][0]) +
148 | 	ci[3 * j + 1] * (pi[1] - cws[0][1]) +
149 | 	ci[3 * j + 2] * (pi[2] - cws[0][2]);
150 |     a[0] = 1.0f - a[1] - a[2] - a[3];
151 |   }
152 | }
153 | 
154 | void epnp::fill_M(CvMat * M,
155 | 		  const int row, const double * as, const double u, const double v)
156 | {
157 |   double * M1 = M->data.db + row * 12;
158 |   double * M2 = M1 + 12;
159 | 
160 |   for(int i = 0; i < 4; i++) {
161 |     M1[3 * i    ] = as[i] * fu;
162 |     M1[3 * i + 1] = 0.0;
163 |     M1[3 * i + 2] = as[i] * (uc - u);
164 | 
165 |     M2[3 * i    ] = 0.0;
166 |     M2[3 * i + 1] = as[i] * fv;
167 |     M2[3 * i + 2] = as[i] * (vc - v);
168 |   }
169 | }
170 | 
171 | void epnp::compute_ccs(const double * betas, const double * ut)
172 | {
173 |   for(int i = 0; i < 4; i++)
174 |     ccs[i][0] = ccs[i][1] = ccs[i][2] = 0.0f;
175 | 
176 |   for(int i = 0; i < 4; i++) {
177 |     const double * v = ut + 12 * (11 - i);
178 |     for(int j = 0; j < 4; j++)
179 |       for(int k = 0; k < 3; k++)
180 | 	ccs[j][k] += betas[i] * v[3 * j + k];
181 |   }
182 | }
183 | 
184 | void epnp::compute_pcs(void)
185 | {
186 |   for(int i = 0; i < number_of_correspondences; i++) {
187 |     double * a = alphas + 4 * i;
188 |     double * pc = pcs + 3 * i;
189 | 
190 |     for(int j = 0; j < 3; j++)
191 |       pc[j] = a[0] * ccs[0][j] + a[1] * ccs[1][j] + a[2] * ccs[2][j] + a[3] * ccs[3][j];
192 |   }
193 | }
194 | 
195 | double epnp::compute_pose(double R[3][3], double t[3])
196 | {
197 |   choose_control_points();
198 |   compute_barycentric_coordinates();
199 | 
200 |   CvMat * M = cvCreateMat(2 * number_of_correspondences, 12, CV_64F);
201 | 
202 |   for(int i = 0; i < number_of_correspondences; i++)
203 |     fill_M(M, 2 * i, alphas + 4 * i, us[2 * i], us[2 * i + 1]);
204 | 
205 |   double mtm[12 * 12], d[12], ut[12 * 12];
206 |   CvMat MtM = cvMat(12, 12, CV_64F, mtm);
207 |   CvMat D   = cvMat(12,  1, CV_64F, d);
208 |   CvMat Ut  = cvMat(12, 12, CV_64F, ut);
209 | 
210 |   cvMulTransposed(M, &MtM, 1);
211 |   cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
212 |   cvReleaseMat(&M);
213 | 
214 |   double l_6x10[6 * 10], rho[6];
215 |   CvMat L_6x10 = cvMat(6, 10, CV_64F, l_6x10);
216 |   CvMat Rho    = cvMat(6,  1, CV_64F, rho);
217 | 
218 |   compute_L_6x10(ut, l_6x10);
219 |   compute_rho(rho);
220 | 
221 |   double Betas[4][4], rep_errors[4];
222 |   double Rs[4][3][3], ts[4][3];
223 | 
224 |   find_betas_approx_1(&L_6x10, &Rho, Betas[1]);
225 |   gauss_newton(&L_6x10, &Rho, Betas[1]);
226 |   rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]);
227 | 
228 |   find_betas_approx_2(&L_6x10, &Rho, Betas[2]);
229 |   gauss_newton(&L_6x10, &Rho, Betas[2]);
230 |   rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]);
231 | 
232 |   find_betas_approx_3(&L_6x10, &Rho, Betas[3]);
233 |   gauss_newton(&L_6x10, &Rho, Betas[3]);
234 |   rep_errors[3] = compute_R_and_t(ut, Betas[3], Rs[3], ts[3]);
235 | 
236 |   int N = 1;
237 |   if (rep_errors[2] < rep_errors[1]) N = 2;
238 |   if (rep_errors[3] < rep_errors[N]) N = 3;
239 | 
240 |   copy_R_and_t(Rs[N], ts[N], R, t);
241 | 
242 |   return rep_errors[N];
243 | }
244 | 
245 | void epnp::copy_R_and_t(const double R_src[3][3], const double t_src[3],
246 | 			double R_dst[3][3], double t_dst[3])
247 | {
248 |   for(int i = 0; i < 3; i++) {
249 |     for(int j = 0; j < 3; j++)
250 |       R_dst[i][j] = R_src[i][j];
251 |     t_dst[i] = t_src[i];
252 |   }
253 | }
254 | 
255 | double epnp::dist2(const double * p1, const double * p2)
256 | {
257 |   return
258 |     (p1[0] - p2[0]) * (p1[0] - p2[0]) +
259 |     (p1[1] - p2[1]) * (p1[1] - p2[1]) +
260 |     (p1[2] - p2[2]) * (p1[2] - p2[2]);
261 | }
262 | 
263 | double epnp::dot(const double * v1, const double * v2)
264 | {
265 |   return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
266 | }
267 | 
268 | double epnp::reprojection_error(const double R[3][3], const double t[3])
269 | {
270 |   double sum2 = 0.0;
271 | 
272 |   for(int i = 0; i < number_of_correspondences; i++) {
273 |     double * pw = pws + 3 * i;
274 |     double Xc = dot(R[0], pw) + t[0];
275 |     double Yc = dot(R[1], pw) + t[1];
276 |     double inv_Zc = 1.0 / (dot(R[2], pw) + t[2]);
277 |     double ue = uc + fu * Xc * inv_Zc;
278 |     double ve = vc + fv * Yc * inv_Zc;
279 |     double u = us[2 * i], v = us[2 * i + 1];
280 | 
281 |     sum2 += sqrt( (u - ue) * (u - ue) + (v - ve) * (v - ve) );
282 |   }
283 | 
284 |   return sum2 / number_of_correspondences;
285 | }
286 | 
287 | void epnp::estimate_R_and_t(double R[3][3], double t[3])
288 | {
289 |   double pc0[3], pw0[3];
290 | 
291 |   pc0[0] = pc0[1] = pc0[2] = 0.0;
292 |   pw0[0] = pw0[1] = pw0[2] = 0.0;
293 | 
294 |   for(int i = 0; i < number_of_correspondences; i++) {
295 |     const double * pc = pcs + 3 * i;
296 |     const double * pw = pws + 3 * i;
297 | 
298 |     for(int j = 0; j < 3; j++) {
299 |       pc0[j] += pc[j];
300 |       pw0[j] += pw[j];
301 |     }
302 |   }
303 |   for(int j = 0; j < 3; j++) {
304 |     pc0[j] /= number_of_correspondences;
305 |     pw0[j] /= number_of_correspondences;
306 |   }
307 | 
308 |   double abt[3 * 3], abt_d[3], abt_u[3 * 3], abt_v[3 * 3];
309 |   CvMat ABt   = cvMat(3, 3, CV_64F, abt);
310 |   CvMat ABt_D = cvMat(3, 1, CV_64F, abt_d);
311 |   CvMat ABt_U = cvMat(3, 3, CV_64F, abt_u);
312 |   CvMat ABt_V = cvMat(3, 3, CV_64F, abt_v);
313 | 
314 |   cvSetZero(&ABt);
315 |   for(int i = 0; i < number_of_correspondences; i++) {
316 |     double * pc = pcs + 3 * i;
317 |     double * pw = pws + 3 * i;
318 | 
319 |     for(int j = 0; j < 3; j++) {
320 |       abt[3 * j    ] += (pc[j] - pc0[j]) * (pw[0] - pw0[0]);
321 |       abt[3 * j + 1] += (pc[j] - pc0[j]) * (pw[1] - pw0[1]);
322 |       abt[3 * j + 2] += (pc[j] - pc0[j]) * (pw[2] - pw0[2]);
323 |     }
324 |   }
325 | 
326 |   cvSVD(&ABt, &ABt_D, &ABt_U, &ABt_V, CV_SVD_MODIFY_A);
327 | 
328 |   for(int i = 0; i < 3; i++)
329 |     for(int j = 0; j < 3; j++)
330 |       R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j);
331 | 
332 |   const double det =
333 |     R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] -
334 |     R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1];
335 | 
336 |   if (det < 0) {
337 |     R[2][0] = -R[2][0];
338 |     R[2][1] = -R[2][1];
339 |     R[2][2] = -R[2][2];
340 |   }
341 | 
342 |   t[0] = pc0[0] - dot(R[0], pw0);
343 |   t[1] = pc0[1] - dot(R[1], pw0);
344 |   t[2] = pc0[2] - dot(R[2], pw0);
345 | }
346 | 
347 | void epnp::print_pose(const double R[3][3], const double t[3])
348 | {
349 |   cout << R[0][0] << " " << R[0][1] << " " << R[0][2] << " " << t[0] << endl;
350 |   cout << R[1][0] << " " << R[1][1] << " " << R[1][2] << " " << t[1] << endl;
351 |   cout << R[2][0] << " " << R[2][1] << " " << R[2][2] << " " << t[2] << endl;
352 | }
353 | 
354 | void epnp::solve_for_sign(void)
355 | {
356 |   if (pcs[2] < 0.0) {
357 |     for(int i = 0; i < 4; i++)
358 |       for(int j = 0; j < 3; j++)
359 | 	ccs[i][j] = -ccs[i][j];
360 | 
361 |     for(int i = 0; i < number_of_correspondences; i++) {
362 |       pcs[3 * i    ] = -pcs[3 * i];
363 |       pcs[3 * i + 1] = -pcs[3 * i + 1];
364 |       pcs[3 * i + 2] = -pcs[3 * i + 2];
365 |     }
366 |   }
367 | }
368 | 
369 | double epnp::compute_R_and_t(const double * ut, const double * betas,
370 | 			     double R[3][3], double t[3])
371 | {
372 |   compute_ccs(betas, ut);
373 |   compute_pcs();
374 | 
375 |   solve_for_sign();
376 | 
377 |   estimate_R_and_t(R, t);
378 | 
379 |   return reprojection_error(R, t);
380 | }
381 | 
382 | // betas10        = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
383 | // betas_approx_1 = [B11 B12     B13         B14]
384 | 
385 | void epnp::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,
386 | 			       double * betas)
387 | {
388 |   double l_6x4[6 * 4], b4[4];
389 |   CvMat L_6x4 = cvMat(6, 4, CV_64F, l_6x4);
390 |   CvMat B4    = cvMat(4, 1, CV_64F, b4);
391 | 
392 |   for(int i = 0; i < 6; i++) {
393 |     cvmSet(&L_6x4, i, 0, cvmGet(L_6x10, i, 0));
394 |     cvmSet(&L_6x4, i, 1, cvmGet(L_6x10, i, 1));
395 |     cvmSet(&L_6x4, i, 2, cvmGet(L_6x10, i, 3));
396 |     cvmSet(&L_6x4, i, 3, cvmGet(L_6x10, i, 6));
397 |   }
398 | 
399 |   cvSolve(&L_6x4, Rho, &B4, CV_SVD);
400 | 
401 |   if (b4[0] < 0) {
402 |     betas[0] = sqrt(-b4[0]);
403 |     betas[1] = -b4[1] / betas[0];
404 |     betas[2] = -b4[2] / betas[0];
405 |     betas[3] = -b4[3] / betas[0];
406 |   } else {
407 |     betas[0] = sqrt(b4[0]);
408 |     betas[1] = b4[1] / betas[0];
409 |     betas[2] = b4[2] / betas[0];
410 |     betas[3] = b4[3] / betas[0];
411 |   }
412 | }
413 | 
414 | // betas10        = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
415 | // betas_approx_2 = [B11 B12 B22                            ]
416 | 
417 | void epnp::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,
418 | 			       double * betas)
419 | {
420 |   double l_6x3[6 * 3], b3[3];
421 |   CvMat L_6x3  = cvMat(6, 3, CV_64F, l_6x3);
422 |   CvMat B3     = cvMat(3, 1, CV_64F, b3);
423 | 
424 |   for(int i = 0; i < 6; i++) {
425 |     cvmSet(&L_6x3, i, 0, cvmGet(L_6x10, i, 0));
426 |     cvmSet(&L_6x3, i, 1, cvmGet(L_6x10, i, 1));
427 |     cvmSet(&L_6x3, i, 2, cvmGet(L_6x10, i, 2));
428 |   }
429 | 
430 |   cvSolve(&L_6x3, Rho, &B3, CV_SVD);
431 | 
432 |   if (b3[0] < 0) {
433 |     betas[0] = sqrt(-b3[0]);
434 |     betas[1] = (b3[2] < 0) ? sqrt(-b3[2]) : 0.0;
435 |   } else {
436 |     betas[0] = sqrt(b3[0]);
437 |     betas[1] = (b3[2] > 0) ? sqrt(b3[2]) : 0.0;
438 |   }
439 | 
440 |   if (b3[1] < 0) betas[0] = -betas[0];
441 | 
442 |   betas[2] = 0.0;
443 |   betas[3] = 0.0;
444 | }
445 | 
446 | // betas10        = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
447 | // betas_approx_3 = [B11 B12 B22 B13 B23                    ]
448 | 
449 | void epnp::find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho,
450 | 			       double * betas)
451 | {
452 |   double l_6x5[6 * 5], b5[5];
453 |   CvMat L_6x5 = cvMat(6, 5, CV_64F, l_6x5);
454 |   CvMat B5    = cvMat(5, 1, CV_64F, b5);
455 | 
456 |   for(int i = 0; i < 6; i++) {
457 |     cvmSet(&L_6x5, i, 0, cvmGet(L_6x10, i, 0));
458 |     cvmSet(&L_6x5, i, 1, cvmGet(L_6x10, i, 1));
459 |     cvmSet(&L_6x5, i, 2, cvmGet(L_6x10, i, 2));
460 |     cvmSet(&L_6x5, i, 3, cvmGet(L_6x10, i, 3));
461 |     cvmSet(&L_6x5, i, 4, cvmGet(L_6x10, i, 4));
462 |   }
463 | 
464 |   cvSolve(&L_6x5, Rho, &B5, CV_SVD);
465 | 
466 |   if (b5[0] < 0) {
467 |     betas[0] = sqrt(-b5[0]);
468 |     betas[1] = (b5[2] < 0) ? sqrt(-b5[2]) : 0.0;
469 |   } else {
470 |     betas[0] = sqrt(b5[0]);
471 |     betas[1] = (b5[2] > 0) ? sqrt(b5[2]) : 0.0;
472 |   }
473 |   if (b5[1] < 0) betas[0] = -betas[0];
474 |   betas[2] = b5[3] / betas[0];
475 |   betas[3] = 0.0;
476 | }
477 | 
478 | void epnp::compute_L_6x10(const double * ut, double * l_6x10)
479 | {
480 |   const double * v[4];
481 | 
482 |   v[0] = ut + 12 * 11;
483 |   v[1] = ut + 12 * 10;
484 |   v[2] = ut + 12 *  9;
485 |   v[3] = ut + 12 *  8;
486 | 
487 |   double dv[4][6][3];
488 | 
489 |   for(int i = 0; i < 4; i++) {
490 |     int a = 0, b = 1;
491 |     for(int j = 0; j < 6; j++) {
492 |       dv[i][j][0] = v[i][3 * a    ] - v[i][3 * b];
493 |       dv[i][j][1] = v[i][3 * a + 1] - v[i][3 * b + 1];
494 |       dv[i][j][2] = v[i][3 * a + 2] - v[i][3 * b + 2];
495 | 
496 |       b++;
497 |       if (b > 3) {
498 | 	a++;
499 | 	b = a + 1;
500 |       }
501 |     }
502 |   }
503 | 
504 |   for(int i = 0; i < 6; i++) {
505 |     double * row = l_6x10 + 10 * i;
506 | 
507 |     row[0] =        dot(dv[0][i], dv[0][i]);
508 |     row[1] = 2.0f * dot(dv[0][i], dv[1][i]);
509 |     row[2] =        dot(dv[1][i], dv[1][i]);
510 |     row[3] = 2.0f * dot(dv[0][i], dv[2][i]);
511 |     row[4] = 2.0f * dot(dv[1][i], dv[2][i]);
512 |     row[5] =        dot(dv[2][i], dv[2][i]);
513 |     row[6] = 2.0f * dot(dv[0][i], dv[3][i]);
514 |     row[7] = 2.0f * dot(dv[1][i], dv[3][i]);
515 |     row[8] = 2.0f * dot(dv[2][i], dv[3][i]);
516 |     row[9] =        dot(dv[3][i], dv[3][i]);
517 |   }
518 | }
519 | 
520 | void epnp::compute_rho(double * rho)
521 | {
522 |   rho[0] = dist2(cws[0], cws[1]);
523 |   rho[1] = dist2(cws[0], cws[2]);
524 |   rho[2] = dist2(cws[0], cws[3]);
525 |   rho[3] = dist2(cws[1], cws[2]);
526 |   rho[4] = dist2(cws[1], cws[3]);
527 |   rho[5] = dist2(cws[2], cws[3]);
528 | }
529 | 
530 | void epnp::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
531 | 					double betas[4], CvMat * A, CvMat * b)
532 | {
533 |   for(int i = 0; i < 6; i++) {
534 |     const double * rowL = l_6x10 + i * 10;
535 |     double * rowA = A->data.db + i * 4;
536 | 
537 |     rowA[0] = 2 * rowL[0] * betas[0] +     rowL[1] * betas[1] +     rowL[3] * betas[2] +     rowL[6] * betas[3];
538 |     rowA[1] =     rowL[1] * betas[0] + 2 * rowL[2] * betas[1] +     rowL[4] * betas[2] +     rowL[7] * betas[3];
539 |     rowA[2] =     rowL[3] * betas[0] +     rowL[4] * betas[1] + 2 * rowL[5] * betas[2] +     rowL[8] * betas[3];
540 |     rowA[3] =     rowL[6] * betas[0] +     rowL[7] * betas[1] +     rowL[8] * betas[2] + 2 * rowL[9] * betas[3];
541 | 
542 |     cvmSet(b, i, 0, rho[i] -
543 | 	   (
544 | 	    rowL[0] * betas[0] * betas[0] +
545 | 	    rowL[1] * betas[0] * betas[1] +
546 | 	    rowL[2] * betas[1] * betas[1] +
547 | 	    rowL[3] * betas[0] * betas[2] +
548 | 	    rowL[4] * betas[1] * betas[2] +
549 | 	    rowL[5] * betas[2] * betas[2] +
550 | 	    rowL[6] * betas[0] * betas[3] +
551 | 	    rowL[7] * betas[1] * betas[3] +
552 | 	    rowL[8] * betas[2] * betas[3] +
553 | 	    rowL[9] * betas[3] * betas[3]
554 | 	    ));
555 |   }
556 | }
557 | 
558 | void epnp::gauss_newton(const CvMat * L_6x10, const CvMat * Rho,
559 | 			double betas[4])
560 | {
561 |   const int iterations_number = 5;
562 | 
563 |   double a[6*4], b[6], x[4];
564 |   CvMat A = cvMat(6, 4, CV_64F, a);
565 |   CvMat B = cvMat(6, 1, CV_64F, b);
566 |   CvMat X = cvMat(4, 1, CV_64F, x);
567 | 
568 |   for(int k = 0; k < iterations_number; k++) {
569 |     compute_A_and_b_gauss_newton(L_6x10->data.db, Rho->data.db,
570 | 				 betas, &A, &B);
571 |     qr_solve(&A, &B, &X);
572 | 
573 |     for(int i = 0; i < 4; i++)
574 |       betas[i] += x[i];
575 |   }
576 | }
577 | 
578 | void epnp::qr_solve(CvMat * A, CvMat * b, CvMat * X)
579 | {
580 |   static int max_nr = 0;
581 |   static double * A1, * A2;
582 | 
583 |   const int nr = A->rows;
584 |   const int nc = A->cols;
585 | 
586 |   if (max_nr != 0 && max_nr < nr) {
587 |     delete [] A1;
588 |     delete [] A2;
589 |   }
590 |   if (max_nr < nr) {
591 |     max_nr = nr;
592 |     A1 = new double[nr];
593 |     A2 = new double[nr];
594 |   }
595 | 
596 |   double * pA = A->data.db, * ppAkk = pA;
597 |   for(int k = 0; k < nc; k++) {
598 |     double * ppAik = ppAkk, eta = fabs(*ppAik);
599 |     for(int i = k + 1; i < nr; i++) {
600 |       double elt = fabs(*ppAik);
601 |       if (eta < elt) eta = elt;
602 |       ppAik += nc;
603 |     }
604 | 
605 |     if (eta == 0) {
606 |       A1[k] = A2[k] = 0.0;
607 |       cerr << "God damnit, A is singular, this shouldn't happen." << endl;
608 |       return;
609 |     } else {
610 |       double * ppAik = ppAkk, sum = 0.0, inv_eta = 1. / eta;
611 |       for(int i = k; i < nr; i++) {
612 | 	*ppAik *= inv_eta;
613 | 	sum += *ppAik * *ppAik;
614 | 	ppAik += nc;
615 |       }
616 |       double sigma = sqrt(sum);
617 |       if (*ppAkk < 0)
618 | 	sigma = -sigma;
619 |       *ppAkk += sigma;
620 |       A1[k] = sigma * *ppAkk;
621 |       A2[k] = -eta * sigma;
622 |       for(int j = k + 1; j < nc; j++) {
623 | 	double * ppAik = ppAkk, sum = 0;
624 | 	for(int i = k; i < nr; i++) {
625 | 	  sum += *ppAik * ppAik[j - k];
626 | 	  ppAik += nc;
627 | 	}
628 | 	double tau = sum / A1[k];
629 | 	ppAik = ppAkk;
630 | 	for(int i = k; i < nr; i++) {
631 | 	  ppAik[j - k] -= tau * *ppAik;
632 | 	  ppAik += nc;
633 | 	}
634 |       }
635 |     }
636 |     ppAkk += nc + 1;
637 |   }
638 | 
639 |   // b <- Qt b
640 |   double * ppAjj = pA, * pb = b->data.db;
641 |   for(int j = 0; j < nc; j++) {
642 |     double * ppAij = ppAjj, tau = 0;
643 |     for(int i = j; i < nr; i++)	{
644 |       tau += *ppAij * pb[i];
645 |       ppAij += nc;
646 |     }
647 |     tau /= A1[j];
648 |     ppAij = ppAjj;
649 |     for(int i = j; i < nr; i++) {
650 |       pb[i] -= tau * *ppAij;
651 |       ppAij += nc;
652 |     }
653 |     ppAjj += nc + 1;
654 |   }
655 | 
656 |   // X = R-1 b
657 |   double * pX = X->data.db;
658 |   pX[nc - 1] = pb[nc - 1] / A2[nc - 1];
659 |   for(int i = nc - 2; i >= 0; i--) {
660 |     double * ppAij = pA + i * nc + (i + 1), sum = 0;
661 | 
662 |     for(int j = i + 1; j < nc; j++) {
663 |       sum += *ppAij * pX[j];
664 |       ppAij++;
665 |     }
666 |     pX[i] = (pb[i] - sum) / A2[i];
667 |   }
668 | }
669 | 
670 | 
671 | 
672 | void epnp::relative_error(double & rot_err, double & transl_err,
673 | 			  const double Rtrue[3][3], const double ttrue[3],
674 | 			  const double Rest[3][3],  const double test[3])
675 | {
676 |   double qtrue[4], qest[4];
677 | 
678 |   mat_to_quat(Rtrue, qtrue);
679 |   mat_to_quat(Rest, qest);
680 | 
681 |   double rot_err1 = sqrt((qtrue[0] - qest[0]) * (qtrue[0] - qest[0]) +
682 | 			 (qtrue[1] - qest[1]) * (qtrue[1] - qest[1]) +
683 | 			 (qtrue[2] - qest[2]) * (qtrue[2] - qest[2]) +
684 | 			 (qtrue[3] - qest[3]) * (qtrue[3] - qest[3]) ) /
685 |     sqrt(qtrue[0] * qtrue[0] + qtrue[1] * qtrue[1] + qtrue[2] * qtrue[2] + qtrue[3] * qtrue[3]);
686 | 
687 |   double rot_err2 = sqrt((qtrue[0] + qest[0]) * (qtrue[0] + qest[0]) +
688 | 			 (qtrue[1] + qest[1]) * (qtrue[1] + qest[1]) +
689 | 			 (qtrue[2] + qest[2]) * (qtrue[2] + qest[2]) +
690 | 			 (qtrue[3] + qest[3]) * (qtrue[3] + qest[3]) ) /
691 |     sqrt(qtrue[0] * qtrue[0] + qtrue[1] * qtrue[1] + qtrue[2] * qtrue[2] + qtrue[3] * qtrue[3]);
692 | 
693 |   rot_err = min(rot_err1, rot_err2);
694 | 
695 |   transl_err =
696 |     sqrt((ttrue[0] - test[0]) * (ttrue[0] - test[0]) +
697 | 	 (ttrue[1] - test[1]) * (ttrue[1] - test[1]) +
698 | 	 (ttrue[2] - test[2]) * (ttrue[2] - test[2])) /
699 |     sqrt(ttrue[0] * ttrue[0] + ttrue[1] * ttrue[1] + ttrue[2] * ttrue[2]);
700 | }
701 | 
702 | void epnp::mat_to_quat(const double R[3][3], double q[4])
703 | {
704 |   double tr = R[0][0] + R[1][1] + R[2][2];
705 |   double n4;
706 | 
707 |   if (tr > 0.0f) {
708 |     q[0] = R[1][2] - R[2][1];
709 |     q[1] = R[2][0] - R[0][2];
710 |     q[2] = R[0][1] - R[1][0];
711 |     q[3] = tr + 1.0f;
712 |     n4 = q[3];
713 |   } else if ( (R[0][0] > R[1][1]) && (R[0][0] > R[2][2]) ) {
714 |     q[0] = 1.0f + R[0][0] - R[1][1] - R[2][2];
715 |     q[1] = R[1][0] + R[0][1];
716 |     q[2] = R[2][0] + R[0][2];
717 |     q[3] = R[1][2] - R[2][1];
718 |     n4 = q[0];
719 |   } else if (R[1][1] > R[2][2]) {
720 |     q[0] = R[1][0] + R[0][1];
721 |     q[1] = 1.0f + R[1][1] - R[0][0] - R[2][2];
722 |     q[2] = R[2][1] + R[1][2];
723 |     q[3] = R[2][0] - R[0][2];
724 |     n4 = q[1];
725 |   } else {
726 |     q[0] = R[2][0] + R[0][2];
727 |     q[1] = R[2][1] + R[1][2];
728 |     q[2] = 1.0f + R[2][2] - R[0][0] - R[1][1];
729 |     q[3] = R[0][1] - R[1][0];
730 |     n4 = q[2];
731 |   }
732 |   double scale = 0.5f / double(sqrt(n4));
733 | 
734 |   q[0] *= scale;
735 |   q[1] *= scale;
736 |   q[2] *= scale;
737 |   q[3] *= scale;
738 | }
739 | 


--------------------------------------------------------------------------------
/epnp/epnp.h:
--------------------------------------------------------------------------------
  1 | // Copyright (c) 2009, V. Lepetit, EPFL
  2 | // All rights reserved.
  3 | 
  4 | // Redistribution and use in source and binary forms, with or without
  5 | // modification, are permitted provided that the following conditions are met: 
  6 | 
  7 | // 1. Redistributions of source code must retain the above copyright notice, this
  8 | //    list of conditions and the following disclaimer. 
  9 | // 2. Redistributions in binary form must reproduce the above copyright notice,
 10 | //    this list of conditions and the following disclaimer in the documentation
 11 | //    and/or other materials provided with the distribution. 
 12 | 
 13 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 14 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 15 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 16 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 17 | // ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 18 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 19 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 20 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 21 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 22 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 23 | 
 24 | // The views and conclusions contained in the software and documentation are those
 25 | // of the authors and should not be interpreted as representing official policies, 
 26 | //   either expressed or implied, of the FreeBSD Project.
 27 | 
 28 | #ifndef epnp_h
 29 | #define epnp_h
 30 | 
 31 | #include <cv.h>
 32 | 
 33 | class epnp {
 34 |  public:
 35 |   epnp(void);
 36 |   ~epnp();
 37 | 
 38 |   void set_internal_parameters(const double uc, const double vc,
 39 | 			       const double fu, const double fv);
 40 | 
 41 |   void set_maximum_number_of_correspondences(const int n);
 42 |   void reset_correspondences(void);
 43 |   void add_correspondence(const double X, const double Y, const double Z,
 44 | 			  const double u, const double v);
 45 | 
 46 |   double compute_pose(double R[3][3], double T[3]);
 47 | 
 48 |   void relative_error(double & rot_err, double & transl_err,
 49 | 		      const double Rtrue[3][3], const double ttrue[3],
 50 | 		      const double Rest[3][3],  const double test[3]);
 51 | 
 52 |   void print_pose(const double R[3][3], const double t[3]);
 53 |   double reprojection_error(const double R[3][3], const double t[3]);
 54 | 
 55 |  private:
 56 |   void choose_control_points(void);
 57 |   void compute_barycentric_coordinates(void);
 58 |   void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v);
 59 |   void compute_ccs(const double * betas, const double * ut);
 60 |   void compute_pcs(void);
 61 | 
 62 |   void solve_for_sign(void);
 63 | 
 64 |   void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas);
 65 |   void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas);
 66 |   void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas);
 67 |   void qr_solve(CvMat * A, CvMat * b, CvMat * X);
 68 | 
 69 |   double dot(const double * v1, const double * v2);
 70 |   double dist2(const double * p1, const double * p2);
 71 | 
 72 |   void compute_rho(double * rho);
 73 |   void compute_L_6x10(const double * ut, double * l_6x10);
 74 | 
 75 |   void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]);
 76 |   void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
 77 | 				    double cb[4], CvMat * A, CvMat * b);
 78 | 
 79 |   double compute_R_and_t(const double * ut, const double * betas,
 80 | 			 double R[3][3], double t[3]);
 81 | 
 82 |   void estimate_R_and_t(double R[3][3], double t[3]);
 83 | 
 84 |   void copy_R_and_t(const double R_dst[3][3], const double t_dst[3],
 85 | 		    double R_src[3][3], double t_src[3]);
 86 | 
 87 |   void mat_to_quat(const double R[3][3], double q[4]);
 88 | 
 89 | 
 90 |   double uc, vc, fu, fv;
 91 | 
 92 |   double * pws, * us, * alphas, * pcs;
 93 |   int maximum_number_of_correspondences;
 94 |   int number_of_correspondences;
 95 | 
 96 |   double cws[4][3], ccs[4][3];
 97 |   double cws_determinant;
 98 | };
 99 | 
100 | #endif
101 | 


--------------------------------------------------------------------------------
/epnp/main.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright (c) 2009, V. Lepetit, EPFL
  2 | // All rights reserved.
  3 | 
  4 | // Redistribution and use in source and binary forms, with or without
  5 | // modification, are permitted provided that the following conditions are met: 
  6 | 
  7 | // 1. Redistributions of source code must retain the above copyright notice, this
  8 | //    list of conditions and the following disclaimer. 
  9 | // 2. Redistributions in binary form must reproduce the above copyright notice,
 10 | //    this list of conditions and the following disclaimer in the documentation
 11 | //    and/or other materials provided with the distribution. 
 12 | 
 13 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 14 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 15 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 16 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
 17 | // ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 18 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 19 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 20 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 21 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 22 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 23 | 
 24 | // The views and conclusions contained in the software and documentation are those
 25 | // of the authors and should not be interpreted as representing official policies, 
 26 | //   either expressed or implied, of the FreeBSD Project.
 27 |   
 28 | #include <iostream>
 29 | #include <fstream>
 30 | using namespace std;
 31 | 
 32 | #include "epnp.h"
 33 | 
 34 | const double uc = 320;
 35 | const double vc = 240;
 36 | const double fu = 800;
 37 | const double fv = 800;
 38 | 
 39 | 
 40 | // MtM takes more time than 12x12 opencv SVD with about 180 points and more:
 41 | 
 42 | const int n = 10;
 43 | const double noise = 10;
 44 | 
 45 | double rand(double min, double max)
 46 | {
 47 |   return min + (max - min) * double(rand()) / RAND_MAX;
 48 | }
 49 | 
 50 | void random_pose(double R[3][3], double t[3])
 51 | {
 52 |   const double range = 1;
 53 | 
 54 |   double phi   = rand(0, range * 3.14159 * 2);
 55 |   double theta = rand(0, range * 3.14159);
 56 |   double psi   = rand(0, range * 3.14159 * 2);
 57 | 
 58 |   R[0][0] = cos(psi) * cos(phi) - cos(theta) * sin(phi) * sin(psi);
 59 |   R[0][1] = cos(psi) * sin(phi) + cos(theta) * cos(phi) * sin(psi);
 60 |   R[0][2] = sin(psi) * sin(theta);
 61 | 
 62 |   R[1][0] = -sin(psi) * cos(phi) - cos(theta) * sin(phi) * cos(psi);
 63 |   R[1][1] = -sin(psi) * sin(phi) + cos(theta) * cos(phi) * cos(psi);
 64 |   R[1][2] = cos(psi) * sin(theta);
 65 | 
 66 |   R[2][0] = sin(theta) * sin(phi);
 67 |   R[2][1] = -sin(theta) * cos(phi);
 68 |   R[2][2] = cos(theta);
 69 | 
 70 |   t[0] = 0.0f;
 71 |   t[1] = 0.0f;
 72 |   t[2] = 6.0f;
 73 | }
 74 | 
 75 | void random_point(double & Xw, double & Yw, double & Zw)
 76 | {
 77 |   double theta = rand(0, 3.14159), phi = rand(0, 2 * 3.14159), R = rand(0, +2);
 78 | 
 79 |   Xw =  sin(theta) * sin(phi) * R;
 80 |   Yw = -sin(theta) * cos(phi) * R;
 81 |   Zw =  cos(theta) * R;
 82 | }
 83 | 
 84 | void project_with_noise(double R[3][3], double t[3],
 85 | 			double Xw, double Yw, double Zw,
 86 | 			double & u, double & v)
 87 | {
 88 |   double Xc = R[0][0] * Xw + R[0][1] * Yw + R[0][2] * Zw + t[0];
 89 |   double Yc = R[1][0] * Xw + R[1][1] * Yw + R[1][2] * Zw + t[1];
 90 |   double Zc = R[2][0] * Xw + R[2][1] * Yw + R[2][2] * Zw + t[2];
 91 | 
 92 |   double nu = rand(-noise, +noise);
 93 |   double nv = rand(-noise, +noise);
 94 |   u = uc + fu * Xc / Zc + nu;
 95 |   v = vc + fv * Yc / Zc + nv;
 96 | }
 97 | 
 98 | int main(int /*argc*/, char ** /*argv*/)
 99 | {
100 |   epnp PnP;
101 | 
102 |   srand(time(0));
103 | 
104 |   PnP.set_internal_parameters(uc, vc, fu, fv);
105 |   PnP.set_maximum_number_of_correspondences(n);
106 | 
107 |   double R_true[3][3], t_true[3];
108 |   random_pose(R_true, t_true);
109 | 
110 |   PnP.reset_correspondences();
111 |   for(int i = 0; i < n; i++) {
112 |     double Xw, Yw, Zw, u, v;
113 | 
114 |     random_point(Xw, Yw, Zw);
115 | 
116 |     project_with_noise(R_true, t_true, Xw, Yw, Zw, u, v);
117 |     PnP.add_correspondence(Xw, Yw, Zw, u, v);
118 |   }
119 | 
120 |   double R_est[3][3], t_est[3];
121 |   double err2 = PnP.compute_pose(R_est, t_est);
122 |   double rot_err, transl_err;
123 | 
124 |   PnP.relative_error(rot_err, transl_err, R_true, t_true, R_est, t_est);
125 |   cout << ">>> Reprojection error: " << err2 << endl;
126 |   cout << ">>> rot_err: " << rot_err << ", transl_err: " << transl_err << endl;
127 |   cout << endl;
128 |   cout << "'True reprojection error':"
129 |        << PnP.reprojection_error(R_true, t_true) << endl;
130 |   cout << endl;
131 |   cout << "True pose:" << endl;
132 |   PnP.print_pose(R_true, t_true);
133 |   cout << endl;
134 |   cout << "Found pose:" << endl;
135 |   PnP.print_pose(R_est, t_est);
136 | 
137 |   return 0;
138 | }
139 | 


--------------------------------------------------------------------------------