opencv
diff --git a/‎doc/opencv.bib
Copy file name to clipboard
+15-1Lines changed: 15 additions & 1 deletion b/‎doc/opencv.bib
Copy file name to clipboard
+15-1Lines changed: 15 additions & 1 deletion
diff --git a/‎modules/calib3d/include/opencv2/calib3d.hpp
Copy file name to clipboard
+263-27Lines changed: 263 additions & 27 deletions b/‎modules/calib3d/include/opencv2/calib3d.hpp
Copy file name to clipboard
+263-27Lines changed: 263 additions & 27 deletions
diff --git a/‎modules/calib3d/src/ap3p.cpp
Copy file name to clipboard
+66-38Lines changed: 66 additions & 38 deletions b/‎modules/calib3d/src/ap3p.cpp
Copy file name to clipboard
+66-38Lines changed: 66 additions & 38 deletions
diff --git a/‎modules/calib3d/src/ap3p.h
Copy file name to clipboard
+13-5Lines changed: 13 additions & 5 deletions b/‎modules/calib3d/src/ap3p.h
Copy file name to clipboard
+13-5Lines changed: 13 additions & 5 deletions
@@ -209,7 +209,21 @@ @article{Chaumette06
   hal_id = {inria-00350283},
   hal_version = {v1},
 }
