Merge pull request #3157 from PhilLab:pnpFix

modules/calib3d/src/dls.h

@@ -24,9 +24,9 @@ private:
     {
         for(int i = 0; i < N; i++)
         {
-            p.at<double>(0,i) = opoints.at<OpointType>(0,i).x;
-            p.at<double>(1,i) = opoints.at<OpointType>(0,i).y;
-            p.at<double>(2,i) = opoints.at<OpointType>(0,i).z;
+            p.at<double>(0,i) = opoints.at<OpointType>(i).x;
+            p.at<double>(1,i) = opoints.at<OpointType>(i).y;
+            p.at<double>(2,i) = opoints.at<OpointType>(i).z;
 
             // compute mean of object points
             mn.at<double>(0) += p.at<double>(0,i);
@@ -34,12 +34,12 @@ private:
             mn.at<double>(2) += p.at<double>(2,i);
 
             // make z into unit vectors from normalized pixel coords
-            double sr = std::pow(ipoints.at<IpointType>(0,i).x, 2) +
-                        std::pow(ipoints.at<IpointType>(0,i).y, 2) + (double)1;
+            double sr = std::pow(ipoints.at<IpointType>(i).x, 2) +
+                        std::pow(ipoints.at<IpointType>(i).y, 2) + (double)1;
                    sr = std::sqrt(sr);
 
-            z.at<double>(0,i) = ipoints.at<IpointType>(0,i).x / sr;
-            z.at<double>(1,i) = ipoints.at<IpointType>(0,i).y / sr;
+            z.at<double>(0,i) = ipoints.at<IpointType>(i).x / sr;
+            z.at<double>(1,i) = ipoints.at<IpointType>(i).y / sr;
             z.at<double>(2,i) = (double)1 / sr;
         }
 

modules/calib3d/src/epnp.h

@@ -27,12 +27,12 @@ class epnp {
   {
       for(int i = 0; i < number_of_correspondences; i++)
       {
-          pws[3 * i    ] = opoints.at<OpointType>(0,i).x;
-          pws[3 * i + 1] = opoints.at<OpointType>(0,i).y;
-          pws[3 * i + 2] = opoints.at<OpointType>(0,i).z;
+          pws[3 * i    ] = opoints.at<OpointType>(i).x;
+          pws[3 * i + 1] = opoints.at<OpointType>(i).y;
+          pws[3 * i + 2] = opoints.at<OpointType>(i).z;
 
-          us[2 * i    ] = ipoints.at<IpointType>(0,i).x*fu + uc;
-          us[2 * i + 1] = ipoints.at<IpointType>(0,i).y*fv + vc;
+          us[2 * i    ] = ipoints.at<IpointType>(i).x*fu + uc;
+          us[2 * i + 1] = ipoints.at<IpointType>(i).y*fv + vc;
       }
   }
   double reprojection_error(const double R[3][3], const double t[3]);

modules/calib3d/src/p3p.h

@@ -37,11 +37,11 @@ class p3p
       points.resize(20);
       for(int i = 0; i < 4; i++)
       {
-          points[i*5] = ipoints.at<IpointType>(0,i).x*fx + cx;
-          points[i*5+1] = ipoints.at<IpointType>(0,i).y*fy + cy;
-          points[i*5+2] = opoints.at<OpointType>(0,i).x;
-          points[i*5+3] = opoints.at<OpointType>(0,i).y;
-          points[i*5+4] = opoints.at<OpointType>(0,i).z;
+          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;
       }
   }
   void init_inverse_parameters();