Merge pull request #11 from clockj/shijie

update crossPoint function: not quit due to parallel line but just ou…

Author: clockj

demo/python/demo_math.ipynb

@@ -8,7 +8,7 @@
     {
      "data": {
       "text/plain": [
-       "<pyOpenLPT.PythonStreamRedirector at 0x20e13232670>"
+       "<pyOpenLPT.PythonStreamRedirector at 0x2f910d01cf0>"
       ]
      },
      "execution_count": 1,
@@ -231,6 +231,47 @@
     "print(pt3d.norm())"
    ]
   },
+  {
+   "cell_type": "code",
+   "execution_count": 3,
+   "metadata": {},
+   "outputs": [
+    {
+     "name": "stdout",
+     "output_type": "stream",
+     "text": [
+      "True\n"
+     ]
+    },
+    {
+     "name": "stderr",
+     "output_type": "stream",
+     "text": [
+      "myMATH::crossPoint warning:The two lines are parallel!\n",
+      "\n",
+      "Matrix = \n",
+      "(dim_row, dim_col) = (2,1)\n",
+      "| 0.000e+00 |\n",
+      "| 0.000e+00 |\n",
+      "\n"
+     ]
+    }
+   ],
+   "source": [
+    "pt2d_1 = pyOpenLPT.math.Pt2D(1,2)\n",
+    "unit_1 = pyOpenLPT.math.Pt2D(1,1)\n",
+    "line1 = pyOpenLPT.math.Line2D(pt2d_1,unit_1)\n",
+    "\n",
+    "pt2d_2 = pyOpenLPT.math.Pt2D(3,4)\n",
+    "unit_2 = pyOpenLPT.math.Pt2D(1,1)\n",
+    "line2 = pyOpenLPT.math.Line2D(pt2d_2,unit_2)\n",
+    "\n",
+    "pt_cross, is_parallel = pyOpenLPT.math.crossPoint(line1,line2)\n",
+    "\n",
+    "pt_cross.print(3)\n",
+    "print(is_parallel)"
+   ]
+  },
   {
    "cell_type": "markdown",
    "metadata": {},

inc/libMath/STBCommons.h

@@ -50,7 +50,7 @@ struct PixelRange
         {
             row_min = row;
         }
-    }; // TODO: we can add a choice to increase search area
+    }; 
 
     // Note: before using SetRowRange or SetColRange
     //       make sure it has been initialized!!!
@@ -64,7 +64,7 @@ struct PixelRange
         {
             col_min = col;
         }
-    }; // TODO: we can add a choice to increase search area
+    }; 
 
     // Note: before using SetRowRange or SetColRange
     //       make sure it has been initialized!!!

inc/libMath/myMATH.h

@@ -151,7 +151,7 @@ void triangulation (Pt3D& pt_world, double& error,
 
 
 // Find cross points of two 2d lines
-Pt2D crossPoint (Line2D const& line1, Line2D const& line2);
+bool crossPoint (Pt2D& pt2d, Line2D const& line1, Line2D const& line2);
 
 
 // Create identity matrix 

inc/libSTB/StereoMatch.h

@@ -154,7 +154,10 @@ class StereoMatch
         std::vector<std::vector<Tracer2D>> const& tr2d_list
     );
 
-    PixelRange findSearchRegion (int id, std::vector<Line2D> const& sight2D_list);
+    // find search region for tracer
+    // output: PixelRange, is_correct
+    //  if is_correct = true, the search region is valid (no parallel lines, able to find cross points)
+    std::pair<PixelRange, bool> findSearchRegion (int id, std::vector<Line2D> const& sight2D_list);
 
     void iterOnObjIDMap (
         int id, 

src/pybind_OpenLPT/pymyMath.cpp

@@ -82,7 +82,12 @@ void init_myMath(py::module& m)
         return std::make_pair(pt_world_list, error_list);
     }, "Triangulate 3D points from all lists of 3D lines of sight");
 
