[feat] add HmdCalibratorIndustry method

ReadMe.md

@@ -5,8 +5,8 @@ Calibration tool for calibing the transformation between the glass_screen and fi
 the useful function as following:
 
  Firstly, calib the tracking cameras: fisheye camera by using  the cpp file: calib_fisheye_cam.cpp     
- 
- Then, use HMDCLAIB::HmdCalibrator hmdcalibrator to calib, you can refer the test/perform_Interaction_calib.cpp :
+  
+ Then, use HMDCLAIB::HmdCalibratorInteraction hmdcalibrator to calib, you can refer the test/perform_Interaction_calib.cpp :
 
  1. iteractive cilick the screen display and get get fisheye uv, by using the function : hmdcalibrator.GetCaibCorrespondence()
 

include/calib_hmd.h

@@ -21,6 +21,25 @@ namespace HMDCLAIB{
     Eigen::Matrix4f T_rw;
   };
   
+  struct EyeCameraInfo// industrial camera
+  {
+    cv::Size image_size;
+    Eigen::Matrix3f K_l;
+    Eigen::VectorXf D_l;       
+    Eigen::Matrix3f K_r; 
+    Eigen::VectorXf D_r;      
+    Eigen::Matrix4f T_lw;
+    Eigen::Matrix4f T_rw;
+  };
+
+  struct GlassInfo
+  {
+    float hfov_l;// degree
+    float vfov_l;// degree
+    float hfov_r;// degree
+    float vfov_r;// degree
+  };
+  
   struct CalibResultInfo
   {
     Eigen::Matrix3f K_eyel;      
@@ -29,10 +48,15 @@ namespace HMDCLAIB{
     Eigen::Matrix4f T_eyer_tracking;
   };
 
-  class HmdCalibrator{
+  enum DeviceType{
+    left = 0,
+    right =1
+  }
+
+  class HmdCalibratorInteraction{
     
     public: 
-      HmdCalibrator(const std::string& left_image_dir,const std::string& right_image_dir,const std::string& screen_uv_path, std::string image_type="png", std::string tracking_stereo_uv_path = "tracking_stereo_uv.txt"){
+      HmdCalibratorInteraction(const std::string& left_image_dir,const std::string& right_image_dir,const std::string& screen_uv_path, std::string image_type="png", std::string tracking_stereo_uv_path = "tracking_stereo_uv.txt"){
         _left_image_dir = left_image_dir;
         _right_image_dir = right_image_dir;
         _screen_uv_path = screen_uv_path;
@@ -41,7 +65,7 @@ namespace HMDCLAIB{
         _screen_uv.clear();
         _tracking_stereo_uv.clear();
       }
-      ~HmdCalibrator(){
+      ~HmdCalibratorInteraction(){
         _screen_uv.clear();
         _tracking_stereo_uv.clear();
       }
@@ -73,4 +97,32 @@ namespace HMDCLAIB{
   
   };
 
+
+  class HmdCalibratorIndustry{
+ 
+    public: 
+      explicit HmdCalibratorIndustry(TrackingCameraInfo trackingcamerainfo, EyeCameraInfo eyecamerainfo, GlassInfo glassinfo){
+        _trackingcamerainfo = std::make_shared<TrackingCameraInfo>(trackingcamerainfo);
+        _eyecamerainfo = std::make_shared<EyeCameraInfo>(eyecamerainfo);
+        _glassinfo = std::make_shared<GlassInfo>(glassinfo);
+      }
+      ~HmdCalibratorIndustry() = default;
+
+    private:
+      bool GetCaibCorrespondence(const cv::Mat& chessboard,const cv::Mat& chessboard_in_eye_Camera,cv::Size chessboard_pattern);  
+      bool PerformCalib(CalibResultInfo& result_info);     
+
+      Eigen::Matrix3f _H_screenl_eyel;
+      Eigen::Matrix3f _K_on_l;
+      Eigen::Matrix3f _K_off_l;
+
+      Eigen::Matrix3f _H_screenr_eyer;
+      Eigen::Matrix3f _K_on_r;
+      Eigen::Matrix3f _K_off_r;
+
+      std::shared_ptr<TrackingCameraInfo> _trackingcamerainfo;
+      std::shared_ptr<EyeCameraInfo>  _eyecamerainfo;
+      std::shared_ptr<GlassInfo>  _glassinfo;
+  }; 
+
 }
\ No newline at end of file

test/CMakeLists.txt

-add_executable(calib_fisheye_cam calib_fisheye_cam.cpp)
-target_link_libraries(calib_fisheye_cam
+add_executable(calib_stereo_fisheye_cam ./calib_camera_tools/calib_stereo_fisheye_cam.cpp)
+target_link_libraries(calib_stereo_fisheye_cam
                                 CALIB_GLASS
                                 ${OpenCV_LIBS}
                                 GlobImage
                                 )                   
                                
- 
-add_executable(calib_pinhole calib_pinhole.cpp)
-target_link_libraries(calib_pinhole
+add_executable(calib_stereo_pinhole ./calib_camera_tools/calib_stereo_pinhole.cpp)
+target_link_libraries(calib_stereo_pinhole
+                                CALIB_GLASS
+                                ${OpenCV_LIBS}
+                                GlobImage)  
+
+add_executable(calib_mono_pinhole ./calib_camera_tools/calib_mono_pinhole.cpp)
+target_link_libraries(calib_mono_pinhole
                       CALIB_GLASS
                       ${OpenCV_LIBS}
                       GlobImage)                                    

test/calib_pinhole.cpp → test/calib_camera_tools/calib_mono_pinhole.cpp

@@ -89,8 +89,8 @@ int main(){
    std::cout<<"error: "<<error<<std::endl;
    std::cout << "K1" << Mat(K1)<<std::endl;
    std::cout << "D1" << D1<<", size:"<<D1.size()<<" type: "<<D1.type()<<std::endl;
-   printf("Done Calibration\n");
-   printf("Starting Rectification\n");
+   printf("Done Single PineHole Calibration\n");
+   printf("Starting Single PineHole  Rectification\n");
   
 ////////////show  rectify/////////////////////////
     cv::Mat remapx1,remapx2,remapy1,remapy2;

test/calib_fisheye_cam.cpp → test/calib_camera_tools/calib_stereo_fisheye_cam.cpp

@@ -102,8 +102,9 @@ int main(){
    std::cout << "D2" << D2<<std::endl;
    std::cout << "R" << Mat(R)<<std::endl;
    std::cout << "T" << T<<std::endl;
-   printf("Done Calibration\n");
-   printf("Starting Rectification\n");
+   printf("Done FishEye Stereo Calibration\n");
+   printf("Starting  FishEye Stereo  Rectification\n");
+
    cv::Mat R1, R2, P1, P2, Q;
    cv::fisheye::stereoRectify(K1, D1, K2, D2, img1.size(), R, T, R1, R2, P1, P2, 
 Q, cv::CALIB_ZERO_DISPARITY, img1.size());

test/calib_camera_tools/calib_stereo_pinhole.cpp

+#include <iostream>
+#include "GlobImage.h"
+#include <opencv2/core/core.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <stdio.h>
+#include <iostream>
+ 
+
+using namespace std;
+using namespace cv;
+
+vector< vector< Point3d > > object_points;
+vector< vector< Point2f > > imagePoints1, imagePoints2;
+vector< Point2f > corners1, corners2;
+vector< vector< Point2d > > left_img_points, right_img_points;
+
+Mat img1, img2, gray1, gray2, spl1, spl2;
+ 
+void load_image_points(int board_width, int board_height, float square_size, const std::vector<std::string>& left_file,
+const std::vector<std::string>& right_file) {
+  Size board_size = Size(board_width, board_height);
+  int board_n = board_width * board_height;
+
+  for (int i = 0; i < left_file.size(); i++) {
+
+    img1 = imread(left_file[i]);
+    img2 = imread(right_file[i]);
+    cv::cvtColor(img1,gray1,cv::COLOR_RGB2GRAY);
+    cv::cvtColor(img2,gray2,cv::COLOR_RGB2GRAY);
+        bool found1 = false, found2 = false;
+
+    found1 = cv::findChessboardCorners(img1, board_size, corners1,
+    cv::CALIB_CB_ADAPTIVE_THRESH |  cv::CALIB_CB_FILTER_QUADS);
+    found2 = cv::findChessboardCorners(img2, board_size, corners2,
+    cv::CALIB_CB_ADAPTIVE_THRESH |  cv::CALIB_CB_FILTER_QUADS);
+    if (found1)
+    {
+        std::cout<<"found left"<< left_file[i]<<std::endl;
+      cv::cornerSubPix(gray1, corners1, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(  cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.1));
+      cv::drawChessboardCorners(gray1, board_size, corners1, found1);
+    }
+    if (found2)
+    {
+      std::cout<<"found right"<< right_file[i]<<std::endl;
+      cv::cornerSubPix(gray2, corners2, cv::Size(5, 5), cv::Size(-1, -1),cv::TermCriteria(  cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.1));
+      cv::drawChessboardCorners(gray2, board_size, corners2, found2);
+    }
+    vector<cv::Point3d> obj;
+    for( int i = 0; i < board_height; ++i )
+      for( int j = 0; j < board_width; ++j )
+        obj.push_back(Point3d(double( (float)j * square_size ), double( (float)i * square_size ), 0));
+    
+    if (found1 && found2) {
+      cout << i << ". Found corners!" << endl;
+      imagePoints1.push_back(corners1);
+      imagePoints2.push_back(corners2);
+      object_points.push_back(obj);
+      if(i==left_file.size()-1){
+        cv::imshow("gray1",gray1);
+        cv::imshow("gray2",gray2);
+        cv::waitKey(0);
+      }
+    }
+  }
+
+  for (int i = 0; i < imagePoints1.size(); i++) {
+    vector< Point2d > v1, v2;
+    for (int j = 0; j < imagePoints1[i].size(); j++) {
+      v1.push_back(Point2d((double)imagePoints1[i][j].x, (double)imagePoints1[i][j].y));
+      v2.push_back(Point2d((double)imagePoints2[i][j].x, (double)imagePoints2[i][j].y));
+    }
+    left_img_points.push_back(v1);
+    right_img_points.push_back(v2);
+  }
+}
+
+int main(){
+  cv::String left_pattern = "/home/hxc/Camera/cam0/data/*.png";
+  cv::String  right_pattern = "/home/hxc/Camera/cam1/data/*.png";
+  std::vector<std::string> left_file = GLOB::GlobImage::ReadImage(left_pattern);  
+  std::vector<std::string> right_file = GLOB::GlobImage::ReadImage(right_pattern); 
+  int board_width = 11;
+  int board_height = 8;
+  float square_size = 0.035;
+  load_image_points( board_width,board_height,square_size, left_file, right_file);
+  cv::Matx33d K1, K2, R;
+  cv::Vec3d T;
+  cv::Vec4d D1, D2;
+  int flag = 0;
+  flag |= cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
+  flag |= cv::fisheye::CALIB_CHECK_COND;
+  flag |= cv::fisheye::CALIB_FIX_SKEW;
+ 
+
+  cv::Mat E,F;                                  
+  double error =  cv::stereoCalibrate(object_points, left_img_points, right_img_points,
+      K1, D1, K2, D2, img1.size(), R, T,E,F, flag,
+      cv::TermCriteria(3, 12, 0));
+   std::cout<<"error: "<<error<<std::endl;
+   std::cout << "K1" << Mat(K1)<<std::endl;
+   std::cout << "K2" << Mat(K2)<<std::endl;
+   std::cout << "D1" << D1<<std::endl;
+   std::cout << "D2" << D2<<std::endl;
+   std::cout << "R" << Mat(R)<<std::endl;
+   std::cout << "T" << T<<std::endl;
+   printf("Done PineHole Stereo Calibration\n");
+   printf("Starting  PineHole Stereo  Rectification\n");
+   cv::Mat R1, R2, P1, P2, Q;
+ 
+   cv::stereoRectify(K1, D1, K2, D2, img1.size(), R, T, R1, R2, P1, P2, 
+Q, cv::CALIB_ZERO_DISPARITY);
+
+  std::cout << "R1" << R1<<std::endl;
+  std::cout << "R2" << R2<<std::endl;
+  std::cout << "P1" << P1<<std::endl;
+  std::cout << "P2" << P2<<std::endl;
+  std::cout << "Q" << Q<<std::endl;
+  std::cout<<"baseline: "<<1/Q.at<double>(3,2)<<endl;
+ 
+////////////show  rectify/////////////////////////
+    cv::Mat remapx1,remapx2,remapy1,remapy2;
+    cv::initUndistortRectifyMap(K1, D1, R1,K1, img1.size(), CV_16SC2, remapx1, remapy1);
+    cv::initUndistortRectifyMap(K2, D2, R2,K2, img1.size(), CV_16SC2, remapx2, remapy2);
+
+    cv::Mat grayImageL,grayImageR;
+    cv::Mat rgbImageL = imread(left_file[0]);
+    cvtColor(rgbImageL, grayImageL, cv::COLOR_RGB2GRAY);
+
+     cv::Mat rgbImageR = imread(right_file[0]);
+    cvtColor(rgbImageR, grayImageR, cv::COLOR_RGB2GRAY);
+
+    imshow("ImageL Before Rectify", grayImageL);
+    imshow("ImageR Before Rectify", grayImageR);
+
+    cv::Mat rectifyImageL,rectifyImageR;
+    remap(grayImageL, rectifyImageL, remapx1, remapy1, INTER_LINEAR);
+    remap(grayImageR, rectifyImageR, remapx2, remapy2, INTER_LINEAR);
+
+    Mat rgbRectifyImageL, rgbRectifyImageR;
+    cvtColor(rectifyImageL, rgbRectifyImageL, cv::COLOR_GRAY2RGB);
+    cvtColor(rectifyImageR, rgbRectifyImageR, cv::COLOR_GRAY2RGB);
+    imshow("ImageL After Rectify", rgbRectifyImageL);
+    imshow("ImageR After Rectify", rgbRectifyImageR);
+
+    cv::Mat canvas;
+    int w, h;
+    w = img1.cols;
+    h = img1.rows;
+    canvas.create(h, w * 2, CV_8UC3);
+    cv::Mat canvasPart = canvas(Rect(w * 0, 0, w, h)); 
+    rgbRectifyImageL.copyTo(canvasPart); 
+    canvasPart = canvas(Rect(w, 0, w, h));   
+    rgbRectifyImageR.copyTo(canvasPart);
+    for (int i = 0; i < canvas.rows; i += 16)
+        cv::line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);
+    cv::imshow("rectified", canvas);
+    cv::waitKey(0);
+
+}
\ No newline at end of file

test/perform_Interaction_calib.cpp

@@ -4,7 +4,7 @@ int main(){
   std::string left_image_dir = "/home/hxc/calib/mav0/cam0/data";
   std::string right_image_dir = "/home/hxc/calib/mav0/cam1/data";
   std::string screen_uv_path = "/home/hxc/calib/mav0/xy/data.txt";
-  HMDCLAIB::HmdCalibrator hmdcalibrator(left_image_dir,right_image_dir,screen_uv_path);
+  HMDCLAIB::HmdCalibratorInteraction hmdcalibrator(left_image_dir,right_image_dir,screen_uv_path);
   hmdcalibrator.GetCaibCorrespondence();
   HMDCLAIB::TrackingCameraInfo cameraInfo;
   int mono_image_width  = 640;

test/perform_industrycam_calib.cpp

+#include "calib_hmd.h"
+
+int main(){
+  HMDCLAIB::TrackingCameraInfo trackingcameraInfo;
+  HMDCLAIB::EyeCameraInfo eyecameraInfo;//industry camera
+  
+}  
\ No newline at end of file