Merge pull request #12 from clockj/shijie

Shijie

Author: clockj

demo/python/demo_camera_refraction.ipynb

@@ -2,7 +2,7 @@
  "cells": [
   {
    "cell_type": "code",
-   "execution_count": 1,
+   "execution_count": null,
    "metadata": {},
    "outputs": [],
    "source": [
@@ -2199,7 +2199,7 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "OpenLPTGUI",
+   "display_name": "OpenLPT",
    "language": "python",
    "name": "python3"
   },

demo/python/demo_lpt.ipynb

@@ -432,7 +432,7 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "OpenLPTGUI",
+   "display_name": "OpenLPT",
    "language": "python",
    "name": "python3"
   },

demo/python/demo_math.ipynb

@@ -14,6 +14,7 @@
 #include <iostream>
 #include <sstream>
 #include <limits>
+#include <tuple>
 
 #include "myMATH.h"
 #include "Matrix.h"
@@ -54,10 +55,24 @@ struct PolyParam
                               // dv/dvar2 coeff,x_power, y_power, z_power
 };
 
+struct PinPlateParam: public PinholeParam
+{ 
+    Plane3D plane; // reference refractive plane (farthest plane to camera), normal vector is pointing away from the camera
+    Pt3D pt3d_closest; // closest point to the camera
+    std::vector<double> refract_array; // refractive index array (from farthest to nearest)
+    std::vector<double> w_array; // width of the refractive plate (from farthest to nearest)
+    int n_plate; // number of refractive plates
+    double proj_tol; // projection tolerance squatre
+    int proj_nmax; // maximum number of iterations for projection
+    double lr; // learning rate
+    double refract_ratio_max; // max(refract_array[0]/[i])
+};
+
 enum CameraType
 {
     PINHOLE = 0,
-    POLYNOMIAL
+    POLYNOMIAL,
+    PINPLATE
 };
 
 class Camera
@@ -66,6 +81,7 @@ class Camera
     CameraType   _type;
     PinholeParam _pinhole_param;
     PolyParam    _poly_param; 
+    PinPlateParam _pinplate_param;
 
     Camera ();
     Camera (const Camera& c); // Camera deep copy
@@ -97,15 +113,17 @@ class Camera
     //            //
     // Projection //
     //            //
-    Pt2D project (Pt3D const& pt_world) const;
+    Pt2D project (Pt3D const& pt_world, bool is_print_detail = false) const;
 
     // Project world coordinate [mm] to image coordinate [mm]: 
     //  (xw,yw,zw) -> (x,y,z) -> (Xu,Yu,0)
     //  (x,y,z) = R @ (xw,yw,zw) + T
     //  (Xu,Yu,0) = (fx*x/z + cx, fy*y/z + cy)
     // input: pt_world: point location in world coordinate 
     // output
-    Pt2D worldToUndistImg (Pt3D const& pt_world) const;
+    Pt2D worldToUndistImg (Pt3D const& pt_world, PinholeParam const& param) const;
+
+    std::tuple<bool, Pt3D, double> refractPlate (Pt3D const& pt_world) const;
 
     // Project Image in camera coordinate [mm] to pixel: 
     //  (Xu,Yu,0) -> (Xd,Yd,0) -> (Xf,Yf,0)
@@ -117,7 +135,7 @@ class Camera
     // | 
     // |
     // \/ y direction (downwards)
-    Pt2D distort (Pt2D const& pt_img_undist) const;
+    Pt2D distort (Pt2D const& pt_img_undist, PinholeParam const& param) const;
 
     // Polynomial model 
     // Project world coordinate [mm] to distorted image [px]:
@@ -135,14 +153,16 @@ class Camera
     // input: pt_pix: point location in pixel unit on image 
     //        (no use of z coordinate of pt_pix)
     // output: Matrix (camera coordinate) (Xu,Yu,0)
-    Pt2D undistort (Pt2D const& pt_img_dist) const;
+    Pt2D undistort (Pt2D const& pt_img_dist, PinholeParam const& param) const;
 
     // Project image coordinate [mm] to world coordinate [mm]: 
     //  (Xu,Yu,0) -> (x,y,z) -> (xw,yw,zw)
     // input: pt_img_undist: undistorted image coordinate 
     // output: line of sight
     Line3D pinholeLine (Pt2D const& pt_img_undist) const;
 
+    Line3D pinplateLine (Pt2D const& pt_img_undist) const;
+
     // Polynomial Model
     Pt3D polyImgToWorld (Pt2D const& pt_img_dist, double plane_world) const;
 