-    m.def("crossPoint", &myMATH::crossPoint, "Find the cross point of two 2D lines");
+    // m.def("crossPoint", &myMATH::crossPoint, "Find the cross point of two 2D lines");
+    m.def("crossPoint", [](Line2D const& line1, Line2D const& line2){
+        Pt2D pt2d;
+        bool is_parallel = myMATH::crossPoint(pt2d, line1, line2);
+        return std::make_pair(pt2d, is_parallel);
+    }, "Find the cross point of two 2D lines");
 
     m.def("eye", &myMATH::eye<double>, "Create an identity Matrix<double>");
 

src/srcMath/myMATH.cpp

@@ -299,24 +299,36 @@ void triangulation(Pt3D& pt_world, double& error,
 }
 
 // Find the cross points of two 2d lines
-Pt2D crossPoint (Line2D const& line1, Line2D const& line2)
+bool crossPoint (Pt2D& pt2d, Line2D const& line1, Line2D const& line2)
 {
     double den = line1.unit_vector[0] * line2.unit_vector[1] - line1.unit_vector[1] * line2.unit_vector[0];
-    if (std::fabs(den) < SMALLNUMBER)
+    
+    // if (std::fabs(den) < SMALLNUMBER)
+    // {
+    //     std::cerr << "myMATH::crossPoint: "
+    //               << "The two lines are parallel!"
+    //               << std::endl;
+    //     throw error_range;
+    // }
+    bool is_parallel = false;
+    if (std::fabs(den) < 1e-10)
     {
-        std::cerr << "myMATH::crossPoint: "
+        std::cout << "myMATH::crossPoint warning:"
                   << "The two lines are parallel!"
                   << std::endl;
-        throw error_range;
+        is_parallel = true;
+        pt2d[0] = 0;
+        pt2d[1] = 0;
+        return is_parallel;
     }
 
     double num = line2.unit_vector[1] * (line2.pt[0] - line1.pt[0]) - line2.unit_vector[0] * (line2.pt[1] - line1.pt[1]);
+    double factor = num / den;
 
-    Pt2D res(line1.unit_vector);
-    res *= num / den;
-    res += line1.pt;
+    pt2d[0] = (line1.unit_vector[0]) * factor + line1.pt[0];
+    pt2d[1] = (line1.unit_vector[1]) * factor + line1.pt[1];
 
-    return res;
+    return is_parallel;
 }
 
 // Polynomial fit

src/srcSTB/StereoMatch.hpp

