├── EpipolarRectify ├── test_img.jpg ├── CMakeLists.txt ├── calib.yaml └── rectify.cpp ├── CMakeLists.txt ├── README.md ├── README.zh-CN.md ├── DetectCorner ├── chessboard.h ├── findCorner.h ├── chessboard.cpp └── findCorner.cpp ├── cout_style.h ├── TS.h ├── multi_calib.h ├── TS.cpp ├── main.cpp └── multi_calib.cpp /EpipolarRectify/test_img.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/imuncle/TSCM_Calib/main/EpipolarRectify/test_img.jpg -------------------------------------------------------------------------------- /EpipolarRectify/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(EpipolarRectify) 3 | 4 | add_compile_options(-std=c++11) 5 | set(CMAKE_BUILD_TYPE "Release") 6 | find_package(OpenCV REQUIRED) 7 | 8 | include_directories( 9 | DetectCorner 10 | ${OpenCV_INCLUDE_DIRECTORIES} 11 | ) 12 | 13 | add_executable(${PROJECT_NAME} 14 | rectify.cpp 15 | ) 16 | 17 | target_link_libraries(${PROJECT_NAME} 18 | ${OpenCV_LIBRARIES} 19 | ) 20 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(TSCM_Calib) 3 | 4 | add_compile_options(-std=c++11) 5 | set(CMAKE_BUILD_TYPE "Release") 6 | find_package(OpenCV REQUIRED) 7 | find_package( Ceres REQUIRED) 8 | 9 | include_directories( 10 | DetectCorner 11 | ${OpenCV_INCLUDE_DIRECTORIES} 12 | ${CERES_INCLUDE_DIRS} 13 | ) 14 | 15 | add_executable(${PROJECT_NAME} 16 | main.cpp 17 | TS.cpp 18 | multi_calib.cpp 19 | DetectCorner/findCorner.cpp 20 | DetectCorner/chessboard.cpp 21 | ) 22 | 23 | target_link_libraries(${PROJECT_NAME} 24 | ${OpenCV_LIBRARIES} 25 | ${CERES_LIBRARIES} 26 | ) 27 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | English | [中文简体](README.zh-CN.md) 2 | 3 | # TSCM-Calib 4 | 5 | The multi-camera calibration tool based on Triple Sphere Camera Model. 6 | 7 | ## Dependence 8 | 9 | * OpenCV 10 | 11 | you can install it by `sudo apt install libopencv-dev`. 12 | 13 | * Ceres-Solver 14 | 15 | you can install it by `sudo apt install libceres-dev`. 16 | 17 | ## build 18 | 19 | ```bash 20 | mkdir build 21 | cd build 22 | cmake .. 23 | make 24 | ``` 25 | 26 | ## Usage 27 | 28 | Change the image path in `main` function. The tool will output the calibration result in yaml format, saved in `build` directory. 29 | 30 | > Note: press `Esc` or `Q` to exit. 31 | 32 | > Note: The filenames should be exactly same for the pictures captured simulately from different cameras. 33 | 34 | > Note: The Triple Sphere Camera Model can be found in [OmniVidar](https://openaccess.thecvf.com/content/CVPR2023/papers/Xie_OmniVidar_Omnidirectional_Depth_Estimation_From_Multi-Fisheye_Images_CVPR_2023_paper.pdf). 35 | -------------------------------------------------------------------------------- /README.zh-CN.md: -------------------------------------------------------------------------------- 1 | [English](README.md) | 中文简体 2 | 3 | # TSCM-Calib 4 | 5 | 基于`Triple Sphere Camera Model`的多相机标定工具。 6 | 7 | ## Dependence 8 | 9 | * OpenCV 10 | 11 | 在ubuntu下安装： `sudo apt install libopencv-dev`. 12 | 13 | * Ceres-Solver 14 | 15 | 在ubuntu下安装： `sudo apt install libceres-dev`. 16 | 17 | ## build 18 | 19 | ```bash 20 | mkdir build 21 | cd build 22 | cmake .. 23 | make 24 | ``` 25 | 26 | ## Usage 27 | 28 | 修改`main`函数里的图片路径，可根据自己的需求增删相机数。标定结束后会在`build`目录下生成`yaml`格式的标定文件。 29 | 30 | `EpipolarRecity`文件夹下是用于四目鱼眼相机极线校正的小工具，对应于了论文[OmniVidar](https://openaccess.thecvf.com/content/CVPR2023/papers/Xie_OmniVidar_Omnidirectional_Depth_Estimation_From_Multi-Fisheye_Images_CVPR_2023_paper.pdf)的3.2小节。 31 | 32 | > Note: 标定结束后会出现一个多相机位姿可视化窗口，按下键盘上的`Esc`或`Q`键，或者直接关闭该窗口可以退出程序。 33 | 34 | > Note: 不同相机在同一时刻拍摄的照片的文件名应当保持一致。 35 | 36 | > Note: 相机模型的投影方程和逆投影方程可以参考这篇论文： [OmniVidar](https://openaccess.thecvf.com/content/CVPR2023/papers/Xie_OmniVidar_Omnidirectional_Depth_Estimation_From_Multi-Fisheye_Images_CVPR_2023_paper.pdf). 37 | -------------------------------------------------------------------------------- /DetectCorner/chessboard.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARD_H 2 | #define CHESSBOARD_H 3 | 4 | #include 5 | 6 | struct Corner_t 7 | { 8 | std::vector p; 9 | std::vector v1; 10 | std::vector v2; 11 | std::vector score; 12 | }; 13 | 14 | std::vector chessboardsFromCorners(struct Corner_t corners); 15 | cv::Mat initChessboard(struct Corner_t corners, int idx); 16 | double average(double* a, int length); 17 | double stdd(double* a, int length, double mean); 18 | void directionalNeighbor(int idx, cv::Point2d v, cv::Mat chessboard, struct Corner_t corners, int* neighbor_idx, double* min_dist); 19 | double chessboardEnergy(cv::Mat chessboard, struct Corner_t corners); 20 | cv::Mat growChessboard(cv::Mat chessboard, struct Corner_t corners, int boarder_type); 21 | cv::Point2d predictCorners(cv::Point2d p1, cv::Point2d p2, cv::Point2d p3); 22 | std::vector assignClosestCorners(std::vector cand, std::vector pred); 23 | void plotChessboards(cv::Mat img, std::vector chessboards, struct Corner_t corners); 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /DetectCorner/findCorner.h: -------------------------------------------------------------------------------- 1 | #ifndef FIND_CORNER_H 2 | #define FIND_CORNER_H 3 | 4 | #include 5 | #include "chessboard.h" 6 | 7 | struct Template_t 8 | { 9 | cv::Mat a1; 10 | cv::Mat a2; 11 | cv::Mat b1; 12 | cv::Mat b2; 13 | }; 14 | 15 | struct Chessboarder_t 16 | { 17 | struct Corner_t corners; 18 | std::vector chessboard; 19 | }; 20 | 21 | struct Chessboarder_t findCorner(cv::Mat img, int sigma); 22 | void secondDerivCornerMetric(cv::Mat I, int sigma, cv::Mat* cxy, cv::Mat* c45, cv::Mat* Ix, cv::Mat* Iy, cv::Mat* Ixy, cv::Mat* I_45_45); 23 | std::vector nonMaximumSuppression(cv::Mat img, int n, double tau, int margin); 24 | struct Corner_t getOrientations(cv::Mat img_angle, cv::Mat img_weight, struct Corner_t corners, int r); 25 | std::vector edgeOrientations(cv::Mat img_angle, cv::Mat img_weight); 26 | std::vector findModesMeanShift(cv::Mat hist, int sigma); 27 | double cornerCorrelationScore(cv::Mat img, cv::Mat img_weight, cv::Point2d v1, cv::Point2d v2); 28 | struct Corner_t scoreCorners(cv::Mat img, cv::Mat img_weight, struct Corner_t corners, int* radius); 29 | struct Corner_t subPixelLocation(cv::Mat metric, struct Corner_t corners); 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /EpipolarRectify/calib.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | cam0: !!opencv-matrix 4 | rows: 1 5 | cols: 9 6 | dt: d 7 | data: [ 4.3129641731951233e+02, 4.3077528857601646e+02, 8 | 6.4653015901902177e+02, 5.2120451427825685e+02, 9 | -2.7125775332873053e-01, -8.7861849854000834e-02, 10 | 5.6023435889162265e-01, 0., 0. ] 11 | Twc0: !!opencv-matrix 12 | rows: 3 13 | cols: 4 14 | dt: d 15 | data: [ 1., 0., 0., 0., 0., 1., 0., 0., 0., 0., 1., 0. ] 16 | cam1: !!opencv-matrix 17 | rows: 1 18 | cols: 9 19 | dt: d 20 | data: [ 4.3366730337860304e+02, 4.3377366718652252e+02, 21 | 6.5043289767844408e+02, 5.3217610796339648e+02, 22 | -2.5567341708788405e-01, -8.0998645840408265e-02, 23 | 5.6043293184809229e-01, 0., 0. ] 24 | Twc1: !!opencv-matrix 25 | rows: 3 26 | cols: 4 27 | dt: d 28 | data: [ 5.0160892202284401e-03, -1.2446352011191332e-02, 29 | 9.9990995953163087e-01, 3.1111069091426958e+02, 30 | -4.9652802236104215e-02, 9.9868604260337857e-01, 31 | 1.2680202652341772e-02, -3.2581972269830493e+00, 32 | -9.9875394271013351e-01, -4.9711936502369769e-02, 33 | 4.3915020377227887e-03, -3.0250006677005149e+02 ] 34 | cam2: !!opencv-matrix 35 | rows: 1 36 | cols: 9 37 | dt: d 38 | data: [ 4.4342294254852777e+02, 4.4269548663571004e+02, 39 | 6.5012232252239130e+02, 5.1864631548858017e+02, 40 | -2.3275919129762454e-01, -8.7007852953879805e-02, 41 | 5.6302432477866149e-01, 0., 0. ] 42 | Twc2: !!opencv-matrix 43 | rows: 3 44 | cols: 4 45 | dt: d 46 | data: [ -9.9912757728632307e-01, -4.1543088687854141e-02, 47 | 4.2727143873527804e-03, -4.5684542332524316e+00, 48 | -4.1723107975334954e-02, 9.9738123070453755e-01, 49 | -5.9075061567300073e-02, -3.5570993658832819e+01, 50 | -1.8073646121761103e-03, -5.9201794065508177e-02, 51 | -9.9824439943962817e-01, -6.1759896466830685e+02 ] 52 | cam3: !!opencv-matrix 53 | rows: 1 54 | cols: 9 55 | dt: d 56 | data: [ 4.3725205336966712e+02, 4.3738251105641092e+02, 57 | 6.4148306394889755e+02, 5.5309342913742341e+02, 58 | -2.6287894613485679e-01, -8.5693153628330507e-02, 59 | 5.6177801764159951e-01, 0., 0. ] 60 | Twc3: !!opencv-matrix 61 | rows: 3 62 | cols: 4 63 | dt: d 64 | data: [ -1.0309658021738319e-02, -5.9375126415255344e-02, 65 | -9.9818250100602735e-01, -3.0116931471069142e+02, 66 | 5.1486531371731037e-02, 9.9687992136190828e-01, 67 | -5.9829419793145176e-02, -2.9127716209064634e+01, 68 | 9.9862047247129027e-01, -5.2009775510466122e-02, 69 | -7.2204717690991169e-03, -3.0393000777920400e+02 ] 70 | -------------------------------------------------------------------------------- /cout_style.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace std; 6 | 7 | const std::vector TERMINAL_CONTROL={std::string("\033[0m"), // 关闭属性 8 | std::string("\033[1m"), // 设置高亮 9 | std::string("\033[3m"), // 斜体 10 | std::string("\033[4m"), // 下划线 11 | std::string("\033[5m"), // 闪烁（无效） 12 | std::string("\033[7m"), // 反显 13 | std::string("\033[8m"), // 消隐 14 | std::string("\033[30m"), // 设置前景色:黑色 15 | std::string("\033[40m"), // 设置背景色:黑色 16 | std::string("\033[31m"), // 设置前景色:深红 17 | std::string("\033[41m"), // 设置背景色:深红 18 | std::string("\033[32m"), // 设置前景色:绿色 19 | std::string("\033[42m"), // 设置背景色:绿色 20 | std::string("\033[33m"), // 设置前景色:黄色 21 | std::string("\033[43m"), // 设置背景色:黄色 22 | std::string("\033[34m"), // 设置前景色:蓝色 23 | std::string("\033[44m"), // 设置背景色:蓝色 24 | std::string("\033[35m"), // 设置前景色:紫色 25 | std::string("\033[45m"), // 设置背景色:紫色 26 | std::string("\033[36m"), // 设置前景色:深绿色 27 | std::string("\033[46m"), // 设置背景色:深绿色 28 | std::string("\033[37m"), // 设置前景色:白色 29 | std::string("\033[47m"), // 设置背景色:白色 30 | std::string("\033[nA"), // 光标上移n行 31 | std::string("\033[nB"), // 光标下移n行 32 | std::string("\033[nC"), // 光标右移n列 33 | std::string("\033[nD"), // 光标左移n列 34 | std::string("\033[y;xH"), // 设置光标位置(无效) 35 | std::string("\033[2J"), // 清屏 36 | std::string("\033[K"), // 清除从光标道行尾的内容 37 | std::string("\033[s"), // 保存光标位置 38 | std::string("\033[u"), // 恢复光标位置 39 | std::string("\033[?25l"), // 隐藏光标 40 | std::string("\033[?25h"), // 显示光标 41 | }; 42 | 43 | inline std::ostream& blue(std::ostream &s) 44 | { 45 | // s << "\033[0m\033[34m\033[1m"; // ok 46 | s << "\033[0;1;34m"; 47 | return s; 48 | } 49 | 50 | inline std::ostream& red(std::ostream &s) 51 | { 52 | // s << "\033[0m\033[31m\033[1m"; // ok 53 | s << "\033[0;1;31m"; 54 | return s; 55 | } 56 | 57 | inline std::ostream& green(std::ostream &s) 58 | { 59 | // s << "\033[0m\033[32m\033[1m"; // ok 60 | s << "\033[0;1;32m"; 61 | return s; 62 | } 63 | 64 | inline std::ostream& yellow(std::ostream &s) 65 | { 66 | // s << "\033[0m\033[33m\033[1m"; // ok 67 | s << "\033[0;1;33m"; 68 | return s; 69 | } 70 | 71 | inline std::ostream& white(std::ostream &s) 72 | { 73 | // s << "\033[0m\033[37m\033[1m"; // ok 74 | s << "\033[0;1;37m"; 75 | return s; 76 | } 77 | 78 | 79 | inline std::ostream& reset(std::ostream& s) 80 | { 81 | s << "\033[0m"; 82 | return s; 83 | } 84 | -------------------------------------------------------------------------------- /TS.h: -------------------------------------------------------------------------------- 1 | #ifndef TS_H 2 | #define TS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | class TripleSphereCamera 9 | { 10 | public: 11 | TripleSphereCamera(); 12 | TripleSphereCamera(double fx, double fy, double cx, double cy, double xi, double lamda, double alpha); 13 | bool calibrate(const std::vector> pixels, 14 | std::vector has_chessboard, 15 | const std::vector& worlds, 16 | const cv::Size img_size, 17 | const cv::Size chessboard_num); 18 | void Reproject(const std::vector& worlds, cv::Mat Rt, std::vector& pixels); 19 | void undistort(double fx, double fy, double cx, double cy, cv::Size img_size, cv::Mat& mapx, cv::Mat& mapy); 20 | double cx() {return cx_;} 21 | double cy() {return cy_;} 22 | double fx() {return fx_;} 23 | double fy() {return fy_;} 24 | double xi() {return xi_;} 25 | double lamda() {return lamda_;} 26 | double alpha() {return alpha_;} 27 | double b() {return b_;} 28 | double c() {return c_;} 29 | std::vector Rt() {return Rt_;} 30 | cv::Mat Rt(int id) {return Rt_[id];} 31 | void setRt(std::vector Rts) {Rt_ = Rts;} 32 | void setPixels(std::vector> pixels) {pixels_ = pixels;} 33 | std::vector> pixels() {return pixels_;} 34 | std::vector has_chessboard() {return has_chessboard_;} 35 | bool has_chessboard(int id) {return has_chessboard_[id];} 36 | void setHasChessboard(std::vector has_chessboard) {has_chessboard_ = has_chessboard;} 37 | cv::Mat undistort_chessboard(cv::Mat src, int index, cv::Size chessboard, double chessboard_size); 38 | cv::Point2d project(cv::Mat P); 39 | cv::Point3d get_unit_sphere_coordinate(cv::Point2d pixel, cv::Mat transform=cv::Mat::eye(3,3,CV_64F)) 40 | { 41 | double x = pixel.x - cx_; 42 | double y = pixel.y - cy_; 43 | double mx = (fy_*x - b_*y)/(fx_*fy_-b_*c_); 44 | double my = (-c_*x + fx_*y)/(fx_*fy_-b_*c_); 45 | double ksai = alpha_ / (1 - alpha_); 46 | double r_square = mx*mx + my*my; 47 | double gamma = (ksai+std::sqrt(1+(1-ksai*ksai)*r_square))/(r_square+1); 48 | double yita = lamda_*(gamma-ksai)+std::sqrt(((gamma-ksai)*(gamma-ksai)-1)*lamda_*lamda_+1); 49 | double mz = yita*(gamma-ksai); 50 | double mu = xi_*(mz-lamda_)+std::sqrt(xi_*xi_*((mz-lamda_)*(mz-lamda_)-1)+1); 51 | x = mu*yita*gamma*mx; 52 | y = mu*yita*gamma*my; 53 | double z = mu*(mz-lamda_) - xi_; 54 | cv::Mat p = (cv::Mat_(3,1) << x, y, z); 55 | p = transform * p; 56 | return cv::Point3d(p.at(0), p.at(1), p.at(2)); 57 | } 58 | double ReprojectError(const std::vector& pixels, const std::vector& worlds, cv::Mat R, cv::Mat t) 59 | { 60 | double error = 0; 61 | for(int i = 0; i < worlds.size(); i++) 62 | { 63 | cv::Mat P = (cv::Mat_(3,1) << worlds[i].x, worlds[i].y, worlds[i].z); 64 | P = R * P + t; 65 | cv::Point2d project_p = project(P); 66 | error += std::sqrt((pixels[i].x-project_p.x)*(pixels[i].x-project_p.x) + (pixels[i].y-project_p.y)*(pixels[i].y-project_p.y)); 67 | } 68 | return error; 69 | } 70 | private: 71 | void estimate_focal(const std::vector>& pixels, const std::vector& worlds, cv::Size img_size, const cv::Size chessboard_num); 72 | void estimate_extrinsic(const std::vector>& pixels, const std::vector& worlds, const cv::Size chessboard_num); 73 | void initialize_param(const std::vector>& pixels, const std::vector& worlds); 74 | double ReprojectError(const std::vector& pixels, const std::vector& worlds, cv::Mat Rt, 75 | double cx, double cy, double fx, double fy, double xi, double lamda, double alpha, double b, double c); 76 | bool refinement(const std::vector>& pixels, const std::vector& worlds); 77 | bool has_init_guess_; 78 | double cx_; 79 | double cy_; 80 | double fx_; 81 | double fy_; 82 | double xi_; 83 | double lamda_; 84 | double alpha_; 85 | double b_; 86 | double c_; 87 | std::vector Rt_; 88 | std::vector> rt_; 89 | std::vector> pixels_; 90 | std::vector intrinsic_; 91 | std::vector has_chessboard_; 92 | 93 | struct ReprojectionError 94 | { 95 | ReprojectionError(const cv::Point2d img_pts_, const cv::Point3d board_pts_) 96 | :_img_pts(img_pts_), _board_pts(board_pts_) 97 | { 98 | } 99 | 100 | template 101 | bool operator()(const T* const intrinsic_, 102 | const T* const rt_,//6 : angle axis and translation 103 | T* residuls)const 104 | { 105 | // intrinsic: fx fy cx cy xi lambda alpha 106 | T p[3]; 107 | p[0] = T(_board_pts.x); 108 | p[1] = T(_board_pts.y); 109 | p[2] = T(0.0); 110 | T one = T(1.0); 111 | T P[3]; 112 | ceres::AngleAxisRotatePoint(rt_, p, P); 113 | P[0] += rt_[3]; 114 | P[1] += rt_[4]; 115 | P[2] += rt_[5]; 116 | 117 | T d1 = ceres::sqrt(P[0]*P[0]+P[1]*P[1]+P[2]*P[2]); 118 | T d2 = ceres::sqrt(P[0]*P[0]+P[1]*P[1]+(P[2]+intrinsic_[4]*d1)*(P[2]+intrinsic_[4]*d1)); 119 | T d3 = ceres::sqrt(P[0]*P[0]+P[1]*P[1]+(P[2]+intrinsic_[4]*d1+intrinsic_[5]*d2)*(P[2]+intrinsic_[4]*d1+intrinsic_[5]*d2)); 120 | T ksai = P[2]+intrinsic_[4]*d1+intrinsic_[5]*d2+intrinsic_[6]/(one-intrinsic_[6])*d3; // TS Model 121 | // T ksai = P[2]+intrinsic_[4]*d1+intrinsic_[6]/(one-intrinsic_[6])*d2; // DS Model 122 | // T pixel_x = intrinsic_[0] * P[0]/ksai + intrinsic_[7] * P[1]/ksai + intrinsic_[2]; 123 | // T pixel_y = intrinsic_[8] * P[0]/ksai + intrinsic_[1] * P[1]/ksai + intrinsic_[3]; 124 | T pixel_x = intrinsic_[0] * P[0]/ksai + intrinsic_[2]; 125 | T pixel_y = intrinsic_[1] * P[1]/ksai + intrinsic_[3]; 126 | 127 | // residuls 128 | residuls[0] = T(_img_pts.x) - pixel_x; 129 | residuls[1] = T(_img_pts.y) - pixel_y; 130 | return true; 131 | } 132 | const cv::Point2d _img_pts; 133 | const cv::Point3d _board_pts; 134 | }; 135 | }; 136 | 137 | #endif 138 | -------------------------------------------------------------------------------- /multi_calib.h: -------------------------------------------------------------------------------- 1 | #ifndef MULTI_CALIB_H 2 | #define MULTI_CALIB_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "TS.h" 8 | 9 | class MultiCalib_camera 10 | { 11 | public: 12 | MultiCalib_camera(){} 13 | MultiCalib_camera(double cx, double cy, double fx, double fy, double xi, double lamda, double alpha, double b, double c, 14 | cv::Mat R, cv::Mat t, std::vector has_chessboard, 15 | std::vector> pixel_coordinates){ 16 | cv::Mat r; 17 | cv::Rodrigues(R, r); 18 | std::vector rt{r.at(0,0), r.at(1,0), r.at(2,0), t.at(0,0), t.at(1,0), t.at(2,0)}; 19 | rt_ = rt; 20 | R_ = R; 21 | t_ = t; 22 | intrinsic_ = std::vector{fx, fy, cx, cy, xi, lamda, alpha, b, c}; 23 | cx_ = intrinsic_[2]; 24 | cy_ = intrinsic_[3]; 25 | fx_ = intrinsic_[0]; 26 | fy_ = intrinsic_[1]; 27 | xi_ = intrinsic_[4]; 28 | lamda_ = intrinsic_[5]; 29 | alpha_ = intrinsic_[6]; 30 | b_ = intrinsic_[7]; 31 | c_ = intrinsic_[8]; 32 | intrinsic_matrix_ = (cv::Mat_(1,9) << fx_, fy_, cx_, cy_, xi_, lamda_, alpha_, b_, c_); 33 | has_chessboard_ = has_chessboard; 34 | pixel_coordinates_ = pixel_coordinates; 35 | is_initial_ = true; 36 | } 37 | ~MultiCalib_camera(){} 38 | bool is_initial() {return is_initial_;} 39 | bool has_chessboard(int id) {return has_chessboard_[id];} 40 | std::vector> pixels() {return pixel_coordinates_;} 41 | void update_Rt(cv::Mat R, cv::Mat t) {R_ = R; t_ = t;} 42 | void update_param() 43 | { 44 | cx_ = intrinsic_[2]; 45 | cy_ = intrinsic_[3]; 46 | fx_ = intrinsic_[0]; 47 | fy_ = intrinsic_[1]; 48 | xi_ = intrinsic_[4]; 49 | lamda_ = intrinsic_[5]; 50 | alpha_ = intrinsic_[6]; 51 | b_ = intrinsic_[7]; 52 | c_ = intrinsic_[8]; 53 | cv::Mat r = (cv::Mat_(3, 1) << rt_[0], rt_[1], rt_[2]); 54 | cv::Rodrigues(r, R_); 55 | t_ = (cv::Mat_(3,1) << rt_[3], rt_[4], rt_[5]); 56 | intrinsic_matrix_ = (cv::Mat_(1,9) << fx_, fy_, cx_, cy_, xi_, lamda_, alpha_, b_, c_); 57 | } 58 | cv::Mat R() {return R_;} 59 | cv::Mat t() {return t_;} 60 | double cx() {return cx_;} 61 | double cy() {return cy_;} 62 | double fx() {return fx_;} 63 | double fy() {return fy_;} 64 | double xi() {return xi_;} 65 | double lamda() {return lamda_;} 66 | double alpha() {return alpha_;} 67 | double b() {return b_;} 68 | double c() {return c_;} 69 | std::vector intrinsic_; 70 | std::vector rt_; 71 | cv::Mat intrinsic_matrix_; 72 | private: 73 | std::vector has_chessboard_; 74 | std::vector> pixel_coordinates_; 75 | cv::Mat R_; 76 | cv::Mat t_; 77 | double cx_; 78 | double cy_; 79 | double fx_; 80 | double fy_; 81 | double xi_; 82 | double lamda_; 83 | double alpha_; 84 | double b_; 85 | double c_; 86 | bool is_initial_ = false; 87 | }; 88 | 89 | class MultiCalib_chessboard 90 | { 91 | public: 92 | MultiCalib_chessboard(){} 93 | MultiCalib_chessboard(cv::Mat R, cv::Mat t){ 94 | cv::Mat r; 95 | cv::Rodrigues(R, r); 96 | std::vector rt{r.at(0,0), r.at(1,0), r.at(2,0), t.at(0,0), t.at(1,0), t.at(2,0)}; 97 | rt_ = rt; 98 | R_ = R; 99 | t_ = t; 100 | is_initial_ = true; 101 | } 102 | ~MultiCalib_chessboard(){} 103 | bool is_initial() {return is_initial_;} 104 | void update_param() { 105 | cv::Mat r = (cv::Mat_(3, 1) << rt_[0], rt_[1], rt_[2]); 106 | cv::Rodrigues(r, R_); 107 | t_ = (cv::Mat_(3,1) << rt_[3], rt_[4], rt_[5]); 108 | } 109 | cv::Mat R() {return R_;} 110 | cv::Mat t() {return t_;} 111 | std::vector rt_; 112 | private: 113 | std::vector camera_id; 114 | cv::Mat R_; 115 | cv::Mat t_; 116 | bool is_initial_ = false; 117 | }; 118 | 119 | class MultiCalib 120 | { 121 | public: 122 | MultiCalib(std::vector cameras, const std::vector& worlds); 123 | ~MultiCalib(){} 124 | void calibrate(); 125 | void show_result(); 126 | std::vector cameras_; 127 | std::vector chessboards_; 128 | std::vector worlds_; 129 | private: 130 | void Rt_to_R_t(cv::Mat Rt, cv::Mat& R, cv::Mat& t) 131 | { 132 | cv::Vec3f r1(Rt.at(0,0),Rt.at(1,0),Rt.at(2,0)); 133 | cv::Vec3f r2(Rt.at(0,1),Rt.at(1,1),Rt.at(2,1)); 134 | cv::Vec3f r3 = r1.cross(r2); 135 | R = (cv::Mat_(3,3) << r1[0], r2[0], r3[0], r1[1], r2[1], r3[1], r1[2], r2[2], r3[2]); 136 | t = (cv::Mat_(3,1) << Rt.at(0,2), Rt.at(1,2), Rt.at(2,2)); 137 | } 138 | struct ReprojectionError 139 | { 140 | ReprojectionError(const cv::Point2d observ_p_, 141 | const cv::Point3d board_pts_) 142 | :observ_p(observ_p_), board_pts(board_pts_) 143 | { 144 | } 145 | 146 | template 147 | bool operator()(const T* const camera_rt_, 148 | const T* const chessbaord_rt_, 149 | const T* const intrinsic_, 150 | T* residuls)const 151 | { 152 | // 先根据棋盘的外参和角点编号计算角点的世界坐标 153 | T chessboard_p[3]; 154 | chessboard_p[0] = T(board_pts.x); 155 | chessboard_p[1] = T(board_pts.y); 156 | chessboard_p[2] = T(0.0); 157 | T chessboard_world_p[3]; 158 | ceres::AngleAxisRotatePoint(chessbaord_rt_, chessboard_p, chessboard_world_p); 159 | chessboard_world_p[0] += chessbaord_rt_[3]; 160 | chessboard_world_p[1] += chessbaord_rt_[4]; 161 | chessboard_world_p[2] += chessbaord_rt_[5]; 162 | // 根据相机的外参计算角点在相机坐标系下的坐标 163 | T chessboard_camera_p[3]; 164 | ceres::AngleAxisRotatePoint(camera_rt_, chessboard_world_p, chessboard_camera_p); 165 | chessboard_camera_p[0] += camera_rt_[3]; 166 | chessboard_camera_p[1] += camera_rt_[4]; 167 | chessboard_camera_p[2] += camera_rt_[5]; 168 | // 根据相机内参计算角点的投影坐标 169 | T one = T(1.0); 170 | T d1 = ceres::sqrt(chessboard_camera_p[0]*chessboard_camera_p[0]+chessboard_camera_p[1]*chessboard_camera_p[1]+chessboard_camera_p[2]*chessboard_camera_p[2]); 171 | T d2 = ceres::sqrt(chessboard_camera_p[0]*chessboard_camera_p[0]+chessboard_camera_p[1]*chessboard_camera_p[1]+(chessboard_camera_p[2]+intrinsic_[4]*d1)*(chessboard_camera_p[2]+intrinsic_[4]*d1)); 172 | T d3 = ceres::sqrt(chessboard_camera_p[0]*chessboard_camera_p[0]+chessboard_camera_p[1]*chessboard_camera_p[1]+(chessboard_camera_p[2]+intrinsic_[4]*d1+intrinsic_[5]*d2)*(chessboard_camera_p[2]+intrinsic_[4]*d1+intrinsic_[5]*d2)); 173 | T ksai = chessboard_camera_p[2]+intrinsic_[4]*d1+intrinsic_[5]*d2+intrinsic_[6]/(one-intrinsic_[6])*d3; 174 | // T ksai = chessboard_camera_p[2]+intrinsic_[6]/(one-intrinsic_[6])*d1; 175 | // T pixel_x = intrinsic_[0] * chessboard_camera_p[0]/ksai + intrinsic_[7] * chessboard_camera_p[1]/ksai + intrinsic_[2]; 176 | // T pixel_y = intrinsic_[8] * chessboard_camera_p[0]/ksai + intrinsic_[1] * chessboard_camera_p[1]/ksai + intrinsic_[3]; 177 | T pixel_x = intrinsic_[0] * chessboard_camera_p[0]/ksai + intrinsic_[2]; 178 | T pixel_y = intrinsic_[1] * chessboard_camera_p[1]/ksai + intrinsic_[3]; 179 | 180 | // T x = chessboard_camera_p[0]/ksai; 181 | // T y = chessboard_camera_p[1]/ksai; 182 | // T r_sqrure = x*x + y*y; 183 | 184 | // T pixel_x = x*(T(1.0)+intrinsic_[7]*r_sqrure+intrinsic_[8]*r_sqrure*r_sqrure+intrinsic_[11]*r_sqrure*r_sqrure*r_sqrure) + 185 | // T(2.0)*intrinsic_[9]*x*y + intrinsic_[10]*(r_sqrure+T(2.0)*x*x); 186 | // T pixel_y = y*(T(1.0)+intrinsic_[7]*r_sqrure+intrinsic_[8]*r_sqrure*r_sqrure+intrinsic_[11]*r_sqrure*r_sqrure*r_sqrure) + 187 | // intrinsic_[9]*(r_sqrure+T(2.0)*y*y) + T(2.0)*intrinsic_[10]*x*y; 188 | // pixel_x = pixel_x * intrinsic_[0] + intrinsic_[2]; 189 | // pixel_y = pixel_y * intrinsic_[1] + intrinsic_[3]; 190 | 191 | // 计算残差 192 | residuls[0] = T(observ_p.x) - pixel_x; 193 | residuls[1] = T(observ_p.y) - pixel_y; 194 | return true; 195 | } 196 | private: 197 | const cv::Point2d observ_p; 198 | const cv::Point3d board_pts; 199 | }; 200 | }; 201 | 202 | #endif 203 | -------------------------------------------------------------------------------- /EpipolarRectify/rectify.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | class TScamera 4 | { 5 | public: 6 | TScamera(double fx, double fy, double cx, double cy, double xi, double lambda, double alpha, double b, double c) 7 | :fx_(fx),fy_(fy),cx_(cx),cy_(cy),xi_(xi),lamda_(lambda),alpha_(alpha),b_(b),c_(c){w2_ = 0.42399;} 8 | cv::Point3d get_unit_sphere_coordinate(cv::Point2d pixel) 9 | { 10 | double x = pixel.x - cx_; 11 | double y = pixel.y - cy_; 12 | double mx = (fy_*x - b_*y)/(fx_*fy_-b_*c_); 13 | double my = (-c_*x + fx_*y)/(fx_*fy_-b_*c_); 14 | double ksai = alpha_ / (1 - alpha_); 15 | double r_square = mx*mx + my*my; 16 | double gamma = (ksai+std::sqrt(1+(1-ksai*ksai)*r_square))/(r_square+1); 17 | double yita = lamda_*(gamma-ksai)+std::sqrt(((gamma-ksai)*(gamma-ksai)-1)*lamda_*lamda_+1); 18 | double mz = yita*(gamma-ksai); 19 | double mu = xi_*(mz-lamda_)+std::sqrt(xi_*xi_*((mz-lamda_)*(mz-lamda_)-1)+1); 20 | return cv::Point3d(mu*yita*gamma*mx, mu*yita*gamma*my, mu*(mz-lamda_) - xi_); 21 | } 22 | cv::Point2d project(cv::Point3d p) 23 | { 24 | double X = p.x; 25 | double Y = p.y; 26 | double Z = p.z; 27 | double d1 = std::sqrt(X*X+Y*Y+Z*Z); 28 | if(Z <= -w2_*d1) return cv::Point2d(-1, -1); 29 | double d2 = std::sqrt(X*X+Y*Y+std::pow(Z+xi_*d1,2)); 30 | double d3 = std::sqrt(X*X+Y*Y+std::pow(Z+xi_*d1+lamda_*d2,2)); 31 | double ksai = Z+xi_*d1+lamda_*d2+alpha_/(1-alpha_)*d3; 32 | double pixel_x = fx_ * X/ksai + b_ * Y/ksai + cx_; 33 | double pixel_y = c_ * X/ksai + fy_ * Y/ksai + cy_; 34 | return cv::Point2d(pixel_x, pixel_y); 35 | } 36 | private: 37 | double fx_; 38 | double fy_; 39 | double cx_; 40 | double cy_; 41 | double xi_; 42 | double lamda_; 43 | double alpha_; 44 | double b_; 45 | double c_; 46 | double w2_; 47 | }; 48 | 49 | // 双目极线矫正 50 | class Remap 51 | { 52 | public: 53 | Remap(cv::Mat& front, cv::Mat Tfront, cv::Mat& right, cv::Mat& Tright, cv::Mat& rear, cv::Mat& Trear, cv::Mat& left, cv::Mat& Tleft){ 54 | front_cam = std::make_shared(front.at(0), front.at(1), front.at(2), 55 | front.at(3), front.at(4), front.at(5), 56 | front.at(6), front.at(7), front.at(8)); 57 | right_cam = std::make_shared(right.at(0), right.at(1), right.at(2), 58 | right.at(3), right.at(4), right.at(5), 59 | right.at(6), right.at(7), right.at(8)); 60 | rear_cam = std::make_shared(rear.at(0), rear.at(1), rear.at(2), 61 | rear.at(3), rear.at(4), rear.at(5), 62 | rear.at(6), rear.at(7), rear.at(8)); 63 | left_cam = std::make_shared(left.at(0), left.at(1), left.at(2), 64 | left.at(3), left.at(4), left.at(5), 65 | left.at(6), left.at(7), left.at(8)); 66 | R_front = Tfront.colRange(0,3); 67 | t_front = std::vector{Tfront.at(0,3), Tfront.at(1,3), Tfront.at(2,3)}; 68 | R_right = Tright.colRange(0,3); 69 | t_right = std::vector{Tright.at(0,3), Tright.at(1,3), Tright.at(2,3)}; 70 | R_rear = Trear.colRange(0,3); 71 | t_rear = std::vector{Trear.at(0,3), Trear.at(1,3), Trear.at(2,3)}; 72 | R_left = Tleft.colRange(0,3); 73 | t_left = std::vector{Tleft.at(0,3), Tleft.at(1,3), Tleft.at(2,3)}; 74 | 75 | image_size = cv::Size(400, 400); 76 | 77 | right_mapx = cv::Mat(cv::Size(400,1600), CV_32FC1); 78 | right_mapy = cv::Mat(cv::Size(400,1600), CV_32FC1); 79 | left_mapx = cv::Mat(cv::Size(400,1600), CV_32FC1); 80 | left_mapy = cv::Mat(cv::Size(400,1600), CV_32FC1); 81 | 82 | cx = cy = 200; 83 | fx = fy = 200; 84 | } 85 | 86 | void init_remap() 87 | { 88 | cv::Mat R_front_right = calc_R(t_front, t_right); 89 | cv::Mat R_right_rear = calc_R(t_right, t_rear); 90 | cv::Mat R_rear_left = calc_R(t_rear, t_left); 91 | cv::Mat R_left_front = calc_R(t_left, t_front); 92 | cv::Mat R; 93 | 94 | // front and right 95 | R = R_front.t() * R_front_right; 96 | for(int i = 0; i < image_size.height; i++) 97 | { 98 | for(int j = 0; j < image_size.width; j++) 99 | { 100 | cv::Mat p = (cv::Mat_(3,1) << (j-cx)/fx, (i-cy)/fy, 1); 101 | p = R * p; 102 | cv::Point2d pixel = front_cam->project(cv::Point3d(p.at(0), p.at(1), p.at(2))); 103 | left_mapx.at(i,j) = pixel.x; 104 | left_mapy.at(i,j) = pixel.y; 105 | } 106 | } 107 | R = R_right.t() * R_front_right; 108 | for(int i = 0; i < image_size.height; i++) 109 | { 110 | for(int j = 0; j < image_size.width; j++) 111 | { 112 | cv::Mat p = (cv::Mat_(3,1) << (j-cx)/fx, (i-cy)/fy, 1); 113 | p = R * p; 114 | cv::Point2d pixel = right_cam->project(cv::Point3d(p.at(0), p.at(1), p.at(2))); 115 | right_mapx.at(i,j) = pixel.x+1280; 116 | right_mapy.at(i,j) = pixel.y; 117 | } 118 | } 119 | 120 | // right and rear 121 | R = R_right.t() * R_right_rear; 122 | for(int i = 0; i < image_size.height; i++) 123 | { 124 | for(int j = 0; j < image_size.width; j++) 125 | { 126 | cv::Mat p = (cv::Mat_(3,1) << (j-cx)/fx, (i-cy)/fy, 1); 127 | p = R * p; 128 | cv::Point2d pixel = right_cam->project(cv::Point3d(p.at(0), p.at(1), p.at(2))); 129 | left_mapx.at(i+image_size.height,j) = pixel.x+1280; 130 | left_mapy.at(i+image_size.height,j) = pixel.y; 131 | } 132 | } 133 | R = R_rear.t() * R_right_rear; 134 | for(int i = 0; i < image_size.height; i++) 135 | { 136 | for(int j = 0; j < image_size.width; j++) 137 | { 138 | cv::Mat p = (cv::Mat_(3,1) << (j-cx)/fx, (i-cy)/fy, 1); 139 | p = R * p; 140 | cv::Point2d pixel = rear_cam->project(cv::Point3d(p.at(0), p.at(1), p.at(2))); 141 | right_mapx.at(i+image_size.height,j) = pixel.x; 142 | right_mapy.at(i+image_size.height,j) = pixel.y+1080; 143 | } 144 | } 145 | 146 | // rear and left 147 | R = R_rear.t() * R_rear_left; 148 | for(int i = 0; i < image_size.height; i++) 149 | { 150 | for(int j = 0; j < image_size.width; j++) 151 | { 152 | cv::Mat p = (cv::Mat_(3,1) << (j-cx)/fx, (i-cy)/fy, 1); 153 | p = R * p; 154 | cv::Point2d pixel = rear_cam->project(cv::Point3d(p.at(0), p.at(1), p.at(2))); 155 | left_mapx.at(i+image_size.height*2,j) = pixel.x; 156 | left_mapy.at(i+image_size.height*2,j) = pixel.y+1080; 157 | 158 | } 159 | } 160 | R = R_left.t() * R_rear_left; 161 | for(int i = 0; i < image_size.height; i++) 162 | { 163 | for(int j = 0; j < image_size.width; j++) 164 | { 165 | cv::Mat p = (cv::Mat_(3,1) << (j-cx)/fx, (i-cy)/fy, 1); 166 | p = R * p; 167 | cv::Point2d pixel = left_cam->project(cv::Point3d(p.at(0), p.at(1), p.at(2))); 168 | right_mapx.at(i+image_size.height*2,j) = pixel.x+1280; 169 | right_mapy.at(i+image_size.height*2,j) = pixel.y+1080; 170 | } 171 | } 172 | 173 | // left and front 174 | R = R_left.t() * R_left_front; 175 | for(int i = 0; i < image_size.height; i++) 176 | { 177 | for(int j = 0; j < image_size.width; j++) 178 | { 179 | cv::Mat p = (cv::Mat_(3,1) << (j-cx)/fx, (i-cy)/fy, 1); 180 | p = R * p; 181 | cv::Point2d pixel = left_cam->project(cv::Point3d(p.at(0), p.at(1), p.at(2))); 182 | left_mapx.at(i+image_size.height*3,j) = pixel.x+1280; 183 | left_mapy.at(i+image_size.height*3,j) = pixel.y+1080; 184 | 185 | } 186 | } 187 | R = R_front.t() * R_left_front; 188 | for(int i = 0; i < image_size.height; i++) 189 | { 190 | for(int j = 0; j < image_size.width; j++) 191 | { 192 | cv::Mat p = (cv::Mat_(3,1) << (j-cx)/fx, (i-cy)/fy, 1); 193 | p = R * p; 194 | cv::Point2d pixel = front_cam->project(cv::Point3d(p.at(0), p.at(1), p.at(2))); 195 | right_mapx.at(i+image_size.height*3,j) = pixel.x; 196 | right_mapy.at(i+image_size.height*3,j) = pixel.y; 197 | } 198 | } 199 | } 200 | void remap(cv::Mat& src, cv::Mat& dst, int code) 201 | { 202 | switch (code) 203 | { 204 | case 0: 205 | cv::remap(src, dst, left_mapx, left_mapy, cv::INTER_LINEAR);break; 206 | case 1: 207 | cv::remap(src, dst, right_mapx, right_mapy, cv::INTER_LINEAR);break; 208 | 209 | default: 210 | break; 211 | } 212 | } 213 | 214 | private: 215 | void normalize(double* vector) 216 | { 217 | double norm = std::sqrt(vector[0]*vector[0]+vector[1]*vector[1]+vector[2]*vector[2]); 218 | if(norm == 0) return; 219 | vector[0] /= norm; 220 | vector[1] /= norm; 221 | vector[2] /= norm; 222 | } 223 | 224 | void product(double* a, double* b, double* c) 225 | { 226 | // 0 -a[2] a[1] b[0] = c[0] 227 | // a[2] 0 -a[0] * b[1] = c[1] 228 | // -a[1] a[0] 0 b[2] = c[2] 229 | c[0] = -a[2]*b[1] + a[1]*b[2]; 230 | c[1] = a[2]*b[0] - a[0]*b[2]; 231 | c[2] = -a[1]*b[0] + a[0]*b[1]; 232 | } 233 | 234 | cv::Mat calc_R(std::vector& t1, std::vector& t2) 235 | { 236 | double x_axis[3] = {t2[0]-t1[0], t2[1]-t1[1], t2[2]-t1[2]}; 237 | normalize(x_axis); 238 | // z轴始终在x-z平面 239 | double z_axis[3] = {-x_axis[2], 0, x_axis[0]}; 240 | normalize(z_axis); 241 | double y_axis[3]; 242 | product(z_axis, x_axis, y_axis); 243 | normalize(y_axis); 244 | cv::Mat R = (cv::Mat_(3,3) << x_axis[0], y_axis[0], z_axis[0], 245 | x_axis[1], y_axis[1], z_axis[1], 246 | x_axis[2], y_axis[2], z_axis[2]); 247 | return R; 248 | } 249 | 250 | cv::Mat R_front, R_right, R_rear, R_left; 251 | std::vector t_front, t_right, t_rear, t_left; 252 | cv::Size image_size; 253 | std::shared_ptr front_cam, right_cam, rear_cam, left_cam; 254 | cv::Mat right_mapx, right_mapy; 255 | cv::Mat left_mapx, left_mapy; 256 | double fx, fy, cx, cy; 257 | }; 258 | 259 | int main(int argc, char** argv) 260 | { 261 | cv::Mat front, Tfront, right, Tright, rear, Trear, left, Tleft; 262 | cv::FileStorage yaml_read("../calib.yaml", cv::FileStorage::READ); 263 | yaml_read["cam0"] >> front; 264 | yaml_read["Twc0"] >> Tfront; 265 | yaml_read["cam1"] >> right; 266 | yaml_read["Twc1"] >> Tright; 267 | yaml_read["cam2"] >> rear; 268 | yaml_read["Twc2"] >> Trear; 269 | yaml_read["cam3"] >> left; 270 | yaml_read["Twc3"] >> Tleft; 271 | Remap remap(front, Tfront, right, Tright, rear, Trear, left, Tleft); 272 | remap.init_remap(); 273 | 274 | cv::Mat img = cv::imread("../test_img.jpg"); 275 | cv::Mat left_img, right_img; 276 | remap.remap(img, left_img, 0); 277 | remap.remap(img, right_img, 1); 278 | cv::imshow("left", left_img); 279 | cv::imshow("right", right_img); 280 | cv::waitKey(0); 281 | return 0; 282 | } -------------------------------------------------------------------------------- /TS.cpp: -------------------------------------------------------------------------------- 1 | #include "TS.h" 2 | 3 | TripleSphereCamera::TripleSphereCamera() 4 | { 5 | has_init_guess_ = false; 6 | cx_ = 0; 7 | cy_ = 0; 8 | fx_ = 0; 9 | fy_ = 0; 10 | xi_ = 0; 11 | alpha_ = 0; 12 | lamda_ = 0; 13 | b_ = c_ = 0; 14 | intrinsic_.resize(9); 15 | } 16 | 17 | TripleSphereCamera::TripleSphereCamera(double fx, double fy, double cx, double cy, double xi, double lamda, double alpha) 18 | { 19 | has_init_guess_= true; 20 | cx_ = cx; 21 | cy_ = cy; 22 | fx_ = fx; 23 | fy_ = fy; 24 | xi_ = xi; 25 | lamda_ = lamda; 26 | alpha_ = alpha; 27 | intrinsic_.resize(9); 28 | } 29 | 30 | bool TripleSphereCamera::calibrate(const std::vector> pixels, 31 | std::vector has_chessboard, 32 | const std::vector& worlds, 33 | const cv::Size img_size, 34 | const cv::Size chessboard_num) 35 | { 36 | pixels_ = pixels; 37 | has_chessboard_ = has_chessboard; 38 | int img_num = pixels.size(); 39 | Rt_.resize(img_num); 40 | rt_.resize(img_num); 41 | if(has_init_guess_ == false) 42 | { 43 | cx_ = img_size.width / 2 - 0.5; 44 | cy_ = img_size.height / 2 - 0.5; 45 | xi_ = 0.0; 46 | lamda_ = 0.0; 47 | alpha_ = 0.5; 48 | estimate_focal(pixels, worlds, img_size, chessboard_num); 49 | std::cout << "[initialize] focal:" << fx_ << ", cx:" << cx_ << ", cy:" << cy_ << ", xi:" << xi_ << ", lamda:" << lamda_ << ", alpha:" << alpha_ << std::endl; 50 | if(fx_ == 0) return false; 51 | } 52 | estimate_extrinsic(pixels, worlds, chessboard_num); 53 | intrinsic_[0] = fx_; 54 | intrinsic_[1] = fy_; 55 | intrinsic_[2] = cx_; 56 | intrinsic_[3] = cy_; 57 | intrinsic_[4] = xi_; 58 | intrinsic_[5] = lamda_; 59 | intrinsic_[6] = alpha_; 60 | intrinsic_[7] = b_; 61 | intrinsic_[8] = c_; 62 | for(int i = 0; i < Rt_.size(); i++) 63 | { 64 | if(has_chessboard_[i] == false) 65 | continue; 66 | cv::Vec3f r1(Rt_[i].at(0,0),Rt_[i].at(1,0),Rt_[i].at(2,0)); 67 | cv::Vec3f r2(Rt_[i].at(0,1),Rt_[i].at(1,1),Rt_[i].at(2,1)); 68 | cv::Vec3f r3 = r1.cross(r2); 69 | cv::Mat R = (cv::Mat_(3,3) << r1[0], r2[0], r3[0], r1[1], r2[1], r3[1], r1[2], r2[2], r3[2]); 70 | cv::Mat r; 71 | cv::Rodrigues(R, r); 72 | std::vector rt{r.at(0,0), r.at(1,0), r.at(2,0), Rt_[i].at(0,2), Rt_[i].at(1,2), Rt_[i].at(2,2)}; 73 | rt_[i] = rt; 74 | } 75 | 76 | bool status = false; 77 | status = refinement(pixels, worlds); 78 | if(status) has_init_guess_ = true; 79 | 80 | fx_ = intrinsic_[0]; 81 | fy_ = intrinsic_[1]; 82 | cx_ = intrinsic_[2]; 83 | cy_ = intrinsic_[3]; 84 | xi_ = intrinsic_[4]; 85 | lamda_ = intrinsic_[5]; 86 | alpha_ = intrinsic_[6]; 87 | b_ = intrinsic_[7]; 88 | c_ = intrinsic_[8]; 89 | for(int i = 0; i < Rt_.size(); i++) 90 | { 91 | if(has_chessboard_[i] == false) 92 | continue; 93 | cv::Mat R; 94 | cv::Mat r = (cv::Mat_(3, 1) << rt_[i][0], rt_[i][1], rt_[i][2]); 95 | cv::Rodrigues(r, R); 96 | Rt_[i].at(0,0) = R.at(0,0); 97 | Rt_[i].at(1,0) = R.at(1,0); 98 | Rt_[i].at(2,0) = R.at(2,0); 99 | Rt_[i].at(0,1) = R.at(0,1); 100 | Rt_[i].at(1,1) = R.at(1,1); 101 | Rt_[i].at(2,1) = R.at(2,1); 102 | Rt_[i].at(0,2) = rt_[i][3]; 103 | Rt_[i].at(1,2) = rt_[i][4]; 104 | Rt_[i].at(2,2) = rt_[i][5]; 105 | } 106 | 107 | return status; 108 | } 109 | 110 | void TripleSphereCamera::estimate_focal(const std::vector>& pixels, const std::vector& worlds, cv::Size img_size, const cv::Size chessboard_num) 111 | { 112 | double focal_ = 0; 113 | // 将像素坐标原点移到(cx, cy) 114 | std::vector> pixels_center; 115 | for(int i = 0; i < pixels.size(); i++) 116 | { 117 | std::vector pixels_tmp; 118 | for(int j = 0; j < pixels[i].size(); j++) 119 | { 120 | cv::Point2d p(pixels[i][j].x - cx_, pixels[i][j].y - cy_); 121 | pixels_tmp.push_back(p); 122 | } 123 | pixels_center.push_back(pixels_tmp); 124 | } 125 | int total_num = 0; 126 | for(int k = 0; k < pixels_center.size(); k++) 127 | { 128 | if(pixels_center[k].size() == 0) continue; 129 | for(int i = 0; i < chessboard_num.height; i++) 130 | { 131 | cv::Mat P(cv::Size(4, chessboard_num.width), CV_64F); 132 | for(int j = 0; j < chessboard_num.width; j++) 133 | { 134 | double x = pixels_center[k][i*chessboard_num.width+j].x; 135 | double y = pixels_center[k][i*chessboard_num.width+j].y; 136 | P.at(j, 0) = x; 137 | P.at(j, 1) = y; 138 | P.at(j, 2) = 0.5; 139 | P.at(j, 3) = -0.5*(x*x+y*y); 140 | } 141 | cv::Mat C; 142 | cv::SVD::solveZ(P, C); 143 | double c1 = C.at(0); 144 | double c2 = C.at(1); 145 | double c3 = C.at(2); 146 | double c4 = C.at(3); 147 | double t = c1*c1 + c2*c2 + c3*c4; 148 | if(t < 0) continue; 149 | double d = std::sqrt(1/t); 150 | double nx = c1 * d; 151 | double ny = c2 * d; 152 | if(nx*nx+ny*ny > 0.95) continue; 153 | double nz = std::sqrt(1-nx*nx-ny*ny); 154 | double gamma = fabs(c3*d/nz); 155 | focal_ += gamma; 156 | total_num++; 157 | } 158 | } 159 | if(total_num > 0) 160 | { 161 | focal_ /= total_num; 162 | } 163 | else 164 | { 165 | std::cout << "焦距估计失败" << std::endl; 166 | } 167 | fx_ = fy_ = focal_; 168 | } 169 | 170 | void TripleSphereCamera::estimate_extrinsic(const std::vector>& pixels, const std::vector& worlds, const cv::Size chessboard_num) 171 | { 172 | for(int k = 0; k < pixels.size(); k++) 173 | { 174 | if(has_chessboard_[k] == false) continue; 175 | std::vector pixels_normalize; 176 | cv::Mat transform = cv::Mat::eye(3,3,CV_64F); 177 | std::vector pixel = pixels[k]; 178 | cv::Point3d p = get_unit_sphere_coordinate(pixel[pixel.size()/2-chessboard_num.width/2-1], transform); 179 | double alpha_angle = std::atan2(p.x, p.z); 180 | double beta_angle = std::asin(p.y); 181 | cv::Mat R1 = (cv::Mat_(3,3) << cos(alpha_angle), 0, -sin(alpha_angle), 182 | 0, 1, 0, 183 | sin(alpha_angle), 0, cos(alpha_angle)); 184 | cv::Mat R2 = (cv::Mat_(3,3) << 1, 0, 0, 185 | 0, cos(beta_angle), -sin(beta_angle), 186 | 0, sin(beta_angle), cos(beta_angle)); 187 | transform = R2 * R1; 188 | for(int i = 0; i < pixels[k].size(); i++) 189 | { 190 | cv::Point3d p = get_unit_sphere_coordinate(pixels[k][i], transform); 191 | pixels_normalize.push_back(cv::Point2d(p.x/p.z, p.y/p.z)); 192 | } 193 | cv::Mat rvec, tvec, Rt; 194 | cv::solvePnPRansac(worlds, pixels_normalize, cv::Mat::eye(3,3,CV_64F), cv::Mat::zeros(4,0,CV_64F), rvec, tvec); 195 | cv::Rodrigues(rvec, Rt); 196 | Rt = transform.t() * Rt; 197 | tvec = transform.t() * tvec; 198 | Rt.at(0,2) = tvec.at(0); 199 | Rt.at(1,2) = tvec.at(1); 200 | Rt.at(2,2) = tvec.at(2); 201 | Rt_[k] = Rt; 202 | } 203 | } 204 | 205 | double TripleSphereCamera::ReprojectError(const std::vector& pixels, const std::vector& worlds, cv::Mat Rt, 206 | double cx, double cy, double fx, double fy, double xi, double lamda, double alpha, double b, double c) 207 | { 208 | double error = 0; 209 | for(int i = 0; i < worlds.size(); i++) 210 | { 211 | cv::Mat P = (cv::Mat_(3,1) << worlds[i].x, worlds[i].y, 1); 212 | P = Rt * P; 213 | double X = P.at(0,0); 214 | double Y = P.at(1,0); 215 | double Z = P.at(2,0); 216 | double d1 = std::sqrt(X*X+Y*Y+Z*Z); 217 | double d2 = std::sqrt(X*X+Y*Y+std::pow(Z+xi*d1,2)); 218 | double d3 = std::sqrt(X*X+Y*Y+std::pow(Z+xi*d1+lamda*d2,2)); 219 | double ksai = Z+xi*d1+lamda*d2+alpha/(1-alpha)*d3; 220 | double pixel_x = fx * X/ksai + b * Y/ksai + cx; 221 | double pixel_y = c * X/ksai + fy * Y/ksai + cy; 222 | error += std::sqrt((pixels[i].x-pixel_x)*(pixels[i].x-pixel_x) + (pixels[i].y-pixel_y)*(pixels[i].y-pixel_y)); 223 | } 224 | return error / worlds.size(); 225 | } 226 | 227 | void TripleSphereCamera::Reproject(const std::vector& worlds, cv::Mat Rt, std::vector& pixels) 228 | { 229 | pixels.resize(worlds.size()); 230 | for(int i = 0; i < worlds.size(); i++) 231 | { 232 | cv::Mat P = (cv::Mat_(3,1) << worlds[i].x, worlds[i].y, 1); 233 | P = Rt * P; 234 | double X = P.at(0,0); 235 | double Y = P.at(1,0); 236 | double Z = P.at(2,0); 237 | double d1 = std::sqrt(X*X+Y*Y+Z*Z); 238 | double d2 = std::sqrt(X*X+Y*Y+std::pow(Z+xi_*d1,2)); 239 | double d3 = std::sqrt(X*X+Y*Y+std::pow(Z+xi_*d1+lamda_*d2,2)); 240 | double ksai = Z+xi_*d1+lamda_*d2+alpha_/(1-alpha_)*d3; 241 | double pixel_x = fx_ * X/ksai + b_ * Y/ksai + cx_; 242 | double pixel_y = c_ * X/ksai + fy_ * Y/ksai + cy_; 243 | pixels[i] = cv::Point2d(pixel_x, pixel_y); 244 | } 245 | } 246 | 247 | bool TripleSphereCamera::refinement(const std::vector>& pixels, const std::vector& worlds) 248 | { 249 | ceres::Problem problem; 250 | 251 | for (int i = 0; i < pixels.size(); i++) 252 | { 253 | if(has_chessboard_[i] == false) 254 | continue; 255 | for(int j = 0; j < pixels[i].size(); j++) 256 | { 257 | ReprojectionError *cost_function = 258 | new ReprojectionError(pixels[i][j], worlds[j]); 259 | 260 | problem.AddResidualBlock( 261 | new ceres::AutoDiffCostFunction< 262 | ReprojectionError, 263 | 2, // num_residuals 264 | 9,6>(cost_function), 265 | NULL, 266 | intrinsic_.data(), 267 | rt_[i].data()); 268 | } 269 | } 270 | // Configure the solver. 271 | ceres::Solver::Options options; 272 | options.linear_solver_type = ceres::DENSE_SCHUR; 273 | options.minimizer_progress_to_stdout =false; 274 | options.max_num_iterations = 100; 275 | 276 | // Solve! 277 | ceres::Solver::Summary summary; 278 | ceres::Solve(options, &problem, &summary); 279 | 280 | std::cout << summary.BriefReport() << std::endl; 281 | return summary.termination_type == ceres::CONVERGENCE; 282 | } 283 | 284 | void TripleSphereCamera::undistort(double fx, double fy, double cx, double cy, cv::Size img_size, cv::Mat& mapx, cv::Mat& mapy) 285 | { 286 | mapx = cv::Mat(img_size, CV_32FC1); 287 | mapy = cv::Mat(img_size, CV_32FC1); 288 | for(int i = 0; i < img_size.height; i++) 289 | { 290 | for(int j = 0; j < img_size.width; j++) 291 | { 292 | // 先根据针孔相机模型计算入射光线向量 293 | double x = (j - cx) / fx; 294 | double y = (i - cy) / fy; 295 | double z = 1.0; 296 | double d1 = std::sqrt(x*x+y*y+z*z); 297 | double d2 = std::sqrt(x*x+y*y+std::pow(z+xi_*d1,2)); 298 | double d3 = std::sqrt(x*x+y*y+std::pow(z+xi_*d1+lamda_*d2,2)); 299 | double ksai = z+xi_*d1+lamda_*d2+alpha_/(1-alpha_)*d3; 300 | double pixel_x = fx_ * x/ksai + b_ * y/ksai + cx_; 301 | double pixel_y = c_ * x/ksai + fy_ * y/ksai + cy_; 302 | mapx.at(i, j) = pixel_x; 303 | mapy.at(i, j) = pixel_y; 304 | } 305 | } 306 | } 307 | 308 | cv::Mat TripleSphereCamera::undistort_chessboard(cv::Mat src, int index, cv::Size chessboard, double chessboard_size) 309 | { 310 | cv::Mat dst; 311 | if(has_chessboard_[index] == false) 312 | return dst; 313 | cv::Size img_size((chessboard.width+1)*chessboard_size, (chessboard.height+1)*chessboard_size); 314 | cv::Mat mapx = cv::Mat(img_size, CV_32FC1); 315 | cv::Mat mapy = cv::Mat(img_size, CV_32FC1); 316 | cv::Mat Rt = Rt_[index]; 317 | for(int i = 0; i < img_size.height; i++) 318 | { 319 | for(int j = 0; j < img_size.width; j++) 320 | { 321 | cv::Mat P = (cv::Mat_(3,1) << (j-chessboard_size), (i-chessboard_size), 1); 322 | P = Rt*P; 323 | cv::Point2d pixel = project(P); 324 | mapx.at(i, j) = pixel.x; 325 | mapy.at(i, j) = pixel.y; 326 | } 327 | } 328 | cv::remap(src, dst, mapx, mapy, cv::INTER_LINEAR); 329 | return dst; 330 | } 331 | 332 | cv::Point2d TripleSphereCamera::project(cv::Mat P) 333 | { 334 | double X = P.at(0,0); 335 | double Y = P.at(1,0); 336 | double Z = P.at(2,0); 337 | double d1 = std::sqrt(X*X+Y*Y+Z*Z); 338 | double d2 = std::sqrt(X*X+Y*Y+std::pow(Z+xi_*d1,2)); 339 | double d3 = std::sqrt(X*X+Y*Y+std::pow(Z+xi_*d1+lamda_*d2,2)); 340 | double ksai = Z+xi_*d1+lamda_*d2+alpha_/(1-alpha_)*d3; 341 | double pixel_x = fx_ * X/ksai + b_ * Y/ksai + cx_; 342 | double pixel_y = c_ * X/ksai + fy_ * Y/ksai + cy_; 343 | return cv::Point2d(pixel_x, pixel_y); 344 | } -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include "findCorner.h" 2 | #include 3 | #include "TS.h" 4 | #include "multi_calib.h" 5 | #include "cout_style.h" 6 | #include 7 | 8 | std::vector monocular_calib(std::vector filenames, double chessboard_size, cv::Size chessboard_num, TripleSphereCamera& ds_camera) 9 | { 10 | 11 | std::vector world_coordinates; 12 | for (int u = 0; u < chessboard_num.height; u++) 13 | { 14 | for (int v = 0; v < chessboard_num.width; v++) 15 | { 16 | world_coordinates.push_back(cv::Point3d(v * chessboard_size, u * chessboard_size, 0)); 17 | } 18 | } 19 | std::vector has_chessboard; 20 | cv::Size img_size; 21 | std::vector> pixel_coordinates; 22 | std::vector imgs_filename; 23 | std::cout << blue << "检测角点..." << reset << std::endl; 24 | struct Chessboarder_t find_corner_chessboard; 25 | for(int i = 0; i < filenames.size(); i++) 26 | { 27 | cv::Mat img = cv::imread(filenames[i].data()); 28 | if(img.empty()) continue; 29 | imgs_filename.push_back(filenames[i]); 30 | img_size = img.size(); 31 | std::vector pixels_per_image; 32 | find_corner_chessboard = findCorner(img, 4); 33 | if(find_corner_chessboard.chessboard.size() != 1 || find_corner_chessboard.chessboard[0].size() != chessboard_num) 34 | { 35 | has_chessboard.push_back(false); 36 | pixel_coordinates.push_back(pixels_per_image); 37 | continue; 38 | } 39 | has_chessboard.push_back(true); 40 | for (int u = 0; u < find_corner_chessboard.chessboard[0].rows; u++) 41 | { 42 | for (int v = 0; v < find_corner_chessboard.chessboard[0].cols; v++) 43 | { 44 | cv::circle(img, find_corner_chessboard.corners.p[find_corner_chessboard.chessboard[0].at(u, v)], 5, cv::Scalar(0,0,255), 2); 45 | pixels_per_image.push_back(find_corner_chessboard.corners.p[find_corner_chessboard.chessboard[0].at(u, v)]); 46 | } 47 | } 48 | 49 | pixel_coordinates.push_back(pixels_per_image); 50 | cv::line(img, pixels_per_image[0], 51 | pixels_per_image[1], cv::Scalar(0,255,0), 2); 52 | cv::line(img, pixels_per_image[0], 53 | pixels_per_image[chessboard_num.width], cv::Scalar(255,0,0), 2); 54 | cv::imshow("img", img); 55 | cv::waitKey(1); 56 | } 57 | ds_camera.calibrate(pixel_coordinates, has_chessboard, world_coordinates, img_size, chessboard_num); 58 | // 精细化角点 59 | std::vector Rts; 60 | Rts = ds_camera.Rt(); 61 | std::cout << blue << "精细化角点..." << reset << std::endl; 62 | for(int i = 0; i < imgs_filename.size(); i++) 63 | { 64 | cv::Mat img = cv::imread(imgs_filename[i].data()); 65 | cv::Mat chessboard_img = ds_camera.undistort_chessboard(img, i, chessboard_num, chessboard_size); 66 | if(chessboard_img.empty()) continue; 67 | for (int u = 0; u < pixel_coordinates[i].size(); u++) 68 | { 69 | cv::circle(img, pixel_coordinates[i][u], 5, cv::Scalar(0,0,255), 2); 70 | } 71 | struct Chessboarder_t find_chessboard = findCorner(chessboard_img, 4); 72 | if(find_chessboard.chessboard.size() == 0 || find_chessboard.chessboard[0].size() != chessboard_num) 73 | { 74 | // 纠正翻转的棋盘，保证棋盘左上角是黑块（要求棋盘格行列数分别为奇数和偶数） 75 | cv::Mat gray; 76 | cv::cvtColor(chessboard_img, gray, cv::COLOR_BGR2GRAY); 77 | cv::Point2d p1(chessboard_size/2, chessboard_size/2); 78 | cv::Point2d p2(chessboard_size*3/2, chessboard_size/2); 79 | cv::Point2d p3(chessboard_size*3/2, chessboard_size*3/2); 80 | cv::Point2d p4(chessboard_size/2, chessboard_size*3/2); 81 | int gray1 = gray.at(int(p1.y), int(p1.x)); 82 | int gray2 = gray.at(int(p2.y), int(p2.x)); 83 | int gray3 = gray.at(int(p3.y), int(p3.x)); 84 | int gray4 = gray.at(int(p4.y), int(p4.x)); 85 | if(gray1 + gray3 > gray2 + gray4) 86 | { 87 | cv::flip(chessboard_img, chessboard_img, -1); 88 | std::vector tmp = pixel_coordinates[i]; 89 | for(int k = 0; k < pixel_coordinates[i].size(); k++) 90 | pixel_coordinates[i][k] = tmp[pixel_coordinates[i].size()-1-k]; 91 | } 92 | continue; 93 | } 94 | for (int u = 0; u < find_chessboard.chessboard[0].rows; u++) 95 | { 96 | for (int v = 0; v < find_chessboard.chessboard[0].cols; v++) 97 | { 98 | cv::Point2d new_corner = find_chessboard.corners.p[find_chessboard.chessboard[0].at(u, v)]; 99 | cv::circle(chessboard_img, new_corner, 5, cv::Scalar(0,0,255), 2); 100 | cv::Mat p = (cv::Mat_(3,1) << new_corner.x-chessboard_size, new_corner.y-chessboard_size, 1); 101 | p = Rts[i]*p; 102 | new_corner = ds_camera.project(p); 103 | pixel_coordinates[i][v+u*chessboard_num.width] = new_corner; 104 | cv::circle(img, new_corner, 2, cv::Scalar(0,255,0), 2); 105 | } 106 | } 107 | // 纠正翻转的棋盘，保证棋盘左上角是黑块（要求棋盘格行列数分别为奇数和偶数） 108 | cv::Mat gray; 109 | cv::cvtColor(chessboard_img, gray, cv::COLOR_BGR2GRAY); 110 | cv::Point2d p1(chessboard_size/2, chessboard_size/2); 111 | cv::Point2d p2(chessboard_size*3/2, chessboard_size/2); 112 | cv::Point2d p3(chessboard_size*3/2, chessboard_size*3/2); 113 | cv::Point2d p4(chessboard_size/2, chessboard_size*3/2); 114 | int gray1 = gray.at(int(p1.y), int(p1.x)); 115 | int gray2 = gray.at(int(p2.y), int(p2.x)); 116 | int gray3 = gray.at(int(p3.y), int(p3.x)); 117 | int gray4 = gray.at(int(p4.y), int(p4.x)); 118 | if(gray1 + gray3 > gray2 + gray4) 119 | { 120 | cv::flip(chessboard_img, chessboard_img, -1); 121 | std::vector tmp = pixel_coordinates[i]; 122 | for(int k = 0; k < pixel_coordinates[i].size(); k++) 123 | pixel_coordinates[i][k] = tmp[pixel_coordinates[i].size()-1-k]; 124 | } 125 | cv::imshow("chessboard", chessboard_img); 126 | cv::imshow("img", img); 127 | cv::waitKey(1); 128 | } 129 | ds_camera.calibrate(pixel_coordinates, has_chessboard, world_coordinates, img_size, chessboard_num); 130 | std::cout << yellow; 131 | std::cout << "fx:" << ds_camera.fx() << ", fy:" << ds_camera.fy() << ", cx:" << ds_camera.cx() << ", cy:" << ds_camera.cy() << ", xi:" << ds_camera.xi() 132 | << ", lamda:" << ds_camera.lamda() << ", alpha:" << ds_camera.alpha() 133 | << ", b:" << ds_camera.b() << ", c:" << ds_camera.c() << std::endl; 134 | std::cout << reset; 135 | 136 | Rts = ds_camera.Rt(); 137 | double error = 0; 138 | int cnt = 0; 139 | for(int i = 0; i < Rts.size(); i++) 140 | { 141 | if(has_chessboard[i] == false) 142 | continue; 143 | cv::Mat img = cv::imread(filenames[i].data()); 144 | cv::Mat chessboard_img = ds_camera.undistort_chessboard(img, i, chessboard_num, chessboard_size); 145 | for(int m = 0; m < chessboard_num.width; m++) 146 | { 147 | cv::line(chessboard_img, cv::Point(chessboard_size*(m+1), 0), 148 | cv::Point(chessboard_size*(m+1), chessboard_size*(chessboard_num.height+1)), cv::Scalar(0,255,0), 1); 149 | } 150 | for(int m = 0; m < chessboard_num.height; m++) 151 | { 152 | cv::line(chessboard_img, cv::Point(0, chessboard_size*(m+1)), 153 | cv::Point(chessboard_size*(chessboard_num.width+1), chessboard_size*(m+1)), cv::Scalar(0,255,0), 1); 154 | } 155 | for(int j = 0; j < pixel_coordinates[i].size(); j++) 156 | { 157 | cv::Point3d p = ds_camera.get_unit_sphere_coordinate(pixel_coordinates[i][j]); 158 | cv::Mat Rt = Rts[i]; 159 | cv::Mat pp = (cv::Mat_(3,1) << p.x, p.y, p.z); 160 | pp = Rt.inv() * pp; 161 | cv::Point2d point(pp.at(0)/pp.at(2), pp.at(1)/pp.at(2)); 162 | cv::circle(chessboard_img, point+cv::Point2d(chessboard_size, chessboard_size), 5, cv::Scalar(0,0,255), 2); 163 | int x = j%chessboard_num.width; 164 | int y = (j-x)/chessboard_num.width; 165 | } 166 | std::vector pixels_reproject; 167 | ds_camera.Reproject(world_coordinates, Rts[i], pixels_reproject); 168 | for (int j = 0; j < pixels_reproject.size(); j++) 169 | { 170 | cv::circle(img, pixel_coordinates[i][j], 5, cv::Scalar(0,0,255), 2); 171 | cv::circle(img, pixels_reproject[j], 5, cv::Scalar(0,255,255), 2); 172 | cv::line(img, pixel_coordinates[i][0], 173 | pixel_coordinates[i][1], cv::Scalar(0,255,0), 2); 174 | cv::line(img, pixel_coordinates[i][0], 175 | pixel_coordinates[i][chessboard_num.width], cv::Scalar(255,0,0), 2); 176 | error += std::sqrt((pixels_reproject[j].x-pixel_coordinates[i][j].x)*(pixels_reproject[j].x-pixel_coordinates[i][j].x)+ 177 | (pixels_reproject[j].y-pixel_coordinates[i][j].y)*(pixels_reproject[j].y-pixel_coordinates[i][j].y)); 178 | cnt++; 179 | } 180 | cv::imshow("img", img); 181 | cv::imshow("chessboard", chessboard_img); 182 | cv::waitKey(1); 183 | } 184 | std::cout << yellow << "mean error: " << error/cnt << reset << std::endl; 185 | return world_coordinates; 186 | } 187 | 188 | int main(int argc, char **argv) 189 | { 190 | int chessboard_size = 45; 191 | cv::Size chessboard_num(11, 8); 192 | char str[80]; 193 | std::vector filenames1; 194 | for(int i = 0; i < 185; i++) 195 | { 196 | sprintf(str, "/home/xiesheng/Downloads/calib_4/video/front/%d.jpg", i); 197 | std::string name = str; 198 | filenames1.push_back(name); 199 | } 200 | TripleSphereCamera camera_1; 201 | std::cout << green << "开始标定相机1..." << reset << std::endl; 202 | std::vector world_coordinates = monocular_calib(filenames1, chessboard_size, chessboard_num, camera_1); 203 | 204 | std::vector filenames2; 205 | for(int i = 0; i < 185; i++) 206 | { 207 | sprintf(str, "/home/xiesheng/Downloads/calib_4/video/right/%d.jpg", i); 208 | std::string name = str; 209 | filenames2.push_back(name); 210 | } 211 | TripleSphereCamera camera_2; 212 | std::cout << green << "开始标定相机2..." << reset << std::endl; 213 | monocular_calib(filenames2, chessboard_size, chessboard_num, camera_2); 214 | 215 | std::vector filenames3; 216 | for(int i = 0; i < 185; i++) 217 | { 218 | sprintf(str, "/home/xiesheng/Downloads/calib_4/video/rear/%d.jpg", i); 219 | std::string name = str; 220 | filenames3.push_back(name); 221 | } 222 | TripleSphereCamera camera_3; 223 | std::cout << green << "开始标定相机3..." << reset << std::endl; 224 | monocular_calib(filenames3, chessboard_size, chessboard_num, camera_3); 225 | 226 | std::vector filenames4; 227 | for(int i = 0; i < 185; i++) 228 | { 229 | sprintf(str, "/home/xiesheng/Downloads/calib_4/video/left/%d.jpg", i); 230 | std::string name = str; 231 | filenames4.push_back(name); 232 | } 233 | TripleSphereCamera camera_4; 234 | std::cout << green << "开始标定相机4..." << reset << std::endl; 235 | monocular_calib(filenames4, chessboard_size, chessboard_num, camera_4); 236 | 237 | std::vector cameras; 238 | 239 | cameras.push_back(camera_1); 240 | cameras.push_back(camera_2); 241 | cameras.push_back(camera_3); 242 | cameras.push_back(camera_4); 243 | MultiCalib mul_calib = MultiCalib(cameras, world_coordinates); 244 | std::cout << green <<"开始联合标定..." << reset << std::endl; 245 | std::cout << "多相机标定前..." << std::endl; 246 | for(int m = 0; m < mul_calib.cameras_.size(); m++) 247 | { 248 | std::vector> pixels = mul_calib.cameras_[m].pixels(); 249 | double error = 0; 250 | int cnt = 0; 251 | for(int i = 0; i < mul_calib.chessboards_.size(); i++) 252 | { 253 | if(mul_calib.chessboards_[i].is_initial()) 254 | { 255 | for(int j = 0; j < pixels[i].size(); j++) 256 | { 257 | cv::Mat R = mul_calib.chessboards_[i].R(); 258 | cv::Mat t = mul_calib.chessboards_[i].t(); 259 | cv::Mat p = (cv::Mat_(3, 1) << world_coordinates[j].x, world_coordinates[j].y, world_coordinates[j].z); 260 | p = R * p + t; 261 | R = mul_calib.cameras_[m].R(); 262 | t = mul_calib.cameras_[m].t(); 263 | p = R * p + t; 264 | double fx = mul_calib.cameras_[m].fx(); 265 | double fy = mul_calib.cameras_[m].fy(); 266 | double cx = mul_calib.cameras_[m].cx(); 267 | double cy = mul_calib.cameras_[m].cy(); 268 | double alpha = mul_calib.cameras_[m].alpha(); 269 | double lamda = mul_calib.cameras_[m].lamda(); 270 | double xi = mul_calib.cameras_[m].xi(); 271 | double b = mul_calib.cameras_[m].b(); 272 | double c = mul_calib.cameras_[m].c(); 273 | double X = p.at(0,0); 274 | double Y = p.at(1,0); 275 | double Z = p.at(2,0); 276 | double d1 = std::sqrt(X*X+Y*Y+Z*Z); 277 | double d2 = std::sqrt(X*X+Y*Y+std::pow(Z+xi*d1,2)); 278 | double d3 = std::sqrt(X*X+Y*Y+std::pow(Z+xi*d1+lamda*d2,2)); 279 | double ksai = Z+xi*d1+lamda*d2+alpha/(1-alpha)*d3; 280 | double pixel_x = fx * X/ksai + b * Y/ksai + cx; 281 | double pixel_y = c * X/ksai + fy * Y/ksai + cy; 282 | error += std::sqrt((pixels[i][j].x-pixel_x)*(pixels[i][j].x-pixel_x) + (pixels[i][j].y-pixel_y)*(pixels[i][j].y-pixel_y)); 283 | cnt++; 284 | } 285 | } 286 | } 287 | std::cout << "camera_" << m << " 重投影误差：" << error/cnt << std::endl; 288 | } 289 | mul_calib.calibrate(); 290 | mul_calib.show_result(); 291 | 292 | // save calibration result 293 | time_t tt; 294 | time( &tt ); 295 | tt = tt + 8*3600; // transform the time zone 296 | tm* t= gmtime( &tt ); 297 | char yaml_filename[100]; 298 | sprintf(yaml_filename, "%d-%02d-%02d %02d-%02d-%02d.yaml", 299 | t->tm_year + 1900, 300 | t->tm_mon + 1, 301 | t->tm_mday, 302 | t->tm_hour, 303 | t->tm_min, 304 | t->tm_sec); 305 | cv::FileStorage fs_write(yaml_filename, cv::FileStorage::WRITE); 306 | for(size_t i = 0; i < mul_calib.cameras_.size(); i++) 307 | { 308 | char cam_name[10]; 309 | sprintf(cam_name, "cam%d", (int)i); 310 | fs_write << cam_name << mul_calib.cameras_[i].intrinsic_matrix_; 311 | sprintf(cam_name, "Twc%d", (int)i); 312 | cv::Mat_ R = mul_calib.cameras_[i].R(); 313 | cv::Mat_ t = mul_calib.cameras_[i].t(); 314 | cv::Mat T = (cv::Mat_(3,4) << R(0,0), R(0,1), R(0,2), t(0), 315 | R(1,0), R(1,1), R(1,2), t(1), 316 | R(2,0), R(2,1), R(2,2), t(2)); 317 | fs_write << cam_name << T; 318 | } 319 | fs_write.release(); 320 | return 0; 321 | } 322 | -------------------------------------------------------------------------------- /DetectCorner/chessboard.cpp: -------------------------------------------------------------------------------- 1 | #include "chessboard.h" 2 | 3 | std::vector chessboardsFromCorners(struct Corner_t corners) 4 | { 5 | std::vector chessboards; 6 | cv::Mat chessboard = cv::Mat::zeros(cv::Size(3, 3), CV_16U); 7 | for (unsigned int i = 0; i < corners.p.size(); i++) 8 | { 9 | chessboard = initChessboard(corners, i); 10 | if (chessboard.at(0, 0) == 0 && chessboard.at(0, 1) == 0) 11 | continue; 12 | if (chessboardEnergy(chessboard, corners) > 0) 13 | continue; 14 | while (1) 15 | { 16 | double enegy = chessboardEnergy(chessboard, corners); 17 | std::vector proposal; 18 | std::vector p_enegy; 19 | for (int j = 0; j < 4; j++) 20 | { 21 | cv::Mat chess = growChessboard(chessboard, corners, j); 22 | proposal.push_back(chess); 23 | p_enegy.push_back(chessboardEnergy(chess, corners)); 24 | } 25 | int min_idx = min_element(p_enegy.begin(), p_enegy.end()) - p_enegy.begin(); 26 | if (p_enegy[min_idx] < enegy) 27 | chessboard = proposal[min_idx]; 28 | else 29 | break; 30 | } 31 | if (chessboardEnergy(chessboard, corners) < -10) 32 | { 33 | std::vector overlap; 34 | if (chessboards.size() > 0) 35 | { 36 | overlap.resize(chessboards.size()); 37 | for (unsigned int j = 0; j < chessboards.size(); j++) 38 | { 39 | auto ptr = chessboards[j].begin(); 40 | for (int k = 0; k < chessboards[j].rows*chessboards[j].cols; k++) 41 | { 42 | auto ret = std::find(chessboard.begin(), chessboard.end(), (*ptr)); 43 | ptr++; 44 | if (ret != chessboard.end()) 45 | { 46 | overlap[j].x = 1; 47 | overlap[j].y = chessboardEnergy(chessboards[j], corners); 48 | break; 49 | } 50 | } 51 | } 52 | bool overlaped = false; 53 | bool lower_enegy = false; 54 | for (unsigned int j = 0; j < overlap.size(); j++) 55 | { 56 | if (overlap[j].x == 1) 57 | { 58 | overlaped = true; 59 | if (overlap[j].y > chessboardEnergy(chessboard, corners)) 60 | { 61 | chessboards[j] = cv::Mat::zeros(cv::Size(2, 2), CV_64F); 62 | lower_enegy = true; 63 | } 64 | } 65 | } 66 | if (overlaped == false || lower_enegy == true) 67 | chessboards.push_back(chessboard); 68 | for (auto iter = chessboards.begin(); iter != chessboards.end(); iter++) 69 | { 70 | if (iter->at(0, 0) == 0 && iter->at(0, 1) == 0) 71 | { 72 | chessboards.erase(iter); 73 | iter--; 74 | } 75 | } 76 | } 77 | else 78 | { 79 | chessboards.push_back(chessboard); 80 | } 81 | } 82 | } 83 | for (unsigned int i = 0; i < chessboards.size(); i++) 84 | { 85 | cv::Mat chess = chessboards[i]; 86 | cv::Mat new_chess; 87 | // 保证列数不小于行数 88 | if (chess.cols < chess.rows) 89 | { 90 | new_chess = cv::Mat::zeros(cv::Size(chess.rows, chess.cols), CV_16U); 91 | for (int j = 0; j < new_chess.rows; j++) 92 | { 93 | for (int k = 0; k < new_chess.cols; k++) 94 | { 95 | new_chess.at(j, k) = chess.at(chess.rows - k - 1, j); 96 | } 97 | } 98 | chess = new_chess.clone(); 99 | } 100 | chessboards[i] = chess; 101 | } 102 | return chessboards; 103 | } 104 | 105 | cv::Mat initChessboard(struct Corner_t corners, int idx) 106 | { 107 | cv::Mat chessboard = cv::Mat::zeros(cv::Size(3, 3), CV_16U); 108 | if (corners.p.size() < 9) 109 | return chessboard; 110 | cv::Point2d v1 = corners.v1[idx]; 111 | cv::Point2d v2 = corners.v2[idx]; 112 | cv::Point2d minus_v1 = cv::Point2d(-corners.v1[idx].x, -corners.v1[idx].y); 113 | cv::Point2d minus_v2 = cv::Point2d(-corners.v2[idx].x, -corners.v2[idx].y); 114 | chessboard.at(1, 1) = idx; 115 | double dist1[2] = { 0 }; 116 | double dist2[6] = { 0 }; 117 | int neighbor_idx = 0; 118 | // find left/right/top/bottom neighbors 119 | directionalNeighbor(idx, v1, chessboard, corners, &neighbor_idx, &dist1[0]); 120 | chessboard.at(1, 2) = neighbor_idx; 121 | directionalNeighbor(idx, minus_v1, chessboard, corners, &neighbor_idx, &dist1[1]); 122 | chessboard.at(1, 0) = neighbor_idx; 123 | directionalNeighbor(idx, v2, chessboard, corners, &neighbor_idx, &dist2[0]); 124 | chessboard.at(2, 1) = neighbor_idx; 125 | directionalNeighbor(idx, minus_v2, chessboard, corners, &neighbor_idx, &dist2[1]); 126 | chessboard.at(0, 1) = neighbor_idx; 127 | // find top-left/top-right/bottom-left/bottom-right neighbors 128 | directionalNeighbor(chessboard.at(1, 0), minus_v2, chessboard, corners, &neighbor_idx, &dist2[2]); 129 | chessboard.at(0, 0) = neighbor_idx; 130 | directionalNeighbor(chessboard.at(1, 0), v2, chessboard, corners, &neighbor_idx, &dist2[3]); 131 | chessboard.at(2, 0) = neighbor_idx; 132 | directionalNeighbor(chessboard.at(1, 2), minus_v2, chessboard, corners, &neighbor_idx, &dist2[4]); 133 | chessboard.at(0, 2) = neighbor_idx; 134 | directionalNeighbor(chessboard.at(1, 2), v2, chessboard, corners, &neighbor_idx, &dist2[5]); 135 | chessboard.at(2, 2) = neighbor_idx; 136 | double ave_1 = average(dist1, 2); 137 | double std_1 = stdd(dist1, 2, ave_1); 138 | if (std_1 / ave_1 > 0.3) 139 | { 140 | return cv::Mat::zeros(cv::Size(3, 3), CV_16U); 141 | } 142 | double ave_2 = average(dist2, 6); 143 | double std_2 = stdd(dist2, 6, ave_2); 144 | if (std_2 / ave_2 > 0.3) 145 | { 146 | return cv::Mat::zeros(cv::Size(3, 3), CV_16U); 147 | } 148 | return chessboard; 149 | } 150 | 151 | double average(double* a, int length) 152 | { 153 | double sum = 0; 154 | for (int i = 0; i < length; i++) 155 | { 156 | sum += a[i]; 157 | } 158 | return sum / length; 159 | } 160 | 161 | double stdd(double* a, int length, double mean) 162 | { 163 | double sum = 0; 164 | for (int i = 0; i < length; i++) 165 | { 166 | sum += (a[i] - mean)*(a[i] - mean); 167 | } 168 | sum = sum / (length-1); 169 | return sqrt(sum); 170 | } 171 | 172 | void directionalNeighbor(int idx, cv::Point2d v, cv::Mat chessboard, struct Corner_t corners, int* neighbor_idx, double* min_dist) 173 | { 174 | 175 | std::vector unused; 176 | std::vector used; 177 | unused.resize(corners.p.size()); 178 | for (unsigned int i = 0; i < unused.size(); i++) 179 | unused[i] = i; 180 | for (int i = 0; i < chessboard.rows; i++) 181 | { 182 | for (int j = 0; j < chessboard.cols; j++) 183 | { 184 | if (chessboard.at(i, j) != 0) 185 | { 186 | used.push_back(chessboard.at(i, j)); 187 | } 188 | } 189 | } 190 | for (auto iterator = unused.begin(); iterator != unused.end(); iterator++) 191 | { 192 | for (unsigned int i = 0; i < used.size(); i++) 193 | { 194 | if (*iterator == used[i]) 195 | { 196 | unused.erase(iterator); 197 | iterator--; 198 | } 199 | } 200 | } 201 | std::vector dist; 202 | dist.resize(unused.size()); 203 | for (unsigned int i = 0; i < unused.size(); i++) 204 | { 205 | cv::Point2d dir = corners.p[unused[i]] - corners.p[idx]; 206 | dist[i] = dir.x*v.x + dir.y*v.y; 207 | cv::Point2d edge = dir - dist[i] * v; 208 | double dist_edge = sqrt(edge.x*edge.x+edge.y*edge.y); 209 | if (dist[i] < 0) dist[i] = 1e10; 210 | dist[i] = dist[i] + 5 * dist_edge; 211 | } 212 | int min_idx = min_element(dist.begin(), dist.end()) - dist.begin(); 213 | *min_dist = dist[min_idx]; 214 | *neighbor_idx = unused[min_idx]; 215 | } 216 | 217 | double chessboardEnergy(cv::Mat chessboard, struct Corner_t corners) 218 | { 219 | double E_structure = 0; 220 | for (int j = 0; j < chessboard.rows; j++) 221 | { 222 | for (int k = 0; k < chessboard.cols-2; k++) 223 | { 224 | std::vector x; 225 | for (int i = k; i <= k + 2; i++) 226 | { 227 | x.push_back(corners.p[chessboard.at(j, i)]); 228 | } 229 | cv::Point2d x_ = x[0] + x[2] - 2 * x[1]; 230 | double x_norm1 = sqrt(x_.x*x_.x+x_.y*x_.y); 231 | x_ = x[0] - x[2]; 232 | double x_norm2 = sqrt(x_.x*x_.x + x_.y*x_.y); 233 | E_structure = std::max(E_structure, x_norm1/x_norm2); 234 | } 235 | } 236 | for (int j = 0; j < chessboard.cols; j++) 237 | { 238 | for (int k = 0; k < chessboard.rows - 2; k++) 239 | { 240 | std::vector x; 241 | for (int i = k; i <= k + 2; i++) 242 | { 243 | x.push_back(corners.p[chessboard.at(i, j)]); 244 | } 245 | cv::Point x_ = x[0] + x[2] - 2 * x[1]; 246 | double x_norm1 = sqrt(x_.x*x_.x + x_.y*x_.y); 247 | x_ = x[0] - x[2]; 248 | double x_norm2 = sqrt(x_.x*x_.x + x_.y*x_.y); 249 | E_structure = std::max(E_structure, x_norm1 / x_norm2); 250 | } 251 | } 252 | return chessboard.rows*chessboard.cols*(E_structure - 1); 253 | } 254 | 255 | cv::Mat growChessboard(cv::Mat chessboard, struct Corner_t corners, int boarder_type) 256 | { 257 | if (chessboard.at(0, 0) == 0 && chessboard.at(0, 1) == 0) 258 | return chessboard; 259 | std::vector unused; 260 | std::vector used; 261 | unused.resize(corners.p.size()); 262 | for (unsigned int i = 0; i < unused.size(); i++) 263 | unused[i] = i; 264 | for (int i = 0; i < chessboard.rows; i++) 265 | { 266 | for (int j = 0; j < chessboard.cols; j++) 267 | { 268 | if (chessboard.at(i, j) != 0) 269 | { 270 | used.push_back(chessboard.at(i, j)); 271 | } 272 | } 273 | } 274 | for (auto iterator = unused.begin(); iterator != unused.end(); iterator++) 275 | { 276 | for (unsigned int i = 0; i < used.size(); i++) 277 | { 278 | if (*iterator == used[i]) 279 | { 280 | unused.erase(iterator); 281 | iterator--; 282 | } 283 | } 284 | } 285 | std::vector cand; 286 | for (unsigned int i = 0; i < unused.size(); i++) 287 | { 288 | cand.push_back(corners.p[unused[i]]); 289 | } 290 | switch (boarder_type) 291 | { 292 | case 0: 293 | { 294 | std::vector pred; 295 | for (int i = 0; i < chessboard.rows; i++) 296 | { 297 | pred.push_back(predictCorners(corners.p[chessboard.at(i, chessboard.cols-3)], 298 | corners.p[chessboard.at(i, chessboard.cols - 2)], 299 | corners.p[chessboard.at(i, chessboard.cols - 1)])); 300 | } 301 | std::vector idx = assignClosestCorners(cand, pred); 302 | if (idx.size() > 0) 303 | { 304 | cv::Mat new_chess = cv::Mat::zeros(cv::Size(1, chessboard.rows), CV_16U); 305 | for (unsigned int j = 0; j < idx.size(); j++) 306 | { 307 | new_chess.at(j, 0) = unused[idx[j]]; 308 | } 309 | cv::Mat new_chessboard = cv::Mat::zeros(cv::Size(chessboard.cols+1, chessboard.rows), CV_16U); 310 | cv::Mat new_roi = new_chessboard(cv::Rect(0, 0, chessboard.cols, chessboard.rows)); 311 | chessboard.convertTo(new_roi, new_roi.type()); 312 | new_roi = new_chessboard(cv::Rect(chessboard.cols, 0, 1, chessboard.rows)); 313 | new_chess.convertTo(new_roi, new_roi.type()); 314 | chessboard = new_chessboard; 315 | } 316 | break; 317 | } 318 | case 1: 319 | { 320 | std::vector pred; 321 | for (int i = 0; i < chessboard.cols; i++) 322 | { 323 | pred.push_back(predictCorners(corners.p[chessboard.at(chessboard.rows - 3, i)], 324 | corners.p[chessboard.at(chessboard.rows - 2, i)], 325 | corners.p[chessboard.at(chessboard.rows - 1, i)])); 326 | } 327 | std::vector idx = assignClosestCorners(cand, pred); 328 | if (idx.size() > 0) 329 | { 330 | cv::Mat new_chess = cv::Mat::zeros(cv::Size(chessboard.cols, 1), CV_16U); 331 | for (unsigned int j = 0; j < idx.size(); j++) 332 | { 333 | new_chess.at(0, j) = unused[idx[j]]; 334 | } 335 | cv::Mat new_chessboard = cv::Mat::zeros(cv::Size(chessboard.cols, chessboard.rows + 1), CV_16U); 336 | cv::Mat new_roi = new_chessboard(cv::Rect(0, 0, chessboard.cols, chessboard.rows)); 337 | chessboard.convertTo(new_roi, new_roi.type()); 338 | new_roi = new_chessboard(cv::Rect(0, chessboard.rows, chessboard.cols, 1)); 339 | new_chess.convertTo(new_roi, new_roi.type()); 340 | chessboard = new_chessboard; 341 | } 342 | break; 343 | } 344 | case 2: 345 | { 346 | std::vector pred; 347 | for (int i = 0; i < chessboard.rows; i++) 348 | { 349 | pred.push_back(predictCorners(corners.p[chessboard.at(i, 2)], 350 | corners.p[chessboard.at(i, 1)], 351 | corners.p[chessboard.at(i, 0)])); 352 | } 353 | std::vector idx = assignClosestCorners(cand, pred); 354 | if (idx.size() > 0) 355 | { 356 | cv::Mat new_chess = cv::Mat::zeros(cv::Size(1, chessboard.rows), CV_16U); 357 | for (unsigned int j = 0; j < idx.size(); j++) 358 | { 359 | new_chess.at(j, 0) = unused[idx[j]]; 360 | } 361 | cv::Mat new_chessboard = cv::Mat::zeros(cv::Size(chessboard.cols + 1, chessboard.rows), CV_16U); 362 | cv::Mat new_roi = new_chessboard(cv::Rect(1, 0, chessboard.cols, chessboard.rows)); 363 | chessboard.convertTo(new_roi, new_roi.type()); 364 | new_roi = new_chessboard(cv::Rect(0, 0, 1, chessboard.rows)); 365 | new_chess.convertTo(new_roi, new_roi.type()); 366 | chessboard = new_chessboard; 367 | } 368 | break; 369 | } 370 | case 3: 371 | { 372 | std::vector pred; 373 | for (int i = 0; i < chessboard.cols; i++) 374 | { 375 | pred.push_back(predictCorners(corners.p[chessboard.at(2, i)], 376 | corners.p[chessboard.at(1, i)], 377 | corners.p[chessboard.at(0, i)])); 378 | } 379 | std::vector idx = assignClosestCorners(cand, pred); 380 | if (idx.size() > 0) 381 | { 382 | cv::Mat new_chess = cv::Mat::zeros(cv::Size(chessboard.cols, 1), CV_16U); 383 | for (unsigned int j = 0; j < idx.size(); j++) 384 | { 385 | new_chess.at(0, j) = unused[idx[j]]; 386 | } 387 | cv::Mat new_chessboard = cv::Mat::zeros(cv::Size(chessboard.cols, chessboard.rows + 1), CV_16U); 388 | cv::Mat new_roi = new_chessboard(cv::Rect(0, 1, chessboard.cols, chessboard.rows)); 389 | chessboard.convertTo(new_roi, new_roi.type()); 390 | new_roi = new_chessboard(cv::Rect(0, 0, chessboard.cols, 1)); 391 | new_chess.convertTo(new_roi, new_roi.type()); 392 | chessboard = new_chessboard; 393 | } 394 | break; 395 | } 396 | } 397 | return chessboard; 398 | } 399 | 400 | cv::Point2d predictCorners(cv::Point2d p1, cv::Point2d p2, cv::Point2d p3) 401 | { 402 | cv::Point2d v1 = p2 - p1; 403 | cv::Point2d v2 = p3 - p2; 404 | double a1 = atan2(v1.y, v1.x); 405 | double a2 = atan2(v2.y, v2.x); 406 | double a3 = 2 * a2 - a1; 407 | double s1 = sqrt(v1.x*v1.x+v1.y*v1.y); 408 | double s2 = sqrt(v2.x*v2.x+v2.y*v2.y); 409 | double s3 = 2 * s2 - s1; 410 | cv::Point2d pred; 411 | pred.x = p3.x + 0.75*s3*cos(a3); 412 | pred.y = p3.y + 0.75*s3*sin(a3); 413 | return pred; 414 | } 415 | 416 | std::vector assignClosestCorners(std::vector cand, std::vector pred) 417 | { 418 | std::vector idx; 419 | if (cand.size() < pred.size()) 420 | return idx; 421 | idx.resize(pred.size()); 422 | cv::Mat D = cv::Mat::zeros(cv::Size(pred.size(), cand.size()), CV_64F); 423 | for (unsigned int i = 0; i < pred.size(); i++) 424 | { 425 | for (unsigned int j = 0; j < cand.size(); j++) 426 | { 427 | cv::Point2d delta = cand[j] - pred[i]; 428 | D.at(j, i) = sqrt(delta.x*delta.x+delta.y*delta.y); 429 | } 430 | } 431 | for (unsigned int i = 0; i < pred.size(); i++) 432 | { 433 | double minVal, maxVal; 434 | int minIdx[2] = {}, maxIdx[2] = {}; // minnimum Index, maximum Index 435 | cv::minMaxIdx(D, &minVal, &maxVal, minIdx, maxIdx); 436 | idx[minIdx[1]] = minIdx[0]; 437 | for (int j = 0; j < D.cols; j++) 438 | { 439 | D.at(minIdx[0], j) = 1e10; 440 | } 441 | for (int j = 0; j < D.rows; j++) 442 | { 443 | D.at(j, minIdx[1]) = 1e10; 444 | } 445 | } 446 | return idx; 447 | } 448 | 449 | void plotChessboards(cv::Mat img, std::vector chessboards, struct Corner_t corners) 450 | { 451 | for (unsigned int i = 0; i < chessboards.size(); i++) 452 | { 453 | cv::Mat chessboard = chessboards[i]; 454 | cv::Point2d o = corners.p[chessboard.at(0, 0)]; 455 | cv::Point2d o1 = corners.p[chessboard.at(0, 1)]; 456 | cv::Point2d o2 = corners.p[chessboard.at(1, 0)]; 457 | if (img.channels() == 3) 458 | { 459 | for (int j = 0; j < chessboard.rows; j++) 460 | { 461 | for (int k = 1; k < chessboard.cols; k++) 462 | { 463 | cv::line(img, corners.p[chessboard.at(j, k - 1)], corners.p[chessboard.at(j, k)], cv::Scalar(0, 0, 255), 2); 464 | } 465 | } 466 | for (int j = 0; j < chessboard.cols; j++) 467 | { 468 | for (int k = 1; k < chessboard.rows; k++) 469 | { 470 | cv::line(img, corners.p[chessboard.at(k - 1, j)], corners.p[chessboard.at(k, j)], cv::Scalar(0, 0, 255), 2); 471 | } 472 | } 473 | cv::line(img, o, o1, cv::Scalar(255, 0, 0), 2); 474 | cv::line(img, o, o2, cv::Scalar(0, 255, 0), 2); 475 | } 476 | else 477 | { 478 | cv::line(img, o, o1, cv::Scalar(100), 2); 479 | cv::line(img, o, o2, cv::Scalar(100), 2); 480 | } 481 | } 482 | } 483 | -------------------------------------------------------------------------------- /DetectCorner/findCorner.cpp: -------------------------------------------------------------------------------- 1 | #include "findCorner.h" 2 | #include "chessboard.h" 3 | #include 4 | #include 5 | #include 6 | 7 | struct Chessboarder_t findCorner(cv::Mat img, int sigma) 8 | { 9 | if (img.channels() == 3) 10 | cv::cvtColor(img, img, cv::COLOR_BGR2GRAY); 11 | cv::Mat du = (cv::Mat_(3, 3) << -1, 0, 1, -1, 0, 1, -1, 0, 1); 12 | cv::Mat dv = du.t(); 13 | cv::Mat img_dv, img_du; 14 | cv::Mat img_angle = cv::Mat::zeros(img.size(), CV_64F); 15 | cv::Mat img_weight = cv::Mat::zeros(img.size(), CV_64F); 16 | cv::Mat img_double; 17 | img.convertTo(img_double, CV_64F); 18 | cv::filter2D(img_double, img_du, img_double.depth(), du, cv::Point(-1, -1)); 19 | cv::filter2D(img_double, img_dv, img_double.depth(), dv, cv::Point(-1, -1)); 20 | for (int i = 0; i < img.rows; i++) 21 | { 22 | for (int j = 0; j < img.cols; j++) 23 | { 24 | img_angle.at(i, j) = double(atan2(img_dv.at(i, j), img_du.at(i, j))); 25 | if (img_angle.at(i, j) < 0) img_angle.at(i, j) += M_PI; 26 | if (img_angle.at(i, j) > M_PI) img_angle.at(i, j) -= M_PI; 27 | img_weight.at(i, j) = sqrt(img_dv.at(i, j) * img_dv.at(i, j) + img_du.at(i, j) * img_du.at(i, j)); 28 | } 29 | } 30 | img.convertTo(img_double, CV_64F); 31 | double minVal, maxVal; 32 | int minIdx[2] = {}, maxIdx[2] = {}; 33 | cv::minMaxIdx(img_double, &minVal, &maxVal, minIdx, maxIdx); 34 | img_double = (img_double - minVal) / (maxVal - minVal); 35 | cv::Mat cxy = img_double.clone(); 36 | cv::Mat Ixy = img_double.clone(); 37 | cv::Mat c45 = img_double.clone(); 38 | cv::Mat Ix = img_double.clone(); 39 | cv::Mat Iy = img_double.clone(); 40 | cv::Mat I_45_45 = img_double.clone(); 41 | secondDerivCornerMetric(img_double, sigma, &cxy, &c45, &Ix, &Iy, &Ixy, &I_45_45); 42 | struct Corner_t corners; 43 | corners.p = nonMaximumSuppression(cxy+c45, 4, 0.07, 5); 44 | corners = getOrientations(img_angle, img_weight, corners, 10); 45 | 46 | int radius[3] = { 8, 12, 16 }; 47 | corners = scoreCorners(img_double, img_weight, corners, radius); 48 | auto iterator_v1 = corners.v1.begin(); 49 | auto iterator_v2 = corners.v2.begin(); 50 | auto iterator_p = corners.p.begin(); 51 | auto iterator_score = corners.score.begin(); 52 | while (iterator_v1 != corners.v1.end()) 53 | { 54 | if (*iterator_score < 0.01) 55 | { 56 | corners.v1.erase(iterator_v1); 57 | corners.v2.erase(iterator_v2); 58 | corners.p.erase(iterator_p); 59 | corners.score.erase(iterator_score); 60 | iterator_v1--; 61 | iterator_v2--; 62 | iterator_p--; 63 | iterator_score--; 64 | } 65 | iterator_v1++; 66 | iterator_v2++; 67 | iterator_p++; 68 | iterator_score++; 69 | } 70 | std::vector chessboards = chessboardsFromCorners(corners); 71 | struct Corner_t new_corner; 72 | for (unsigned int j = 0; j < chessboards.size(); j++) 73 | { 74 | for (int u = 0; u < chessboards[j].rows; u++) 75 | { 76 | for (int v = 0; v < chessboards[j].cols; v++) 77 | { 78 | new_corner.p.push_back(corners.p[chessboards[j].at(u, v)]); 79 | new_corner.v1.push_back(corners.v1[chessboards[j].at(u, v)]); 80 | new_corner.v2.push_back(corners.v2[chessboards[j].at(u, v)]); 81 | } 82 | } 83 | } 84 | new_corner = subPixelLocation(Ixy, new_corner); 85 | int cnt = 0; 86 | for (unsigned int j = 0; j < chessboards.size(); j++) 87 | { 88 | for (int u = 0; u < chessboards[j].rows; u++) 89 | { 90 | for (int v = 0; v < chessboards[j].cols; v++) 91 | { 92 | corners.p[chessboards[j].at(u, v)] = new_corner.p[cnt]; 93 | cnt++; 94 | } 95 | } 96 | } 97 | struct Chessboarder_t chess; 98 | chess.corners = corners; 99 | chess.chessboard = chessboards; 100 | return chess; 101 | } 102 | 103 | void secondDerivCornerMetric(cv::Mat I, int sigma, cv::Mat* cxy, cv::Mat* c45, cv::Mat* Ix, cv::Mat* Iy, cv::Mat* Ixy, cv::Mat* I_45_45) 104 | { 105 | cv::Mat Ig; 106 | cv::GaussianBlur(I, Ig, cv::Size(sigma*7+1, sigma*7+1), sigma, sigma); 107 | cv::Mat du = (cv::Mat_(1, 3) << 1, 0, -1); 108 | cv::Mat dv = du.t(); 109 | cv::filter2D(Ig, *Ix, Ig.depth(), du, cv::Point(-1, -1)); 110 | cv::filter2D(Ig, *Iy, Ig.depth(), dv, cv::Point(-1, -1)); 111 | 112 | cv::Mat I_45 = I.clone(); 113 | cv::Mat I_n45 = I.clone(); 114 | double cosPi4 = cos(M_PI/4); 115 | double cosNegPi4 = cos(-M_PI / 4); 116 | double sinPi4 = sin(M_PI / 4); 117 | double sinNegPi4 = sin(-M_PI / 4); 118 | for (int i = 0; i < I.rows; i++) 119 | { 120 | for (int j = 0; j < I.cols; j++) 121 | { 122 | I_45.at(i, j) = Ix->at(i, j) * cosPi4 + Iy->at(i, j) * sinPi4; 123 | I_n45.at(i, j) = Ix->at(i, j) * cosNegPi4 + Iy->at(i, j) * sinNegPi4; 124 | } 125 | } 126 | 127 | cv::filter2D(*Ix, *Ixy, Ix->depth(), dv, cv::Point(-1, -1)); 128 | cv::Mat I_45_x, I_45_y; 129 | cv::filter2D(I_45, I_45_x, I_45.depth(), du, cv::Point(-1, -1)); 130 | cv::filter2D(I_45, I_45_y, I_45.depth(), dv, cv::Point(-1, -1)); 131 | for (int i = 0; i < I.rows; i++) 132 | { 133 | for (int j = 0; j < I.cols; j++) 134 | { 135 | I_45_45->at(i, j) = I_45_x.at(i, j) * cosNegPi4 + I_45_y.at(i, j) * sinNegPi4; 136 | cxy->at(i, j) = sigma*sigma*fabs(Ixy->at(i, j)) - 1.5*sigma*(fabs(I_45.at(i, j)) + fabs(I_n45.at(i, j))); 137 | if (cxy->at(i, j) < 0) cxy->at(i, j) = 0; 138 | c45->at(i, j) = sigma*sigma*fabs(I_45_45->at(i, j)) - 1.5*sigma*(fabs(Ix->at(i, j)) + fabs(Iy->at(i, j))); 139 | if (c45->at(i, j) < 0) c45->at(i, j) = 0; 140 | } 141 | } 142 | } 143 | 144 | std::vector nonMaximumSuppression(cv::Mat img, int n, double tau, int margin) 145 | { 146 | std::vector results; 147 | int width = img.cols; 148 | int height = img.rows; 149 | for (int i = n + margin; i < width - n - margin; i += n + 1) 150 | { 151 | for (int j = n + margin; j < height - n - margin; j += n + 1) 152 | { 153 | int maxi = i; 154 | int maxj = j; 155 | double maxval = img.at(j, i); 156 | for (int i2 = i; i2 <= i + n; i2++) 157 | { 158 | for (int j2 = j; j2 <= j + n; j2++) 159 | { 160 | double currval = img.at(j2, i2); 161 | if (currval > maxval) 162 | { 163 | maxi = i2; 164 | maxj = j2; 165 | maxval = currval; 166 | } 167 | } 168 | } 169 | 170 | bool failed = false; 171 | for (int i2 = maxi - n; i2 < std::min(maxi + n, width - margin); i2++) 172 | { 173 | for (int j2 = maxj - n; j2 < std::min(maxj + n, height - margin); j2++) 174 | { 175 | double currval = img.at(j2, i2); 176 | if (currval > maxval && (i2i + n || j2j + n)) 177 | { 178 | failed = true; 179 | break; 180 | } 181 | } 182 | if (failed) 183 | break; 184 | } 185 | if (maxval >= tau && failed == false) 186 | { 187 | cv::Point2d point = cv::Point2d(double(maxi), double(maxj)); 188 | results.push_back(point); 189 | } 190 | } 191 | } 192 | return results; 193 | } 194 | 195 | double normpdf(double x, int mu, int sigma) 196 | { 197 | return double(exp(-(x - mu)*(x - mu) / 2 / sigma / sigma) / sqrt(2 * M_PI) / sigma); 198 | } 199 | 200 | struct Corner_t getOrientations(cv::Mat img_angle, cv::Mat img_weight, struct Corner_t corners, int r) 201 | { 202 | int height = img_angle.rows; 203 | int width = img_angle.cols; 204 | corners.score.resize(corners.p.size()); 205 | corners.v1.resize(corners.p.size()); 206 | corners.v2.resize(corners.p.size()); 207 | for (unsigned int i = 0; i < corners.p.size(); i++) 208 | { 209 | int cu = corners.p[i].x; 210 | int cv = corners.p[i].y; 211 | int x1 = std::min(cv + r, height - 1); 212 | int x2 = std::max(cv - r, 0); 213 | int x3 = std::min(cu + r, width - 1); 214 | int x4 = std::max(cu - r, 0); 215 | cv::Mat img_angle_sub = cv::Mat::zeros(cv::Size(x3 - x4 + 1, x1 - x2 + 1), CV_64F); 216 | cv::Mat img_weight_sub = cv::Mat::zeros(cv::Size(x3 - x4 + 1, x1 - x2 + 1), CV_64F); 217 | for (int j = 0; j <= x1 - x2; j++) 218 | { 219 | for (int k = 0; k <= x3 - x4; k++) 220 | { 221 | img_angle_sub.at(j, k) = img_angle.at(j + x2, k + x4); 222 | img_weight_sub.at(j, k) = img_weight.at(j + x2, k + x4); 223 | } 224 | } 225 | cv::Point2d v1, v2; 226 | std::vector v; 227 | v = edgeOrientations(img_angle_sub, img_weight_sub); 228 | v1 = v[0]; 229 | v2 = v[1]; 230 | corners.v1[i] = v1; 231 | corners.v2[i] = v2; 232 | } 233 | return corners; 234 | } 235 | 236 | std::vector edgeOrientations(cv::Mat img_angle, cv::Mat img_weight) 237 | { 238 | std::vector v; 239 | v.push_back(cv::Point2d(0,0)); 240 | v.push_back(cv::Point2d(0,0)); 241 | int bin_num = 32; 242 | cv::Mat angle_hist = cv::Mat::zeros(cv::Size(bin_num, 1), CV_64F); 243 | for (int i = 0; i < img_angle.rows; i++) 244 | { 245 | for (int j = 0; j < img_angle.cols; j++) 246 | { 247 | img_angle.at(i, j) += M_PI / 2; 248 | if (img_angle.at(i, j) > M_PI) img_angle.at(i, j) -= M_PI; 249 | int bin = std::max(std::min(int(floor(img_angle.at(i, j) / (M_PI / bin_num))), bin_num - 1), 0); 250 | angle_hist.at(0, bin) += img_weight.at(i, j); 251 | } 252 | } 253 | std::vector modes = findModesMeanShift(angle_hist, 1); 254 | if (modes.size() <= 1) return v; 255 | for (unsigned int i = 0; i < modes.size(); i++) 256 | { 257 | modes[i].z = modes[i].x*M_PI / bin_num; 258 | } 259 | double delta_angle = 0; 260 | 261 | if (modes[0].z > modes[1].z) 262 | { 263 | v[0] = cv::Point2d(cos(modes[1].z), sin(modes[1].z)); 264 | v[1] = cv::Point2d(cos(modes[0].z), sin(modes[0].z)); 265 | delta_angle = std::min(modes[0].z - modes[1].z, modes[1].z + M_PI - modes[0].z); 266 | if(delta_angle <= 0.3) 267 | v[0] = cv::Point2d(cos(modes[2].z), sin(modes[2].z)); 268 | } 269 | else 270 | { 271 | v[0] = cv::Point2d(cos(modes[0].z), sin(modes[0].z)); 272 | v[1] = cv::Point2d(cos(modes[1].z), sin(modes[1].z)); 273 | delta_angle = std::min(modes[1].z - modes[0].z, modes[0].z + M_PI - modes[1].z); 274 | if(delta_angle <= 0.3) 275 | v[1] = cv::Point2d(cos(modes[2].z), sin(modes[2].z)); 276 | } 277 | return v; 278 | 279 | } 280 | 281 | bool comp(const cv::Point3d &a, const cv::Point3d &b) 282 | { 283 | return a.y > b.y; 284 | } 285 | 286 | std::vector findModesMeanShift(cv::Mat hist, int sigma) 287 | { 288 | std::vector modes; 289 | cv::Mat hist_smoothed = hist.clone(); 290 | for (int i = 0; i < hist.cols; i++) 291 | { 292 | double sum = 0; 293 | for (int j = -round(2 * sigma); j <= round(2 * sigma); j++) 294 | { 295 | int idx = fmod(i + j, hist.cols); 296 | sum += hist.at(0, idx) * normpdf(j, 0, sigma); 297 | } 298 | hist_smoothed.at(0, i) = sum; 299 | } 300 | bool failed = true; 301 | for (int i = 1; i < hist_smoothed.cols; i++) 302 | { 303 | if (fabs(hist_smoothed.at(0, i) - hist_smoothed.at(0, 0)) > 1e-5) 304 | { 305 | failed = false; 306 | break; 307 | } 308 | } 309 | if (failed == true) 310 | return modes; 311 | 312 | for (int i = 0; i < hist_smoothed.cols; i++) 313 | { 314 | int j = i; 315 | while (1) 316 | { 317 | double h0 = hist_smoothed.at(0, j); 318 | int j1 = fmod(j + 1, hist_smoothed.cols); 319 | int j2 = fmod(j - 1, hist_smoothed.cols); 320 | double h1 = hist_smoothed.at(0, j1); 321 | double h2 = hist_smoothed.at(0, j2); 322 | if (h1 >= h0 && h1 >= h2) 323 | j = j1; 324 | else if (h2 > h0 && h2 > h1) 325 | j = j2; 326 | else 327 | break; 328 | } 329 | cv::Point3d mode = cv::Point3d(j, hist_smoothed.at(0, j), 0); 330 | if (modes.size() == 0) 331 | modes.push_back(mode); 332 | else 333 | { 334 | bool find_same = false; 335 | for (unsigned int k = 0; k < modes.size(); k++) 336 | { 337 | if (modes[k].x == j) 338 | { 339 | find_same = true; 340 | break; 341 | } 342 | } 343 | if (find_same == false) 344 | modes.push_back(mode); 345 | } 346 | } 347 | sort(modes.begin(), modes.end(), comp); 348 | return modes; 349 | } 350 | 351 | struct Template_t createCorrelationPatch(double angle_1, double angle_2, int radius) 352 | { 353 | struct Template_t result; 354 | int width = radius * 2 + 1; 355 | int height = radius * 2 + 1; 356 | 357 | result.a1 = cv::Mat::zeros(cv::Size(width, height), CV_64F); 358 | result.a2 = cv::Mat::zeros(cv::Size(width, height), CV_64F); 359 | result.b1 = cv::Mat::zeros(cv::Size(width, height), CV_64F); 360 | result.b2 = cv::Mat::zeros(cv::Size(width, height), CV_64F); 361 | 362 | int mu = radius; 363 | int mv = radius; 364 | 365 | for (int u = 0; u < width; u++) 366 | { 367 | for (int v = 0; v < height; v++) 368 | { 369 | int du = u - mu; 370 | int dv = v - mv; 371 | double dist = sqrt(du*du + dv*dv); 372 | double s1 = -du*sin(angle_1) + dv*cos(angle_1); 373 | double s2 = -du*sin(angle_2) + dv*cos(angle_2); 374 | if (s1 <= -0.1 && s2 <= -0.1) 375 | result.a1.at(v, u) = normpdf(dist, 0, radius / 2); 376 | else if (s1 >= 0.1 && s2 >= 0.1) 377 | result.a2.at(v, u) = normpdf(dist, 0, radius / 2); 378 | else if (s1 <= -0.1 && s2 >= 0.1) 379 | result.b1.at(v, u) = normpdf(dist, 0, radius / 2); 380 | else if (s1 >= 0.1 && s2 <= -0.1) 381 | result.b2.at(v, u) = normpdf(dist, 0, radius / 2); 382 | } 383 | } 384 | cv::normalize(result.a1, result.a1, 1, 0, cv::NORM_L1); 385 | cv::normalize(result.a2, result.a2, 1, 0, cv::NORM_L1); 386 | cv::normalize(result.b1, result.b1, 1, 0, cv::NORM_L1); 387 | cv::normalize(result.b2, result.b2, 1, 0, cv::NORM_L1); 388 | return result; 389 | } 390 | 391 | struct Corner_t scoreCorners(cv::Mat img, cv::Mat img_weight, struct Corner_t corners, int* radius) 392 | { 393 | int width = img.cols; 394 | int height = img.rows; 395 | std::vector score = { 0,0,0 }; 396 | for (unsigned int i = 0; i < corners.p.size(); i++) 397 | { 398 | int u = round(corners.p[i].x); 399 | int v = round(corners.p[i].y); 400 | for (int j = 0; j < 3; j++) 401 | { 402 | score[j] = 0; 403 | if (u >= radius[j] && u < width - radius[j] && v >= radius[j] && v < height - radius[j]) 404 | { 405 | int x1 = v - radius[j]; 406 | int x2 = v + radius[j]; 407 | int x3 = u - radius[j]; 408 | int x4 = u + radius[j]; 409 | cv::Mat img_sub = cv::Mat::zeros(cv::Size(x4 - x3 + 1, x2 - x1 + 1), CV_64F); 410 | cv::Mat img_weight_sub = cv::Mat::zeros(cv::Size(x4 - x3 + 1, x2 - x1 + 1), CV_64F); 411 | for (int x = 0; x <= x4 - x3; x++) 412 | { 413 | for (int y = 0; y <= x2 - x1; y++) 414 | { 415 | img_sub.at(y, x) = img.at(y + x1, x + x3); 416 | img_weight_sub.at(y, x) = img_weight.at(y + x1, x + x3); 417 | } 418 | } 419 | score[j] = cornerCorrelationScore(img_sub, img_weight_sub, corners.v1[i], corners.v2[i]); 420 | } 421 | } 422 | std::sort(score.begin(), score.end()); 423 | corners.score[i] = score[2]; 424 | } 425 | return corners; 426 | } 427 | 428 | double cornerCorrelationScore(cv::Mat img, cv::Mat img_weight, cv::Point2d v1, cv::Point2d v2) 429 | { 430 | int c[2] = { (img_weight.rows - 1) / 2, (img_weight.rows - 1) / 2 }; 431 | cv::Mat img_filter = cv::Mat::ones(img_weight.size(), CV_64F) * -1; 432 | for (int x = 0; x < img_weight.cols; x++) 433 | { 434 | for (int y = 0; y < img_weight.rows; y++) 435 | { 436 | double p1[2], p2[2], p3[2]; 437 | p1[0] = x - c[0]; 438 | p1[1] = y - c[1]; 439 | p2[0] = (p1[0] * v1.x + p1[1] * v1.y)*v1.x; 440 | p2[1] = (p1[0] * v1.x + p1[1] * v1.y)*v1.y; 441 | p3[0] = (p1[0] * v2.x + p1[1] * v2.y)*v2.x; 442 | p3[1] = (p1[0] * v2.x + p1[1] * v2.y)*v2.y; 443 | p2[0] = p1[0] - p2[0]; 444 | p2[1] = p1[1] - p2[1]; 445 | p3[0] = p1[0] - p3[0]; 446 | p3[1] = p1[1] - p3[1]; 447 | if (sqrt(p2[0] * p2[0] + p2[1] * p2[1]) <= 1.5 || 448 | sqrt(p3[0] * p3[0] + p3[1] * p3[1]) <= 1.5) 449 | { 450 | img_filter.at(y, x) = 1; 451 | } 452 | } 453 | } 454 | cv::Mat mat_mean, mat_stddev; 455 | cv::meanStdDev(img_weight, mat_mean, mat_stddev); 456 | img_weight = (img_weight - mat_mean.at(0, 0)) / mat_stddev.at(0, 0); 457 | cv::meanStdDev(img_filter, mat_mean, mat_stddev); 458 | img_filter = (img_filter - mat_mean.at(0, 0)) / mat_stddev.at(0, 0); 459 | double sum = 0, score_gradient = 0; 460 | for (int i = 0; i < img_weight.rows; i++) 461 | { 462 | for (int j = 0; j < img_weight.cols; j++) 463 | { 464 | sum += img_weight.at(i, j)*img_filter.at(i, j); 465 | } 466 | } 467 | score_gradient = std::max(sum / (img_weight.rows*img_weight.cols - 1), 0.0); 468 | struct Template_t Template; 469 | Template = createCorrelationPatch(atan2(v1.y, v1.x), atan2(v2.y, v2.x), c[0]); 470 | double a1 = 0, a2 = 0, b1 = 0, b2 = 0; 471 | for (int i = 0; i < img.rows; i++) 472 | { 473 | for (int j = 0; j < img.cols; j++) 474 | { 475 | a1 += Template.a1.at(i, j) * img.at(i, j); 476 | a2 += Template.a2.at(i, j) * img.at(i, j); 477 | b1 += Template.b1.at(i, j) * img.at(i, j); 478 | b2 += Template.b2.at(i, j) * img.at(i, j); 479 | } 480 | } 481 | double mu = (a1 + a2 + b1 + b2) / 4; 482 | double score_a = std::min(a1 - mu, a2 - mu); 483 | double score_b = std::min(mu - b1, mu - b2); 484 | double score_1 = std::min(score_a, score_b); 485 | score_a = std::min(mu - a1, mu - a2); 486 | score_b = std::min(b1 - mu, b2 - mu); 487 | double score_2 = std::min(score_a, score_b); 488 | double score_intensity = std::max(std::max(score_1, score_2), 0.0); 489 | return score_gradient*score_intensity; 490 | } 491 | 492 | struct Corner_t subPixelLocation(cv::Mat metric, struct Corner_t corners) 493 | { 494 | // cv::Mat X = (cv::Mat_(6, 25) << 0.028571, 0.028571, 0.028571, 0.028571, 0.028571, -0.014286, -0.014286, -0.014286, -0.014286, -0.014286, -0.028571, -0.028571, -0.028571, -0.028571, -0.028571, -0.014286, -0.014286, -0.014286, -0.014286, -0.014286, 0.028571, 0.028571, 0.028571, 0.028571, 0.028571, 0.028571, -0.014286, -0.028571, -0.014286, 0.028571, 0.028571, -0.014286, -0.028571, -0.014286, 0.028571, 0.028571, -0.014286, -0.028571, -0.014286, 0.028571, 0.028571, -0.014286, -0.028571, -0.014286, 0.028571, 0.028571, -0.014286, -0.028571, -0.014286, 0.028571, -0.04, -0.04, -0.04, -0.04, -0.04, -0.02, -0.02, -0.02, -0.02, -0.02, 0, 0, 0, 0, 0, 0.02, 0.02, 0.02, 0.02, 0.02, 0.04, 0.04, 0.04, 0.04, 0.04, -0.04, -0.02, 0, 0.02, 0.04, -0.04, -0.02, 0, 0.02, 0.04, -0.04, -0.02, 0, 0.02, 0.04, -0.04, -0.02, 0, 0.02, 0.04, -0.04, -0.02, 0, 0.02, 0.04, 0.04, 0.02, 0, -0.02, -0.04, 0.02, 0.01, 0, -0.01, -0.02, 0, 0, 0, 0, 0, -0.02, -0.01, 0, 0.01, 0.02, -0.04, -0.02, 0, 0.02, 0.04, -0.074286, 0.011429, 0.04, 0.011429, -0.074286, 0.011429, 0.097143, 0.12571, 0.097143, 0.011429, 0.04, 0.12571, 0.15429, 0.12571, 0.04, 0.011429, 0.097143, 0.12571, 0.097143, 0.011429, -0.074286, 0.011429, 0.04, 0.011429, -0.074286); 495 | cv::Mat A = cv::Mat(cv::Size(6,25), CV_64F); 496 | for(int y = -2; y <= 2; y++) 497 | { 498 | for(int x = -2; x <= 2; x++) 499 | { 500 | int index = (x+2)*5 + y + 2; 501 | A.at(index, 0) = x*x; 502 | A.at(index, 1) = y*y; 503 | A.at(index, 2) = x; 504 | A.at(index, 3) = y; 505 | A.at(index, 4) = x*y; 506 | A.at(index, 5) = 1; 507 | } 508 | } 509 | cv::Mat X = (A.t()*A).inv()*A.t(); 510 | for (unsigned int i = 0; i < corners.p.size(); i++) 511 | { 512 | cv::Point2d p = corners.p[i]; 513 | int halfPatchSize = 2; 514 | cv::Mat patch = cv::Mat::zeros(cv::Size(1, 25), CV_64F); 515 | int cnt = 0; 516 | for (int j = p.x - halfPatchSize; j <= p.x + halfPatchSize; j++) 517 | { 518 | for (int k = p.y - halfPatchSize; k <= p.y + halfPatchSize; k++) 519 | { 520 | patch.at(cnt, 0) = metric.at(k, j); 521 | cnt++; 522 | } 523 | } 524 | cv::Mat beta = X * patch; 525 | double A = beta.at(0,0); 526 | double B = beta.at(1, 0); 527 | double C = beta.at(2, 0); 528 | double D = beta.at(3, 0); 529 | double E = beta.at(4, 0); 530 | double x = -(2 * B*C - D*E) / (4 * A*B - E*E); 531 | double y = -(2 * A*D - C*E) / (4 * A*B - E*E); 532 | if (fabs(x) > halfPatchSize || fabs(y) > halfPatchSize) 533 | { 534 | x = 0; 535 | y = 0; 536 | } 537 | corners.p[i].x += x; 538 | corners.p[i].y += y; 539 | } 540 | return corners; 541 | } 542 | -------------------------------------------------------------------------------- /multi_calib.cpp: -------------------------------------------------------------------------------- 1 | #include "multi_calib.h" 2 | #include "cout_style.h" 3 | 4 | std::vector camera_name{"front", "right", "rear", "left"}; 5 | 6 | MultiCalib::MultiCalib(std::vector cameras, const std::vector& worlds) 7 | { 8 | worlds_ = worlds; 9 | int camera_num = cameras.size(); 10 | int chessboard_num = cameras[0].has_chessboard().size(); 11 | cameras_.resize(camera_num); 12 | chessboards_.resize(chessboard_num); 13 | // 粗略的估计所有相机和棋盘格的位姿 14 | // 首先计算相机的位姿 15 | for(int i = 0; i < camera_num; i++) 16 | { 17 | cv::Mat R, t; 18 | if(i == 0) 19 | { 20 | // 第一个相机假设为世界坐标系原点 21 | R = (cv::Mat_(3,3) << 1,0,0, 0,1,0, 0,0,1); 22 | t = (cv::Mat_(3,1) << 0,0,0); 23 | } 24 | else 25 | { 26 | // 根据上一个初始化的相机，计算这个相机所有可能的位姿 27 | std::vector Rs; 28 | std::vector ts; 29 | for(int j = 0; j < chessboard_num; j++) 30 | { 31 | if(cameras_[i-1].is_initial() == false) 32 | { 33 | std::cout << "请按相邻顺序输出相机" << std::endl; 34 | return; 35 | } 36 | if(cameras[i-1].has_chessboard(j) == false || cameras[i].has_chessboard(j) == false) continue; 37 | cv::Mat chess_Rt = cameras[i].Rt(j); 38 | cv::Mat chess_R_i, chess_t_i, chess_R_k, chess_t_k; 39 | Rt_to_R_t(chess_Rt, chess_R_i, chess_t_i); 40 | chess_Rt = cameras[i-1].Rt(j); 41 | Rt_to_R_t(chess_Rt, chess_R_k, chess_t_k); 42 | cv::Mat camera_R_k = cameras_[i-1].R(); 43 | cv::Mat camera_t_k = cameras_[i-1].t(); 44 | cv::Mat R_ik = chess_R_i * chess_R_k.t(); 45 | cv::Mat t_ik = chess_t_i - R_ik * chess_t_k; 46 | Rs.push_back(R_ik * cameras_[i-1].R()); 47 | ts.push_back(R_ik * cameras_[i-1].t() + t_ik); 48 | } 49 | // 从这些位姿中选取重投影误差最小的位姿 50 | double min_error = 1e10; 51 | double min_id = -1; 52 | for(int j = 0; j < Rs.size(); j++) 53 | { 54 | double error = 0; 55 | for(int k = 0; k < chessboard_num; k++) 56 | { 57 | if(cameras[i-1].has_chessboard(k) == false || cameras[i].has_chessboard(k) == false) continue; 58 | cv::Mat chess_Rt = cameras[i].Rt(k); 59 | cv::Mat chess_R_i, chess_t_i, chess_R_k, chess_t_k; 60 | Rt_to_R_t(chess_Rt, chess_R_i, chess_t_i); 61 | cv::Mat camera_R_k = cameras_[i-1].R(); 62 | cv::Mat camera_t_k = cameras_[i-1].t(); 63 | cv::Mat R_ki = camera_R_k * Rs[j].t(); 64 | cv::Mat t_ki = camera_t_k - R_ki * ts[j]; 65 | chess_R_k = R_ki * chess_R_i; 66 | chess_t_k = R_ki * chess_t_i + t_ki; 67 | double e = cameras[i-1].ReprojectError(cameras[i-1].pixels()[k], worlds, chess_R_k, chess_t_k); 68 | // std::cout << e << std::endl; 69 | error += e; 70 | chess_Rt = cameras[i-1].Rt(k); 71 | Rt_to_R_t(chess_Rt, chess_R_k, chess_t_k); 72 | R_ki = Rs[j] * camera_R_k.t(); 73 | t_ki = ts[j] - R_ki * camera_t_k; 74 | chess_R_i = R_ki * chess_R_k; 75 | chess_t_i = R_ki * chess_t_k + t_ki; 76 | e = cameras[i].ReprojectError(cameras[i].pixels()[k], worlds, chess_R_i, chess_t_i); 77 | // std::cout << e << std::endl; 78 | error += e; 79 | } 80 | if(error < min_error) 81 | { 82 | min_error = error; 83 | min_id = j; 84 | } 85 | } 86 | R = Rs[min_id]; 87 | t = ts[min_id]; 88 | } 89 | cameras_[i] = MultiCalib_camera(cameras[i].cx(), cameras[i].cy(), cameras[i].fx(), cameras[i].fy(), cameras[i].xi(), cameras[i].lamda(), cameras[i].alpha(), 90 | cameras[i].b(), cameras[i].c(), 91 | R, t, cameras[i].has_chessboard(), cameras[i].pixels()); 92 | } 93 | // 然后计算棋盘的位姿 94 | for(int i = 0; i < chessboard_num; i++) 95 | { 96 | cv::Mat R, t; 97 | std::vector camera_ids; 98 | for(int j = 0; j < cameras.size(); j++) 99 | { 100 | if(cameras[j].has_chessboard(i)) camera_ids.push_back(j); 101 | } 102 | if(camera_ids.size() == 0) continue; 103 | if(camera_ids.size() == 1) 104 | { 105 | cv::Mat camera_R = cameras_[camera_ids[0]].R(); 106 | cv::Mat camera_t = cameras_[camera_ids[0]].t(); 107 | cv::Mat chess_Rt = cameras[camera_ids[0]].Rt(i); 108 | cv::Mat chess_R, chess_t; 109 | Rt_to_R_t(chess_Rt, chess_R, chess_t); 110 | R = camera_R.t() * chess_R; 111 | t = camera_R.t() * (chess_t - camera_t); 112 | } 113 | else 114 | { 115 | // 计算棋盘所有可能的位姿 116 | std::vector Rs, ts; 117 | for(int j = 0; j < camera_ids.size(); j++) 118 | { 119 | cv::Mat camera_R = cameras_[camera_ids[j]].R(); 120 | cv::Mat camera_t = cameras_[camera_ids[j]].t(); 121 | cv::Mat chess_Rt = cameras[camera_ids[j]].Rt(i); 122 | cv::Mat chess_R, chess_t; 123 | Rt_to_R_t(chess_Rt, chess_R, chess_t); 124 | Rs.push_back(camera_R.t() * chess_R); 125 | ts.push_back(camera_R.t() * (chess_t - camera_t)); 126 | } 127 | // 选取重投影最小的位姿 128 | double min_error = 1e10; 129 | double min_id = -1; 130 | for(int j = 0; j < Rs.size(); j++) 131 | { 132 | double error = 0; 133 | for(int k = 0; k < camera_ids.size(); k++) 134 | { 135 | cv::Mat camera_R = cameras_[camera_ids[k]].R(); 136 | cv::Mat camera_t = cameras_[camera_ids[k]].t(); 137 | cv::Mat chess_R = camera_R * Rs[j]; 138 | cv::Mat chess_t = camera_R * ts[j] + camera_t; 139 | double e = cameras[camera_ids[k]].ReprojectError(cameras[camera_ids[k]].pixels()[i], worlds, chess_R, chess_t); 140 | error += e; 141 | } 142 | if(error < min_error) 143 | { 144 | min_error = error; 145 | min_id = j; 146 | } 147 | } 148 | R = Rs[min_id]; 149 | t = ts[min_id]; 150 | } 151 | chessboards_[i] = MultiCalib_chessboard(R, t); 152 | } 153 | } 154 | 155 | void MultiCalib::calibrate() 156 | { 157 | int camera_num = cameras_.size(); 158 | int chessboard_num = chessboards_.size(); 159 | 160 | ceres::Problem problem; 161 | 162 | for(int m = 0; m < camera_num; m++) 163 | { 164 | std::vector> pixels = cameras_[m].pixels(); 165 | for(int i = 0; i < chessboard_num; i++) 166 | { 167 | if(chessboards_[i].is_initial()) 168 | { 169 | for(int j = 0; j < pixels[i].size(); j++) 170 | { 171 | if(m == 0) 172 | { 173 | ReprojectionError *cost_function = 174 | new ReprojectionError(pixels[i][j], worlds_[j]); 175 | 176 | problem.AddResidualBlock( 177 | new ceres::AutoDiffCostFunction< 178 | ReprojectionError, 179 | 2, // num_residuals 180 | 6,6,9>(cost_function), 181 | NULL, 182 | cameras_[m].rt_.data(), 183 | chessboards_[i].rt_.data(), 184 | cameras_[m].intrinsic_.data() 185 | ); 186 | problem.SetParameterBlockConstant(cameras_[m].rt_.data()); 187 | } 188 | else 189 | { 190 | ReprojectionError *cost_function = 191 | new ReprojectionError(pixels[i][j], worlds_[j]); 192 | 193 | problem.AddResidualBlock( 194 | new ceres::AutoDiffCostFunction< 195 | ReprojectionError, 196 | 2, // num_residuals 197 | 6,6,9>(cost_function), 198 | NULL, 199 | cameras_[m].rt_.data(), 200 | chessboards_[i].rt_.data(), 201 | cameras_[m].intrinsic_.data() 202 | ); 203 | } 204 | } 205 | } 206 | } 207 | } 208 | // Configure the solver. 209 | ceres::Solver::Options options; 210 | options.linear_solver_type = ceres::DENSE_SCHUR; 211 | options.minimizer_progress_to_stdout =false; 212 | // options.max_num_iterations = 100; 213 | 214 | // Solve! 215 | ceres::Solver::Summary summary; 216 | ceres::Solve(options, &problem, &summary); 217 | 218 | std::cout << summary.BriefReport() << std::endl; 219 | 220 | // 更新相机和棋盘格的参数 221 | for(int i = 0; i < camera_num; i++) 222 | { 223 | if(cameras_[i].is_initial() == false) 224 | continue; 225 | cameras_[i].update_param(); 226 | } 227 | for(int i = 0; i < chessboard_num; i++) 228 | { 229 | if(chessboards_[i].is_initial() == false) 230 | continue; 231 | chessboards_[i].update_param(); 232 | } 233 | // 计算重投影误差 234 | std::cout << blue << "多相机标定结束" << reset << std::endl; 235 | double error_sum = 0; 236 | int cntt = 0; 237 | for(int m = 0; m < camera_num; m++) 238 | { 239 | std::vector> pixels = cameras_[m].pixels(); 240 | double error = 0; 241 | double cnt = 0; 242 | for(int i = 0; i < chessboard_num; i++) 243 | { 244 | if(chessboards_[i].is_initial()) 245 | { 246 | for(int j = 0; j < pixels[i].size(); j++) 247 | { 248 | cv::Mat R = chessboards_[i].R(); 249 | cv::Mat t = chessboards_[i].t(); 250 | cv::Mat p = (cv::Mat_(3, 1) << worlds_[j].x, worlds_[j].y, worlds_[j].z); 251 | p = R * p + t; 252 | R = cameras_[m].R(); 253 | t = cameras_[m].t(); 254 | p = R * p + t; 255 | double fx = cameras_[m].fx(); 256 | double fy = cameras_[m].fy(); 257 | double cx = cameras_[m].cx(); 258 | double cy = cameras_[m].cy(); 259 | double alpha = cameras_[m].alpha(); 260 | double lamda = cameras_[m].lamda(); 261 | double xi = cameras_[m].xi(); 262 | double b = cameras_[m].b(); 263 | double c = cameras_[m].c(); 264 | double X = p.at(0,0); 265 | double Y = p.at(1,0); 266 | double Z = p.at(2,0); 267 | double d1 = std::sqrt(X*X+Y*Y+Z*Z); 268 | double d2 = std::sqrt(X*X+Y*Y+std::pow(Z+xi*d1,2)); 269 | double d3 = std::sqrt(X*X+Y*Y+std::pow(Z+xi*d1+lamda*d2,2)); 270 | double ksai = Z+xi*d1+lamda*d2+alpha/(1-alpha)*d3; 271 | double pixel_x = fx * X/ksai + b * Y/ksai + cx; 272 | double pixel_y = c * X/ksai + fy * Y/ksai + cy; 273 | error += std::sqrt((pixels[i][j].x-pixel_x)*(pixels[i][j].x-pixel_x) + (pixels[i][j].y-pixel_y)*(pixels[i][j].y-pixel_y)); 274 | cnt++; 275 | cntt++; 276 | } 277 | } 278 | } 279 | error_sum += error; 280 | error /= cnt; 281 | std::cout << "camera_" << m << " 重投影误差：" << error << std::endl; 282 | } 283 | std::cout << yellow << "average reproject error: " << error_sum/cntt << reset << std::endl; 284 | } 285 | 286 | ////////////////////////////////////////////// 287 | // 可视化部分 288 | ////////////////////////////////////////////// 289 | 290 | double position_x = 0, position_y = 0, last_position_x = 0, last_position_y = 0, position_z = 500; 291 | double angle_x = 0, angle_y = 0, last_angle_x = 0, last_angle_y = 0; 292 | 293 | bool mouse_l_down = false; 294 | bool mouse_r_down = false; 295 | cv::Point last_position(-1, -1); 296 | void on_MouseHandle(int event, int x, int y, int flag, void *param) 297 | { 298 | float value; 299 | switch (event) 300 | { 301 | case cv::EVENT_LBUTTONDOWN: 302 | mouse_l_down = true; 303 | last_position.x = x; 304 | last_position.y = y; 305 | break; 306 | case cv::EVENT_LBUTTONUP: 307 | mouse_l_down = false; 308 | last_angle_x = last_angle_x + (x - last_position.x) * 0.01; 309 | last_angle_y = last_angle_y + (y - last_position.y) * 0.01; 310 | break; 311 | case cv::EVENT_RBUTTONDOWN: 312 | mouse_r_down = true; 313 | last_position.x = x; 314 | last_position.y = y; 315 | break; 316 | case cv::EVENT_RBUTTONUP: 317 | mouse_r_down = false; 318 | last_position_x = last_position_x + x - last_position.x; 319 | last_position_y = last_position_y - (y - last_position.y); 320 | break; 321 | case cv::EVENT_MOUSEMOVE: 322 | if (mouse_l_down) 323 | { 324 | angle_x = last_angle_x + (x - last_position.x) * 0.01; 325 | angle_y = last_angle_y + (y - last_position.y) * 0.01; 326 | } 327 | else if (mouse_r_down) 328 | { 329 | position_x = last_position_x + x - last_position.x; 330 | position_y = last_position_y - (y - last_position.y); 331 | } 332 | 333 | break; 334 | case cv::EVENT_MOUSEWHEEL: 335 | value = cv::getMouseWheelDelta(flag); 336 | position_z += value*20; 337 | break; 338 | default:; 339 | } 340 | } 341 | 342 | void MultiCalib::show_result() 343 | { 344 | // 根据姿态计算点的世界坐标 345 | std::vector> camera_points; 346 | std::vector> chessboard_points; 347 | std::vector axis_points; 348 | axis_points.push_back(cv::Point3d(0,0,0)); 349 | axis_points.push_back(cv::Point3d(50,0,0)); 350 | axis_points.push_back(cv::Point3d(0,50,0)); 351 | axis_points.push_back(cv::Point3d(0,0,50)); 352 | for(int i = 0; i < cameras_.size(); i++) 353 | { 354 | if(cameras_[i].is_initial()) 355 | { 356 | std::vector points; 357 | cv::Mat R, t, p; 358 | R = cameras_[i].R(); 359 | t = cameras_[i].t(); 360 | std::cout << "=================" << std::endl; 361 | std::cout << yellow << "[camera_" << i << "]: " << std::endl << t << std::endl; 362 | std::cout << R << std::endl; 363 | std::cout << "fx: " << cameras_[i].fx() << ", fy: " << cameras_[i].fy() << ", cx: " << cameras_[i].cx() << ", cy: " << cameras_[i].cy() << 364 | ", xi: " << cameras_[i].xi() << ", lamda: " << cameras_[i].lamda() << ", alpha: " << cameras_[i].alpha() << 365 | ", b: " << cameras_[i].b() << ", c: " << cameras_[i].c() << std::endl; 366 | std::cout << reset; 367 | R = R.inv(); 368 | t = -R * t; 369 | std::cout << "*****************" << std::endl; 370 | std::cout <(3,1) << -10, -10, 0); 372 | p = R * p + t; 373 | points.push_back(cv::Point3d(p.at(0,0), p.at(1,0), p.at(2,0))); 374 | p = (cv::Mat_(3,1) << 10, -10, 0); 375 | p = R * p + t; 376 | points.push_back(cv::Point3d(p.at(0,0), p.at(1,0), p.at(2,0))); 377 | p = (cv::Mat_(3,1) << 10, 10, 0); 378 | p = R * p + t; 379 | points.push_back(cv::Point3d(p.at(0,0), p.at(1,0), p.at(2,0))); 380 | p = (cv::Mat_(3,1) << -10, 10, 0); 381 | p = R * p + t; 382 | points.push_back(cv::Point3d(p.at(0,0), p.at(1,0), p.at(2,0))); 383 | p = (cv::Mat_(3,1) << -20, -20, 30); 384 | p = R * p + t; 385 | points.push_back(cv::Point3d(p.at(0,0), p.at(1,0), p.at(2,0))); 386 | p = (cv::Mat_(3,1) << 20, -20, 30); 387 | p = R * p + t; 388 | points.push_back(cv::Point3d(p.at(0,0), p.at(1,0), p.at(2,0))); 389 | p = (cv::Mat_(3,1) << 20, 20, 30); 390 | p = R * p + t; 391 | points.push_back(cv::Point3d(p.at(0,0), p.at(1,0), p.at(2,0))); 392 | p = (cv::Mat_(3,1) << -20, 20, 30); 393 | p = R * p + t; 394 | points.push_back(cv::Point3d(p.at(0,0), p.at(1,0), p.at(2,0))); 395 | camera_points.push_back(points); 396 | } 397 | } 398 | for(int i = 0; i < chessboards_.size(); i++) 399 | { 400 | if(chessboards_[i].is_initial()) 401 | { 402 | std::vector points; 403 | cv::Mat R, t, p; 404 | R = chessboards_[i].R(); 405 | t = chessboards_[i].t(); 406 | p = (cv::Mat_(3,1) << -80, -50, 0); 407 | p = R * p + t; 408 | points.push_back(cv::Point3d(p.at(0,0), p.at(1,0), p.at(2,0))); 409 | p = (cv::Mat_(3,1) << 80, -50, 0); 410 | p = R * p + t; 411 | points.push_back(cv::Point3d(p.at(0,0), p.at(1,0), p.at(2,0))); 412 | p = (cv::Mat_(3,1) << 80, 50, 0); 413 | p = R * p + t; 414 | points.push_back(cv::Point3d(p.at(0,0), p.at(1,0), p.at(2,0))); 415 | p = (cv::Mat_(3,1) << -80, 50, 0); 416 | p = R * p + t; 417 | points.push_back(cv::Point3d(p.at(0,0), p.at(1,0), p.at(2,0))); 418 | chessboard_points.push_back(points); 419 | } 420 | } 421 | cv::Mat show_img = cv::Mat(cv::Size(800, 800), CV_8UC3, cv::Scalar(255,255,255)); 422 | cv::imshow("show_window", show_img); 423 | cv::setMouseCallback("show_window", on_MouseHandle); 424 | while(true) 425 | { 426 | cv::Mat show_img = cv::Mat(cv::Size(800, 800), CV_8UC3, cv::Scalar(255,255,255)); 427 | cv::Mat r1 = (cv::Mat_(3, 3) << cos(angle_x), 0, sin(angle_x), 428 | 0, 1, 0, 429 | -sin(angle_x), 0, cos(angle_x)); 430 | cv::Mat r2 = (cv::Mat_(3, 3) << 1, 0, 0, 431 | 0, cos(angle_y), sin(angle_y), 432 | 0, -sin(angle_y), cos(angle_y)); 433 | cv::Mat K = (cv::Mat_(3,3) << 500, 0, 400, 434 | 0, 500, 400, 435 | 0, 0, 1); 436 | r1 = r1 * r2; 437 | cv::Mat Rt = (cv::Mat_(3,4) << r1.at(0,0), r1.at(0,1), r1.at(0,2), position_x, 438 | r1.at(1,0), r1.at(1,1), r1.at(1,2), -position_y, 439 | r1.at(2,0), r1.at(2,1), r1.at(2,2), position_z); 440 | K = K * Rt; 441 | for (int k = 0; k < chessboard_points.size(); k++) 442 | { 443 | std::vector points_i; 444 | bool valid = true; 445 | for(int l = 0; l < chessboard_points[k].size(); l++) 446 | { 447 | cv::Mat p = (cv::Mat_(4, 1) << chessboard_points[k][l].x, chessboard_points[k][l].y, chessboard_points[k][l].z, 1); 448 | p = K * p; 449 | p /= p.at(2,0); 450 | points_i.push_back(cv::Point2d(p.at(0,0), p.at(1,0))); 451 | if(p.at(0,0) < 0 || p.at(0,0) > 800 || p.at(1,0) < 0 || p.at(1,0) > 800) 452 | valid = false; 453 | } 454 | if(valid) 455 | { 456 | cv::line(show_img, points_i[0], points_i[1], cv::Scalar(255,0,0), 2); 457 | cv::line(show_img, points_i[1], points_i[2], cv::Scalar(255,0,0), 2); 458 | cv::line(show_img, points_i[2], points_i[3], cv::Scalar(255,0,0), 2); 459 | cv::line(show_img, points_i[3], points_i[0], cv::Scalar(255,0,0), 2); 460 | } 461 | } 462 | std::vector points_copy; 463 | for(int i = 0; i < axis_points.size(); i++) 464 | { 465 | cv::Mat p = (cv::Mat_(4, 1) << axis_points[i].x, axis_points[i].y, axis_points[i].z, 1); 466 | p = K * p; 467 | p /= p.at(2,0); 468 | points_copy.push_back(cv::Point2d(p.at(0,0), p.at(1,0))); 469 | } 470 | cv::arrowedLine(show_img, points_copy[0], points_copy[1], cv::Scalar(255,0,0), 2); 471 | cv::arrowedLine(show_img, points_copy[0], points_copy[2], cv::Scalar(0,255,0), 2); 472 | cv::arrowedLine(show_img, points_copy[0], points_copy[3], cv::Scalar(0,0,255), 2); 473 | for (int k = 0; k < camera_points.size(); k++) 474 | { 475 | std::vector points_i; 476 | for(int l = 0; l < camera_points[k].size(); l++) 477 | { 478 | cv::Mat p = (cv::Mat_(4, 1) << camera_points[k][l].x, camera_points[k][l].y, camera_points[k][l].z, 1); 479 | p = K * p; 480 | p /= p.at(2,0); 481 | points_i.push_back(cv::Point2d(p.at(0,0), p.at(1,0))); 482 | } 483 | cv::line(show_img, points_i[0], points_i[1], cv::Scalar(0,255,0), 2); 484 | cv::line(show_img, points_i[1], points_i[2], cv::Scalar(0,255,0), 2); 485 | cv::line(show_img, points_i[2], points_i[3], cv::Scalar(0,255,0), 2); 486 | cv::line(show_img, points_i[3], points_i[0], cv::Scalar(0,255,0), 2); 487 | cv::line(show_img, points_i[4], points_i[5], cv::Scalar(0,255,0), 2); 488 | cv::line(show_img, points_i[5], points_i[6], cv::Scalar(0,255,0), 2); 489 | cv::line(show_img, points_i[6], points_i[7], cv::Scalar(0,255,0), 2); 490 | cv::line(show_img, points_i[7], points_i[4], cv::Scalar(0,255,0), 2); 491 | cv::line(show_img, points_i[0], points_i[4], cv::Scalar(0,255,0), 2); 492 | cv::line(show_img, points_i[1], points_i[5], cv::Scalar(0,255,0), 2); 493 | cv::line(show_img, points_i[2], points_i[6], cv::Scalar(0,255,0), 2); 494 | cv::line(show_img, points_i[3], points_i[7], cv::Scalar(0,255,0), 2); 495 | } 496 | 497 | 498 | cv::imshow("show_window", show_img); 499 | int key = cv::waitKey(10); 500 | if(key == 'q' || key == 27 || cv::getWindowProperty("show_window", cv::WND_PROP_AUTOSIZE) < 0) 501 | break; 502 | } 503 | } 504 | --------------------------------------------------------------------------------