inc/libMath/Camera.h

@@ -150,6 +150,13 @@ struct Line2D
     Pt2D unit_vector;
 };
 
+// Structure to store 3D plane
+struct Plane3D
+{
+    Pt3D pt;
+    Pt3D norm_vector;
+};
+
 // Image: matrix with double type
 // Image(row_id, col_id) = intensity
 // row_id = img_y, col_id = img_x

inc/libMath/Matrix.h

@@ -135,6 +135,10 @@ double dist2 (Pt3D const& pt, Line3D const& line);
 double dist2 (Pt2D const& pt, Line2D const& line);
 
 
+// Calculate the distance square between point and plane
+double dist2 (Pt3D const& pt, Plane3D const& plane);
+
+
 // Calculate the distance between two points
 double dist (Pt3D const& pt1, Pt3D const& pt2);
 double dist (Pt2D const& pt1, Pt2D const& pt2);
@@ -145,6 +149,10 @@ double dist (Pt3D const& pt, Line3D const& line);
 double dist (Pt2D const& pt, Line2D const& line);
 
 
+// Calculate the distance between point and plane
+double dist (Pt3D const& pt, Plane3D const& plane);
+
+
 // Triangulation
 void triangulation (Pt3D& pt_world, double& error,
                     std::vector<Line3D> const& line_of_sight_list);
@@ -153,6 +161,8 @@ void triangulation (Pt3D& pt_world, double& error,
 // Find cross points of two 2d lines
 bool crossPoint (Pt2D& pt2d, Line2D const& line1, Line2D const& line2);
 
+// Find cross points of 3d line and 3d plane
+bool crossPoint (Pt3D& pt3d, Line3D const& line, Plane3D const& plane);
 
 // Create identity matrix 
 template<class T>

inc/libMath/myMATH.h

@@ -60,16 +60,53 @@ void init_Camera(py::module &m)
             );
         })
         .doc() = "PolyParam struct";
+    
+    py::class_<PinPlateParam, PinholeParam>(m, "PinPlateParam")
+        .def(py::init<>())
+        .def_readwrite("plane", &PinPlateParam::plane)
+        .def_readwrite("pt3d_closest", &PinPlateParam::pt3d_closest)
+        .def_readwrite("refract_array", &PinPlateParam::refract_array)
+        .def_readwrite("w_array", &PinPlateParam::w_array)
+        .def_readwrite("n_plate", &PinPlateParam::n_plate)
+        .def_readwrite("proj_tol", &PinPlateParam::proj_tol)
+        .def_readwrite("proj_nmax", &PinPlateParam::proj_nmax)
+        .def_readwrite("lr", &PinPlateParam::lr)
+        .def_readwrite("refract_ratio_max", &PinPlateParam::refract_ratio_max)
+        .def("to_dict", [](PinPlateParam const& self){
+            return py::dict(
+                "n_row"_a=self.n_row, 
+                "n_col"_a=self.n_col, 
+                "cam_mtx"_a=self.cam_mtx, 
+                "is_distorted"_a=self.is_distorted, 
+                "n_dist_coeff"_a=self.n_dist_coeff, 
+                "dist_coeff"_a=self.dist_coeff, 
+                "r_mtx"_a=self.r_mtx, 
+                "t_vec"_a=self.t_vec, 
+                "r_mtx_inv"_a=self.r_mtx_inv, 
+                "t_vec_inv"_a=self.t_vec_inv, 
+                "plane"_a=self.plane,  
+                "refract_array"_a=self.refract_array, 
+                "w_array"_a=self.w_array, 
+                "n_plate"_a=self.n_plate, 
+                "proj_tol"_a=self.proj_tol, 
+                "proj_nmax"_a=self.proj_nmax,
+                "lr"_a=self.lr,
+                "refract_ratio_max"_a=self.refract_ratio_max
+            );
+        })
+        .doc() = "PinPlateParam struct";
 
     py::enum_<CameraType>(m, "CameraType")
         .value("PINHOLE", CameraType::PINHOLE)
         .value("POLYNOMIAL", CameraType::POLYNOMIAL)
+        .value("PINPLATE", CameraType::PINPLATE)
         .export_values();
 
     py::class_<Camera>(m, "Camera")
         .def_readwrite("_type", &Camera::_type)
         .def_readwrite("_pinhole_param", &Camera::_pinhole_param)
         .def_readwrite("_poly_param", &Camera::_poly_param)
+        .def_readwrite("_pinplate_param", &Camera::_pinplate_param)
         .def(py::init<>())
         .def(py::init<const Camera&>())
         .def(py::init<std::string>())
