Merge pull request #16011 from sebastien-wybo:fix_16007

* Fix #16007 - colinearity computed using all 3 coordinates

* calib3d(test): estimateAffine3D regression 16007

modules/calib3d/src/ptsetreg.cpp

@@ -488,13 +488,13 @@ public:
             for(j = 0; j < i; ++j)
             {
                 Point3f d1 = ptr[j] - ptr[i];
-                float n1 = d1.x*d1.x + d1.y*d1.y;
+                float n1 = d1.x*d1.x + d1.y*d1.y + d1.z*d1.z;
 
                 for(k = 0; k < j; ++k)
                 {
                     Point3f d2 = ptr[k] - ptr[i];
-                    float denom = (d2.x*d2.x + d2.y*d2.y)*n1;
-                    float num = d1.x*d2.x + d1.y*d2.y;
+                    float denom = (d2.x*d2.x + d2.y*d2.y + d2.z*d2.z)*n1;
+                    float num = d1.x*d2.x + d1.y*d2.y + d1.z*d2.z;
 
                     if( num*num > threshold*threshold*denom )
                         return false;

modules/calib3d/test/test_affine3d_estimator.cpp

@@ -192,4 +192,18 @@ void CV_Affine3D_EstTest::run( int /* start_from */)
 
 TEST(Calib3d_EstimateAffine3D, accuracy) { CV_Affine3D_EstTest test; test.safe_run(); }
 
+TEST(Calib3d_EstimateAffine3D, regression_16007)
+{
+    std::vector<cv::Point3f> m1, m2;
+    m1.push_back(Point3f(1.0f, 0.0f, 0.0f)); m2.push_back(Point3f(1.0f, 1.0f, 0.0f));
+    m1.push_back(Point3f(1.0f, 0.0f, 1.0f)); m2.push_back(Point3f(1.0f, 1.0f, 1.0f));
+    m1.push_back(Point3f(0.5f, 0.0f, 0.5f)); m2.push_back(Point3f(0.5f, 1.0f, 0.5f));
+    m1.push_back(Point3f(2.5f, 0.0f, 2.5f)); m2.push_back(Point3f(2.5f, 1.0f, 2.5f));
+    m1.push_back(Point3f(2.0f, 0.0f, 1.0f)); m2.push_back(Point3f(2.0f, 1.0f, 1.0f));
+
+    cv::Mat m3D, inl;
+    int res = cv::estimateAffine3D(m1, m2, m3D, inl);
+    EXPECT_EQ(1, res);
+}
+
 }} // namespace