-
+@article{Collins14
+  year = {2014},
+  issn = {0920-5691},
+  journal = {International Journal of Computer Vision},
+  volume = {109},
+  number = {3},
+  doi = {10.1007/s11263-014-0725-5},
+  title = {Infinitesimal Plane-Based Pose Estimation},
+  url = {https://dx.doi.org/10.1007/s11263-014-0725-5},
+  publisher = {Springer US},
+  keywords = {Plane; Pose; SfM; PnP; Homography},
+  author = {Collins, Toby and Bartoli, Adrien},
+  pages = {252-286},
+  language = {English}
+}
 @article{Daniilidis98,
   author = {Konstantinos Daniilidis},
   title = {Hand-Eye Calibration Using Dual Quaternions},
 
@@ -1,3 +1,4 @@
+#include "precomp.hpp"
 #include "ap3p.h"
 
 #include <cmath>
@@ -154,10 +155,11 @@ ap3p::ap3p(double _fx, double _fy, double _cx, double _cy) {
 // worldPoints: The positions of the 3 feature points stored as column vectors
 // solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame
 // solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame
-int ap3p::computePoses(const double featureVectors[3][3],
-                       const double worldPoints[3][3],
+int ap3p::computePoses(const double featureVectors[3][4],
+                       const double worldPoints[3][4],
                        double solutionsR[4][3][3],
-                       double solutionsT[4][3]) {
+                       double solutionsT[4][3],
+                       bool p4p) {
 
     //world point vectors
     double w1[3] = {worldPoints[0][0], worldPoints[1][0], worldPoints[2][0]};
@@ -246,6 +248,13 @@ int ap3p::computePoses(const double featureVectors[3][3],
     double b3p[3];
     vect_scale((delta / k3b3), b3, b3p);
 
+    double X3 = worldPoints[0][3];
+    double Y3 = worldPoints[1][3];
+    double Z3 = worldPoints[2][3];
+    double mu3 = featureVectors[0][3];
+    double mv3 = featureVectors[1][3];
+    double reproj_errors[4];
+
     int nb_solutions = 0;
     for (int i = 0; i < 4; ++i) {
         double ctheta1p = s[i];
@@ -290,9 +299,29 @@ int ap3p::computePoses(const double featureVectors[3][3],
         solutionsR[nb_solutions][1][2] = R[2][1];
         solutionsR[nb_solutions][2][2] = R[2][2];
 
+        if (p4p) {
+            double X3p = solutionsR[nb_solutions][0][0] * X3 + solutionsR[nb_solutions][0][1] * Y3 + solutionsR[nb_solutions][0][2] * Z3 + solutionsT[nb_solutions][0];
+            double Y3p = solutionsR[nb_solutions][1][0] * X3 + solutionsR[nb_solutions][1][1] * Y3 + solutionsR[nb_solutions][1][2] * Z3 + solutionsT[nb_solutions][1];
+            double Z3p = solutionsR[nb_solutions][2][0] * X3 + solutionsR[nb_solutions][2][1] * Y3 + solutionsR[nb_solutions][2][2] * Z3 + solutionsT[nb_solutions][2];
+            double mu3p = X3p / Z3p;
+            double mv3p = Y3p / Z3p;
+            reproj_errors[nb_solutions] = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
+        }
+
         nb_solutions++;
     }
 
+    //sort the solutions
+    if (p4p) {
+        for (int i = 1; i < nb_solutions; i++) {
+            for (int j = i; j > 0 && reproj_errors[j-1] > reproj_errors[j]; j--) {
+                std::swap(reproj_errors[j], reproj_errors[j-1]);
+                std::swap(solutionsR[j], solutionsR[j-1]);
+                std::swap(solutionsT[j], solutionsT[j-1]);
+            }
+        }
+    }
+
     return nb_solutions;
 }
 
@@ -311,9 +340,10 @@ bool ap3p::solve(cv::Mat &R, cv::Mat &tvec, const cv::Mat &opoints, const cv::Ma
     else
         extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
 
-    bool result = solve(rotation_matrix, translation, points[0], points[1], points[2], points[3], points[4], points[5],
-                        points[6], points[7], points[8], points[9], points[10], points[11], points[12], points[13],
-                        points[14],
+    bool result = solve(rotation_matrix, translation,
+                        points[0], points[1], points[2], points[3], points[4],
+                        points[5], points[6], points[7], points[8], points[9],
+                        points[10], points[11], points[12], points[13],points[14],
                         points[15], points[16], points[17], points[18], points[19]);
     cv::Mat(3, 1, CV_64F, translation).copyTo(tvec);
     cv::Mat(3, 3, CV_64F, rotation_matrix).copyTo(R);
@@ -335,10 +365,13 @@ int ap3p::solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv:
     else
         extract_points<cv::Point3d, cv::Point2f>(opoints, ipoints, points);
 
+    const bool p4p = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F)) == 4;
     int solutions = solve(rotation_matrix, translation,
                           points[0], points[1], points[2], points[3], points[4],
                           points[5], points[6], points[7], points[8], points[9],
-                          points[10], points[11], points[12], points[13], points[14]);
+                          points[10], points[11], points[12], points[13], points[14],
+                          points[15], points[16], points[17], points[18], points[19],
+                          p4p);
 
     for (int i = 0; i < solutions; i++) {
         cv::Mat R, tvec;
@@ -353,42 +386,33 @@ int ap3p::solve(std::vector<cv::Mat> &Rs, std::vector<cv::Mat> &tvecs, const cv:
 }
 
 bool
-ap3p::solve(double R[3][3], double t[3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1,
-            double mv1,
-            double X1, double Y1, double Z1, double mu2, double mv2, double X2, double Y2, double Z2, double mu3,
-            double mv3, double X3, double Y3, double Z3) {
+ap3p::solve(double R[3][3], double t[3],
+            double mu0, double mv0, double X0, double Y0, double Z0,
+            double mu1, double mv1, double X1, double Y1, double Z1,
+            double mu2, double mv2, double X2, double Y2, double Z2,
+            double mu3, double mv3, double X3, double Y3, double Z3) {
     double Rs[4][3][3], ts[4][3];
 
-    int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2);
+    const bool p4p = true;
+    int n = solve(Rs, ts, mu0, mv0, X0, Y0, Z0, mu1, mv1, X1, Y1, Z1, mu2, mv2, X2, Y2, Z2, mu3, mv3, X3, Y3, Z3, p4p);
     if (n == 0)
         return false;
 
-    int ns = 0;
-    double min_reproj = 0;
-    for (int i = 0; i < n; i++) {
-        double X3p = Rs[i][0][0] * X3 + Rs[i][0][1] * Y3 + Rs[i][0][2] * Z3 + ts[i][0];
-        double Y3p = Rs[i][1][0] * X3 + Rs[i][1][1] * Y3 + Rs[i][1][2] * Z3 + ts[i][1];
-        double Z3p = Rs[i][2][0] * X3 + Rs[i][2][1] * Y3 + Rs[i][2][2] * Z3 + ts[i][2];
-        double mu3p = cx + fx * X3p / Z3p;
-        double mv3p = cy + fy * Y3p / Z3p;
-        double reproj = (mu3p - mu3) * (mu3p - mu3) + (mv3p - mv3) * (mv3p - mv3);
-        if (i == 0 || min_reproj > reproj) {
-            ns = i;
-            min_reproj = reproj;
-        }
-    }
-
     for (int i = 0; i < 3; i++) {
         for (int j = 0; j < 3; j++)
-            R[i][j] = Rs[ns][i][j];
-        t[i] = ts[ns][i];
+            R[i][j] = Rs[0][i][j];
+        t[i] = ts[0][i];
     }
 
     return true;
 }
 
-int ap3p::solve(double R[4][3][3], double t[4][3], double mu0, double mv0, double X0, double Y0, double Z0, double mu1,
-                double mv1, double X1, double Y1, double Z1, double mu2, double mv2, double X2, double Y2, double Z2) {
+int ap3p::solve(double R[4][3][3], double t[4][3],
+                double mu0, double mv0, double X0, double Y0, double Z0,
+                double mu1, double mv1, double X1, double Y1, double Z1,
+                double mu2, double mv2, double X2, double Y2, double Z2,
+                double mu3, double mv3, double X3, double Y3, double Z3,
+                bool p4p) {
     double mk0, mk1, mk2;
     double norm;
 
@@ -413,13 +437,17 @@ int ap3p::solve(double R[4][3][3], double t[4][3], double mu0, double mv0, doubl
     mu2 *= mk2;
     mv2 *= mk2;
 
-    double featureVectors[3][3] = {{mu0, mu1, mu2},
-                                   {mv0, mv1, mv2},
-                                   {mk0, mk1, mk2}};
-    double worldPoints[3][3] = {{X0, X1, X2},
-                                {Y0, Y1, Y2},
-                                {Z0, Z1, Z2}};
+    mu3 = inv_fx * mu3 - cx_fx;
+    mv3 = inv_fy * mv3 - cy_fy;
+    double mk3 = 1; //not used
+
+    double featureVectors[3][4] = {{mu0, mu1, mu2, mu3},
+                                   {mv0, mv1, mv2, mv3},
+                                   {mk0, mk1, mk2, mk3}};
+    double worldPoints[3][4] = {{X0, X1, X2, X3},
+                                {Y0, Y1, Y2, Y3},
+                                {Z0, Z1, Z2, Z3}};
 
-    return computePoses(featureVectors, worldPoints, R, t);
+    return computePoses(featureVectors, worldPoints, R, t, p4p);
 }
 }
@@ -1,7 +1,7 @@
 #ifndef P3P_P3P_H
 #define P3P_P3P_H
 
-#include "precomp.hpp"
+#include <opencv2/core.hpp>
 
 namespace cv {
 class ap3p {
@@ -18,14 +18,20 @@ class ap3p {
     void extract_points(const cv::Mat &opoints, const cv::Mat &ipoints, std::vector<double> &points) {
         points.clear();
         int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
-        points.resize(5*npoints);
+        points.resize(5*4); //resize vector to fit for p4p case
         for (int i = 0; i < npoints; i++) {
             points[i * 5] = ipoints.at<IpointType>(i).x * fx + cx;
             points[i * 5 + 1] = ipoints.at<IpointType>(i).y * fy + cy;
             points[i * 5 + 2] = opoints.at<OpointType>(i).x;
             points[i * 5 + 3] = opoints.at<OpointType>(i).y;
             points[i * 5 + 4] = opoints.at<OpointType>(i).z;
         }
+        //Fill vectors with unused values for p3p case
+        for (int i = npoints; i < 4; i++) {
+            for (int j = 0; j < 5; j++) {
+                points[i * 5 + j] = 0;
+            }
+        }
     }
 
     void init_inverse_parameters();
@@ -45,7 +51,9 @@ class ap3p {
     int solve(double R[4][3][3], double t[4][3],
               double mu0, double mv0, double X0, double Y0, double Z0,
               double mu1, double mv1, double X1, double Y1, double Z1,
-              double mu2, double mv2, double X2, double Y2, double Z2);
+              double mu2, double mv2, double X2, double Y2, double Z2,
+              double mu3, double mv3, double X3, double Y3, double Z3,
+              bool p4p);
 
     bool solve(double R[3][3], double t[3],
                double mu0, double mv0, double X0, double Y0, double Z0,
@@ -59,8 +67,8 @@ class ap3p {
     // worldPoints: Positions of the 3 feature points stored as column vectors
     // solutionsR: 4 possible solutions of rotation matrix of the world w.r.t the camera frame
     // solutionsT: 4 possible solutions of translation of the world origin w.r.t the camera frame
-    int computePoses(const double featureVectors[3][3], const double worldPoints[3][3], double solutionsR[4][3][3],
-                     double solutionsT[4][3]);
+    int computePoses(const double featureVectors[3][4], const double worldPoints[3][4], double solutionsR[4][3][3],
+                     double solutionsT[4][3], bool p4p);
 
 };
 }