@@ -81,7 +118,9 @@ void init_Camera(py::module &m)
         .def("rmtxTorvec", &Camera::rmtxTorvec)
         .def("getNRow", &Camera::getNRow)
         .def("getNCol", &Camera::getNCol)
-        .def("project", &Camera::project)
+        .def("project", [](Camera const& self, Pt3D const& pt3d, bool is_print_detail){
+            return self.project(pt3d, is_print_detail);
+        }, py::arg("pt3d"), py::arg("is_print_detail")=false)
         .def("project", [](Camera const& self, std::vector<Pt3D> const& pt3d_list){
             std::vector<Pt2D> pt2d_list(pt3d_list.size());
             #pragma omp parallel for
@@ -92,7 +131,28 @@ void init_Camera(py::module &m)
             return pt2d_list;
         })
         .def("worldToUndistImg", &Camera::worldToUndistImg)
+        .def("worldToUndistImg", [](Camera const& self, std::vector<Pt3D> const& pt3d_list, PinholeParam const& param){
+            int npts = pt3d_list.size();
+            std::vector<Pt2D> pt2d_list(npts);
+            #pragma omp parallel for
+            for (int i = 0; i < npts; i++)
+            {
+                pt2d_list[i] = self.worldToUndistImg(pt3d_list[i], param);
+            }
+            return pt2d_list;
+        })
+        .def("refractPlate", &Camera::refractPlate)
         .def("distort", &Camera::distort)
+        .def("distort", [](Camera const& self, std::vector<Pt2D> const& pt2d_list, PinholeParam const& param){
+            int npts = pt2d_list.size();
+            std::vector<Pt2D> pt2d_list_dist(npts);
+            #pragma omp parallel for
+            for (int i = 0; i < npts; i++)
+            {
+                pt2d_list_dist[i] = self.distort(pt2d_list[i], param);
+            }
+            return pt2d_list_dist;
+        })
         .def("polyProject", &Camera::polyProject)
         .def("lineOfSight", &Camera::lineOfSight)
         .def("lineOfSight", [](Camera const& self, std::vector<Pt2D> const& pt2d_list){
@@ -105,14 +165,26 @@ void init_Camera(py::module &m)
             return line_list;
         })
         .def("undistort", &Camera::undistort)
+        .def("undistort", [](Camera const& self, std::vector<Pt2D> const& pt2d_list, PinholeParam const& param){
+            int npts = pt2d_list.size();
+            std::vector<Pt2D> pt2d_list_undist(npts);
+            #pragma omp parallel for
+            for (int i = 0; i < npts; i++)
+            {
+                pt2d_list_undist[i] = self.undistort(pt2d_list[i], param);
+            }
+            return pt2d_list_undist;
+        })
         .def("pinholeLine", &Camera::pinholeLine)
         .def("polyImgToWorld", &Camera::polyImgToWorld)
         .def("polyLineOfSight", &Camera::polyLineOfSight)
+        .def("pinplateLine", &Camera::pinplateLine)
         .def("to_dict", [](Camera const& self){
             return py::dict(
                 "_type"_a=self._type, 
                 "_pinhole_param"_a=self._pinhole_param, 
-                "_poly_param"_a=self._poly_param
+                "_poly_param"_a=self._poly_param,
+                "_pinplate_param"_a=self._pinplate_param
             );
         })
         .doc() = "Camera class";

src/pybind_OpenLPT/pyCamera.cpp

@@ -3,6 +3,7 @@
 #include <pybind11/stl.h>
 #include <iostream>
 #include <string>
+#include <tuple>
 #include <sstream>
 #include <fstream>
 

src/pybind_OpenLPT/pyInterface.cpp

@@ -244,6 +244,19 @@ void init_Matrix(py::module &m)
         })
         .doc() = "Line2D struct";
 
+    py::class_<Plane3D>(m, "Plane3D")
+        .def(py::init<>())
+        .def(py::init<const Pt3D&, const Pt3D&>())
+        .def_readwrite("pt", &Plane3D::pt)
+        .def_readwrite("norm_vector", &Plane3D::norm_vector)
+        .def("to_dict", [](Plane3D const& self){
+            return py::dict(
+                "pt"_a=self.pt, 
+                "norm_vector"_a=self.norm_vector
+            );
+        })
+        .doc() = "Plane3D struct";
+
     py::class_<Image, Matrix<double>>(m, "Image")
         .def(py::init<>())
         .def(py::init<int, int, double>())