@@ -724,6 +724,7 @@ void StereoMatch::findTracerMatch (
     Line3D sight3D;
     Pt2D pt2d_1;
     Pt2D pt2d_2;
+    bool is_parallel;
     Pt2D unit2d;
     for (int i = 0; i < id; i ++)
     {       
@@ -787,8 +788,18 @@ void StereoMatch::findTracerMatch (
                 sight2D_axis.pt[1] = y_pixel;
 
                 // Get x_pixel (col id) from two lines 
-                pt2d_1 = myMATH::crossPoint(sight2D_axis, sight2D_plus);
-                pt2d_2 = myMATH::crossPoint(sight2D_axis, sight2D_minus);
+                // quit if the two lines are parallel
+                is_parallel = myMATH::crossPoint(pt2d_1, sight2D_axis, sight2D_plus);
+                if (is_parallel)
+                {
+                    return;
+                }
+                is_parallel = myMATH::crossPoint(pt2d_2, sight2D_axis, sight2D_minus);
+                if (is_parallel)
+                {
+                    return;
+                }
+
                 x_pixel_1 = pt2d_1[0];
                 x_pixel_2 = pt2d_2[0];
                 if (x_pixel_1 > x_pixel_2)
@@ -834,8 +845,18 @@ void StereoMatch::findTracerMatch (
                 sight2D_axis.pt[1] = 0;
 
                 // Get y_pixel (row id) from two lines 
-                pt2d_1 = myMATH::crossPoint(sight2D_axis, sight2D_plus);
-                pt2d_2 = myMATH::crossPoint(sight2D_axis, sight2D_minus);
+                // quit if the two lines are parallel
+                is_parallel = myMATH::crossPoint(pt2d_1, sight2D_axis, sight2D_plus);
+                if (is_parallel)
+                {
+                    return;
+                }
+                is_parallel = myMATH::crossPoint(pt2d_2, sight2D_axis, sight2D_minus);
+                if (is_parallel)
+                {
+                    return;
+                }
+
                 y_pixel_1 = pt2d_1[1];
                 y_pixel_2 = pt2d_2[1];
                 if (y_pixel_1 > y_pixel_2)
@@ -876,9 +897,14 @@ void StereoMatch::findTracerMatch (
     else
     {
         // find search region
-        PixelRange search_region = findSearchRegion(id, sight2D_list);
+        std::pair<PixelRange, bool> search_output = findSearchRegion(id, sight2D_list);
+        if (!search_output.second)
+        {
+            return;
+        }
 
         // iterate every pixel in the search region
+        PixelRange search_region = search_output.first;
         for (int i = search_region.row_min; i < search_region.row_max; i ++)
         {
             for (int j = search_region.col_min; j < search_region.col_max; j ++)
@@ -1078,7 +1104,10 @@ void StereoMatch::iterOnObjIDMap (
 // }
 
 
-PixelRange StereoMatch::findSearchRegion (int id, std::vector<Line2D> const& sight2D_list)
+// output: search_region, is_correct
+//  is_correct: true => do not have parallel lines
+//              false => have parallel lines
+std::pair<PixelRange, bool> StereoMatch::findSearchRegion (int id, std::vector<Line2D> const& sight2D_list)
 {
     PixelRange search_region;
 
@@ -1089,11 +1118,12 @@ PixelRange StereoMatch::findSearchRegion (int id, std::vector<Line2D> const& sig
     {
         search_region.row_max = n_row;
         search_region.col_max = n_col;
-        return search_region;
+        return std::pair(search_region, true);
     }
 
     // Find all crossing points
     Pt2D pt_cross;
+    bool is_parallel;
     Pt2D pt_temp;
     Pt2D perp_unit_curr;
     Pt2D perp_unit_test;
@@ -1108,7 +1138,11 @@ PixelRange StereoMatch::findSearchRegion (int id, std::vector<Line2D> const& sig
         for (int j = i+1; j < id; j ++)
         {
             // find the crossing point
-            pt_cross = myMATH::crossPoint(sight2D_list[i], sight2D_list[j]);
+            is_parallel = myMATH::crossPoint(pt_cross, sight2D_list[i], sight2D_list[j]);
+            if (is_parallel)
+            {
+                return std::pair(search_region, false);
+            }
 
             // cos
             perp_unit_test[0] = sight2D_list[j].unit_vector[1];
@@ -1155,7 +1189,7 @@ PixelRange StereoMatch::findSearchRegion (int id, std::vector<Line2D> const& sig
     search_region.col_min = std::max(search_region.col_min, 0);
     search_region.col_max = std::min(search_region.col_max, n_col);
 
-    return search_region;
+    return std::pair(search_region, true);
 }
 
 

test/test_myMATH.cpp

@@ -530,7 +530,9 @@ bool test_function_16 ()
     line2.pt = pt3;
     line2.unit_vector = myMATH::createUnitVector (pt3, pt4);
 
-    Pt2D cross_point = myMATH::crossPoint (line1, line2);
+    Pt2D cross_point;
+    bool is_parallel = myMATH::crossPoint (cross_point, line1, line2);
+    std::cout << "is_parallel: " << is_parallel << std::endl;
 
     if (cross_point != Pt2D(1,2))
     {