├── .gitignore ├── CMakeLists.txt ├── README.md ├── calib_converter ├── CMakeLists.txt ├── calib_converter.cpp ├── calib_converter.h └── main.cpp ├── distort ├── CMakeLists.txt └── main.cpp ├── fisheye-calib.cpp ├── imagelist_creator.cpp ├── ocam_calibration.cpp ├── ocam_calibration.h ├── pinhole-calib.cpp └── stereo-calib.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | b/ 3 | build/ 4 | *.obj 5 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | 3 | project(CameraCalibration) 4 | 5 | 6 | find_package(Eigen3 REQUIRED) 7 | find_package(OpenCV REQUIRED) 8 | find_package(Ceres REQUIRED) 9 | 10 | add_executable(ocam-fisheye-calib fisheye-calib.cpp ocam_calibration.h ocam_calibration.cpp calib_converter/calib_converter.h calib_converter/calib_converter.cpp) 11 | 12 | target_link_libraries(ocam-fisheye-calib ${OpenCV_LIBRARIES} ceres) 13 | target_include_directories(ocam-fisheye-calib PRIVATE ${OpenCV_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS}/ceres/internal/miniglog ${EIGEN3_INCLUDE_DIRS}) 14 | 15 | 16 | add_subdirectory(distort) 17 | add_subdirectory(calib_converter) 18 | 19 | add_executable(pinhole-calib pinhole-calib.cpp) 20 | target_include_directories(pinhole-calib PRIVATE ${OpenCV_INCLUDE_DIRS}) 21 | target_link_libraries(pinhole-calib PRIVATE ${OpenCV_LIBRARIES}) 22 | 23 | add_executable(stereo-calib stereo-calib.cpp) 24 | target_include_directories(stereo-calib PRIVATE ${OpenCV_INCLUDE_DIRS}) 25 | target_link_libraries(stereo-calib PRIVATE ${OpenCV_LIBRARIES}) 26 | 27 | add_executable(imagelist_creator imagelist_creator.cpp) 28 | target_link_libraries(imagelist_creator PRIVATE ${OpenCV_LIBRARIES}) 29 | 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ocam-calib-cpp 2 | Omnidirectional camera calibration implementation c++ 3 | 4 | ## Dependencies 5 | 6 | * OpenCV with contrib (omni module) version >= 3.4 7 | * ceres 8 | * Eigen3 9 | 10 | ## ocam-fisheye-calib 11 | 12 | ``` 13 | his is a camera calibration sample. 14 | Usage: calibration 15 | -w= # the number of inner corners per one of board dimension 16 | -h= # the number of inner corners per another board dimension 17 | ... 18 | [input_data] # input data, one of the following: 19 | # - text file with a list of the images of the board 20 | # the text file can be generated with imagelist_creator 21 | # - name of video file with a video of the board 22 | # if input_data not specified, a live view from the camera is used 23 | 24 | ``` 25 | 26 | ## Example params 27 | 28 | ``` 29 | ./ocam-fisheye-calib -w=8 -h=5 images_cam2.yaml 30 | ``` 31 | 32 | -------------------------------------------------------------------------------- /calib_converter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | 3 | project(calib_converter) 4 | find_package(OpenCV REQUIRED) 5 | find_package(Eigen3 REQUIRED CONFIG) 6 | add_executable(calib_converter calib_converter.cpp calib_converter.h main.cpp) 7 | target_link_libraries(calib_converter ${OpenCV_LIBRARIES} Eigen3::Eigen) 8 | -------------------------------------------------------------------------------- /calib_converter/calib_converter.cpp: -------------------------------------------------------------------------------- 1 | #define _USE_MATH_DEFINES 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include "calib_converter.h" 8 | 9 | /** 10 | * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v) 11 | * 12 | * \param P 3D point coordinates 13 | * \param p return value, contains the image point coordinates 14 | */ 15 | 16 | Eigen::Vector2d WorldToPlane(const Eigen::Vector3d& P, const std::vector& inv_poly, 17 | const Eigen::Vector2d& pp) 18 | { 19 | double norm = std::sqrt(P[0] * P[0] + P[1] * P[1]); 20 | double theta = std::atan2(-P[2], norm); 21 | double rho = 0.0; 22 | double theta_i = 1.0; 23 | 24 | for (int i = 0; i < (int)inv_poly.size(); i++) 25 | { 26 | rho += theta_i * inv_poly[i]; 27 | theta_i *= theta; 28 | } 29 | 30 | double invNorm = 1.0 / norm; 31 | Eigen::Vector2d xn( 32 | P[0] * invNorm * rho, 33 | P[1] * invNorm * rho 34 | ); 35 | 36 | return Eigen::Vector2d( 37 | xn[0] + pp.x(), 38 | xn[1] + pp.y()); 39 | } 40 | 41 | 42 | double calib_converter::convertOcam2Mei(const std::vector & poly, 43 | const std::vector & poly_inv, 44 | const Eigen::Vector2d & principal_point, 45 | const Eigen::Vector2d & img_size, 46 | Eigen::Matrix3f & K_out, 47 | std::array & D_out, 48 | float& xi_out) 49 | { 50 | int cx = 15; 51 | int cy = 10; 52 | double sx = 0.15; 53 | double sy = 0.15; 54 | std::vector obj_pts; 55 | for (int j = 0; j < cy; ++j) 56 | { 57 | for (int i = 0; i < cx; ++i) 58 | { 59 | obj_pts.push_back({ float(i * sx), float(j * sy) , 0.0f }); 60 | } 61 | } 62 | 63 | int num_table_count = 100; 64 | std::vector> object_pts; 65 | std::vector> img_pts; 66 | int num_total_pts = 0; 67 | //cv::Mat tmp_img(img_size.y(), img_size.x(), CV_8UC1); 68 | std::vector orig_ts; 69 | std::vector orig_R; 70 | std::vector orig_euler; 71 | for (int i = 0; i < num_table_count; ++i) 72 | { 73 | //tmp_img.setTo(0); 74 | 75 | std::vector imgpts; 76 | std::vector objpts; 77 | ///generate rotation translation 78 | double rx = (rand() / (double)RAND_MAX - 0.5) * 3.141592 / 4.0; 79 | double ry = (rand() / (double)RAND_MAX - 0.5) * 3.141592 / 5.0; 80 | double rz = (rand() / (double)RAND_MAX - 0.5) * 3.141592 / 4.0; 81 | 82 | double tx = (rand() / (double)RAND_MAX - 0.5) * 4; 83 | double ty = (rand() / (double)RAND_MAX - 0.5) * 4; 84 | double tz = rand() / (double)RAND_MAX * 1.0f; 85 | if (i > 3 * num_table_count / 4) 86 | tz = rand() / (double)RAND_MAX * 6.0 + 0.01; 87 | 88 | Eigen::Matrix3d m; 89 | m = Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()) * 90 | Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) * 91 | Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()); 92 | 93 | for (auto& o : obj_pts) 94 | { 95 | Eigen::Vector3d transformed = m.transpose() * Eigen::Vector3d(o.x, o.y, o.z) + 96 | Eigen::Vector3d(tx, ty, tz); 97 | if (transformed.z() > 0) 98 | { 99 | auto ip = WorldToPlane(transformed, poly_inv, principal_point); 100 | if (ip.x() > 30 && 101 | ip.y() > 30 && 102 | ip.x() < img_size.x() - 30 && 103 | ip.y() < img_size.y() - 30) 104 | { 105 | //cv::circle(tmp_img, cv::Point(ip.x(), ip.y()), 3, cv::Scalar(255), -1); 106 | imgpts.push_back(cv::Point2f(ip.x(), ip.y())); 107 | objpts.push_back(o); 108 | } 109 | } 110 | } 111 | if (imgpts.size() > 25) 112 | { 113 | num_total_pts += (int)imgpts.size(); 114 | img_pts.push_back(std::move(imgpts)); 115 | object_pts.push_back(std::move(objpts)); 116 | //std::cout << tz << std::endl; 117 | //cv::imshow("tmp_img", tmp_img); 118 | //cv::waitKey(0); 119 | orig_ts.push_back(Eigen::Vector3d(tx, ty, tz)); 120 | orig_R.push_back(m); 121 | orig_euler.push_back(Eigen::Vector3d(rx, ry, rz)); 122 | } 123 | } 124 | 125 | ///calculate table 126 | 127 | 128 | 129 | cv::Mat K, xi, D; 130 | xi.setTo(0); 131 | std::vector indices; 132 | std::vector rvecs, tvecs; 133 | //int flags = cv::omnidir::CALIB_FIX_SKEW | cv::omnidir::CALIB_FIX_XI; 134 | int flags = cv::omnidir::CALIB_FIX_SKEW; 135 | cv::TermCriteria critia(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 2000, 0.00001); 136 | double rms; 137 | rms = cv::omnidir::calibrate(object_pts, img_pts, 138 | cv::Size(img_size.x(), img_size.y()), 139 | K, xi, D, rvecs, tvecs, flags, critia, indices); 140 | 141 | Eigen::Matrix3d K_eig; 142 | cv::cv2eigen(K, K_eig); 143 | K_out = K_eig.cast(); 144 | D_out[0] = (float)D.at(0); 145 | D_out[1] = (float)D.at(1); 146 | D_out[2] = (float)D.at(2); 147 | D_out[3] = (float)D.at(3); 148 | D_out[4] = 0.0; 149 | xi_out = (float)xi.at(0); 150 | 151 | double sum_error_t = 0; 152 | double sum_error_rad = 0; 153 | 154 | 155 | for (size_t i = 0; i < indices.size(); ++i) 156 | { 157 | int j = indices[i]; 158 | Eigen::Vector3d t(tvecs[i].at(0), tvecs[i].at(1), tvecs[i].at(2)); 159 | cv::Mat R; 160 | Eigen::Matrix3d R_eigen; 161 | cv::Rodrigues(rvecs[i], R); 162 | cv::cv2eigen(R, R_eigen); 163 | double tmp_err = (orig_ts[j] - t).norm(); 164 | sum_error_t += tmp_err; 165 | if (tmp_err > 1) 166 | { 167 | //cv::waitKey(0); 168 | } 169 | 170 | Eigen::Vector3d r = R_eigen.eulerAngles(0, 1, 2); 171 | Eigen::Vector3d r_orig = orig_R[j].transpose().eulerAngles(0, 1, 2); 172 | tmp_err = (r - r_orig).norm(); 173 | sum_error_rad += tmp_err; 174 | if (tmp_err > 3.141592 / 180.0f) 175 | { 176 | //cv::waitKey(0); 177 | } 178 | } 179 | 180 | std::cout << "Num tables total: " << indices.size(); 181 | std::cout << "sum trans error in meter: " << sum_error_t << ", avg_err: " << sum_error_t / indices.size(); 182 | std::cout << "sum rad error in degree: " << sum_error_rad * 180.0 / 3.141592 << ", avg_err: " << sum_error_rad * 180.0 / 3.141592 / indices.size(); 183 | 184 | std::cout << "Converted parameters: \n"; 185 | std::cout << "K:\n" << K; 186 | std::cout << "xi:\n" << xi; 187 | std::cout << "D:\n" << D; 188 | std::cout << "rms:" << rms << ", num_total_pts: " << num_total_pts; 189 | 190 | 191 | return rms; 192 | } 193 | 194 | -------------------------------------------------------------------------------- /calib_converter/calib_converter.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #define _USE_MATH_DEFINES 3 | #include 4 | #include 5 | 6 | 7 | namespace calib_converter 8 | { 9 | 10 | double convertOcam2Mei(const std::vector & poly, 11 | const std::vector & poly_inv, 12 | const Eigen::Vector2d & principal_point, 13 | const Eigen::Vector2d & img_size, 14 | Eigen::Matrix3f & K_out, 15 | std::array & D_out, 16 | float& xi_out); 17 | } 18 | 19 | -------------------------------------------------------------------------------- /calib_converter/main.cpp: -------------------------------------------------------------------------------- 1 | #include "calib_converter.h" 2 | 3 | int main(int argc, const char* argv[]) 4 | { 5 | 6 | std::vector poly = { -808.761790039798, 0, 0.000580253898344636, -3.42188736867185e-07, 3.00547601256e-10 }; 7 | std::vector poly_inv = { 1169.63512540441, 8 | 623.254085841451, 9 | -50.1297832802301, 10 | 105.460507398029, 11 | 44.1417584226226, 12 | -11.3473206372501, 13 | 29.9396348384398, 14 | 5.08122505159174, 15 | -13.0934254922489, 16 | 16.5033337578018, 17 | 7.60796823512556, 18 | -9.83544659637847, 19 | -2.10464181791684, 20 | 3.32103378094772, 21 | 1.12117493318629 22 | }; 23 | Eigen::Vector2d principal_point{ 1922.61931008, 1078.2706176 }; 24 | Eigen::Vector2d img_size(3840, 2160); 25 | Eigen::Matrix3f K_out; 26 | std::array D_out; 27 | float xi_out; 28 | 29 | calib_converter::convertOcam2Mei(poly, poly_inv, principal_point, img_size, K_out, D_out, xi_out); 30 | return 0; 31 | } -------------------------------------------------------------------------------- /distort/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | 3 | project(ocam_distort) 4 | 5 | 6 | find_package(OpenCV REQUIRED) 7 | find_package(Eigen3 REQUIRED) 8 | 9 | add_executable(ocam_distort main.cpp) 10 | 11 | target_link_libraries(ocam_distort ${OpenCV_LIBRARIES} Eigen3::Eigen) 12 | target_include_directories(ocam_distort PRIVATE ${OpenCV_INCLUDE_DIRS}) 13 | -------------------------------------------------------------------------------- /distort/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | 9 | struct OcamCalibData 10 | { 11 | const int32_t pd = 5; 12 | float poly[5]; 13 | const int32_t ipd = 15; 14 | float inv_poly[15]; 15 | float c; 16 | float d; 17 | float e; 18 | float cx; 19 | float cy; 20 | float iw; 21 | float ih; 22 | }; 23 | 24 | bool loadOcamCalibFile(const std::string& calib_f_name, OcamCalibData& calib_data); 25 | bool loadOcamCalibFileCPP(const std::string& calib_f_name, OcamCalibData& calib_data); 26 | void undistortImageOcam(const OcamCalibData& cd, 27 | const uint8_t* img, 28 | int num_ch, 29 | const Eigen::Matrix3f& new_cam_mat_inv, 30 | uint8_t* o_img, 31 | int32_t ow, 32 | int32_t oh, 33 | int32_t onum_ch) 34 | { 35 | double min_polyval = 1e100; 36 | double max_polyval = 0; 37 | for (auto j = 0; j < oh; ++j) 38 | { 39 | for (auto i = 0; i < ow; ++i) 40 | { 41 | Eigen::Vector2f p((float)i, (float)j); 42 | Eigen::Vector3f tmp = new_cam_mat_inv * p.homogeneous(); 43 | p = tmp.hnormalized(); 44 | 45 | float dist = p.norm(); 46 | 47 | float rho = atan2f(-1, dist); 48 | float tmp_rho = 1; 49 | float polyval = 0; 50 | for (auto k = 0; k < cd.ipd; ++k) 51 | { 52 | float coeff = cd.inv_poly[/*ocam_model_inv.size() - 1 - */k]; 53 | polyval += coeff * tmp_rho; 54 | tmp_rho *= rho; 55 | } 56 | if (polyval < min_polyval) 57 | min_polyval = polyval; 58 | if (polyval > max_polyval) 59 | max_polyval = polyval; 60 | float xx = p.x() / dist * polyval; 61 | float yy = p.y() / dist * polyval; 62 | 63 | xx = yy * cd.e + xx + cd.cx; 64 | yy = yy * cd.c + xx * cd.d + cd.cy; 65 | 66 | 67 | if (yy < (float)cd.ih && xx < (float)cd.iw && yy > 0 && xx > 0) 68 | { 69 | //val = img[int(yy) * int(cd.iw) * num_ch + int(xx) * num_ch]; 70 | memcpy(&o_img[j * ow * onum_ch + i * onum_ch], &img[int(yy) * int(cd.iw) * num_ch + int(xx) * num_ch], onum_ch); 71 | } 72 | else 73 | { 74 | memset(&o_img[j * ow * onum_ch + i * onum_ch], 0, onum_ch); 75 | } 76 | //o_img[j * ow *onum_ch + i * onum_ch] = val; 77 | } 78 | } 79 | min_polyval = 0; 80 | max_polyval = 0; 81 | } 82 | void undistortImageOcam(const std::vector& ocam_model_inv, 83 | float c, 84 | float d, 85 | float e, 86 | float cx, 87 | float cy, 88 | uint8_t* img, 89 | int32_t iw, 90 | int32_t ih, 91 | float new_cam_mat[9], 92 | uint8_t* o_img, 93 | int32_t ow, 94 | int32_t oh, 95 | float cam_rot_x) 96 | { 97 | 98 | for (auto j = 0; j < oh; ++j) 99 | { 100 | for (auto i = 0; i < ow; ++i) 101 | { 102 | //float x = i - (float) ow / 2.0f; 103 | //float y = j - (float) oh / 2.0f; 104 | float x = (i - new_cam_mat[2]) / new_cam_mat[0]; 105 | float y = (j - new_cam_mat[5]) / new_cam_mat[4]; 106 | float z = 1; 107 | 108 | float alfa = cam_rot_x / 180.0f * 3.141592f; 109 | 110 | float co = cosf(alfa); 111 | float si = sinf(alfa); 112 | 113 | float x2 = co * x + si * z; 114 | z = -si * x + co * z; 115 | x = x2 / z; 116 | y /= z; 117 | z /= z; 118 | 119 | float dist = sqrtf(x * x + y * y); 120 | 121 | float rho = atan2f(-z, dist); 122 | float tmp_rho = 1; 123 | float polyval = 0; 124 | for (auto k = 0; k < ocam_model_inv.size(); ++k) 125 | { 126 | float coeff = ocam_model_inv[/*ocam_model_inv.size() - 1 - */k]; 127 | polyval += coeff * tmp_rho; 128 | tmp_rho *= rho; 129 | } 130 | 131 | float xx = x / dist * polyval; 132 | float yy = y / dist * polyval; 133 | 134 | xx = yy * e + xx + cx; 135 | yy = yy * c + xx * d + cy; 136 | 137 | uint8_t val = 0; 138 | if (yy < (float)ih && xx < (float)iw && yy > 0 && xx > 0) 139 | val = img[int(yy) * iw + int(xx)]; 140 | o_img[j * ow + i] = val; 141 | } 142 | } 143 | 144 | } 145 | 146 | 147 | int main() 148 | { 149 | 150 | std::string image_name = "p:/calib_data/ff000451_fisheye/00000_img_1.png"; 151 | std::string calib_name = "d:/projects/ocam-calib-cpp/build/calib_cpp_result.txt"; 152 | OcamCalibData ocd; 153 | if (!loadOcamCalibFileCPP(calib_name, ocd)) 154 | return -1; 155 | 156 | cv::Mat img = cv::imread(image_name); 157 | 158 | cv::Mat gray1, gray2, gray3, gray4; 159 | cv::cvtColor(img, gray1, cv::COLOR_BGR2GRAY); 160 | 161 | // image_name = "d:/tmp/conti_calib/front_0/1_calib00006.png"; 162 | // img = cv::imread(image_name); 163 | // cv::cvtColor(img, gray2, cv::COLOR_BGR2GRAY); 164 | // 165 | // image_name = "d:/tmp/conti_calib/front_0/2_calib00006.png"; 166 | // img = cv::imread(image_name); 167 | // cv::cvtColor(img, gray3, cv::COLOR_BGR2GRAY); 168 | // 169 | // image_name = "d:/tmp/conti_calib/front_0/3_calib00006.png"; 170 | // img = cv::imread(image_name); 171 | // cv::cvtColor(img, gray4, cv::COLOR_BGR2GRAY); 172 | 173 | //cv::imshow("image", gray1); 174 | 175 | 176 | cv::Mat undistorted(gray1.rows, gray1.cols, CV_8UC1); 177 | 178 | float n_f = ocd.iw / (2 * tanf(50 / 180.0f * 3.141592f)); 179 | Eigen::Matrix3f new_cam_mat; 180 | new_cam_mat << 181 | n_f, 0.0f, ocd.iw / 2.0f, 182 | 0, n_f, ocd.ih / 2.0f, 183 | 0.0f, 0.0f, 1.0f; 184 | new_cam_mat = new_cam_mat * Eigen::Vector3f(0.5f, 0.5f, 1.0f).asDiagonal(); 185 | Eigen::Matrix3f cam_mat_inv = new_cam_mat.inverse(); 186 | undistortImageOcam(ocd, gray1.data, 1, 187 | cam_mat_inv, undistorted.data, gray1.cols, gray1.rows, 1); 188 | 189 | //undistortImageOcam(ocd, gray2.data, 1, 190 | // cam_mat_inv, undistorted1.data, gray2.cols, gray2.rows, 1); 191 | // 192 | //undistortImageOcam(ocd, gray3.data, 1, 193 | // cam_mat_inv, undistorted2.data, gray3.cols, gray3.rows, 1); 194 | //int from = 0; 195 | // 196 | cv::imshow("undistorted", undistorted); 197 | cv::imwrite("d:/front_178_d_ocam.png", undistorted); 198 | cv::waitKey(0); 199 | 200 | 201 | } 202 | 203 | bool loadOcamCalibFile(const std::string& calib_f_name, OcamCalibData& calib_data) 204 | { 205 | std::ifstream fs(calib_f_name); 206 | if (!fs.is_open()) 207 | return false; 208 | std::string str; 209 | int poly_read = 0; 210 | while (!fs.eof()) 211 | { 212 | getline(fs, str); 213 | if (str.size() == 0 || str[0] == '#') 214 | continue; 215 | if (poly_read == 0) 216 | { 217 | int32_t s; 218 | std::stringstream ss(str); 219 | ss >> s; 220 | for (int i = 0; i < calib_data.pd; ++i) 221 | ss >> calib_data.poly[i]; 222 | ++poly_read; 223 | } 224 | else if (poly_read == 1) 225 | { 226 | int32_t s; 227 | std::stringstream ss(str); 228 | ss >> s; 229 | for (int i = 0; i < calib_data.ipd; ++i) 230 | ss >> calib_data.inv_poly[i]; 231 | ++poly_read; 232 | } 233 | else if (poly_read == 2) 234 | { 235 | std::stringstream ss(str); 236 | ss >> calib_data.cy; 237 | ss >> calib_data.cx; 238 | ++poly_read; 239 | } 240 | else if (poly_read == 3) 241 | { 242 | std::stringstream ss(str); 243 | ss >> calib_data.c; 244 | ss >> calib_data.d; 245 | ss >> calib_data.e; 246 | ++poly_read; 247 | } 248 | else if (poly_read == 4) 249 | { 250 | std::stringstream ss(str); 251 | ss >> calib_data.ih; 252 | ss >> calib_data.iw; 253 | } 254 | } 255 | 256 | return poly_read == 4; 257 | } 258 | 259 | bool loadOcamCalibFileCPP(const std::string& calib_f_name, OcamCalibData& calib_data) 260 | { 261 | std::ifstream fs(calib_f_name); 262 | if (!fs.is_open()) 263 | return false; 264 | std::string str; 265 | int poly_read = 0; 266 | getline(fs, str); 267 | while (!fs.eof() && poly_read < 4) 268 | { 269 | getline(fs, str); 270 | str = str.substr(str.find(':') + 1); 271 | if (str.size() == 0 || str[0] == '#') 272 | continue; 273 | if (poly_read == 2) 274 | { 275 | std::stringstream ss(str); 276 | for (int i = 0; i < calib_data.pd; ++i) 277 | ss >> calib_data.poly[i]; 278 | ++poly_read; 279 | } 280 | else if (poly_read == 3) 281 | { 282 | std::stringstream ss(str); 283 | memset(calib_data.inv_poly, 0, sizeof(calib_data.inv_poly)); 284 | for (int i = 0; i < calib_data.ipd; ++i) 285 | ss >> calib_data.inv_poly[i]; 286 | ++poly_read; 287 | } 288 | else if (poly_read == 1) 289 | { 290 | std::stringstream ss(str); 291 | ss >> calib_data.cy; 292 | ss >> calib_data.cx; 293 | ++poly_read; 294 | } 295 | else if (poly_read == 0) 296 | { 297 | std::stringstream ss(str); 298 | ss >> calib_data.ih; 299 | ss >> calib_data.iw; 300 | ++poly_read; 301 | } 302 | } 303 | calib_data.c = 1.0; 304 | calib_data.d = 0.0; 305 | calib_data.e = 0.0; 306 | return poly_read == 4; 307 | } 308 | -------------------------------------------------------------------------------- /fisheye-calib.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/core.hpp" 2 | #include 3 | #include "opencv2/imgproc.hpp" 4 | #include "opencv2/calib3d.hpp" 5 | #include "opencv2/imgcodecs.hpp" 6 | #include "opencv2/videoio.hpp" 7 | #include "opencv2/highgui.hpp" 8 | 9 | #include "ocam_calibration.h" 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "calib_converter/calib_converter.h" 17 | 18 | using namespace cv; 19 | using namespace std; 20 | 21 | const char * usage = 22 | " \nexample command line for calibration from a live feed.\n" 23 | " calibration -w=4 -h=5 -s=0.025 -o=camera.yml -op -oe\n" 24 | " \n" 25 | " example command line for calibration from a list of stored images:\n" 26 | " imagelist_creator image_list.xml *.png\n" 27 | " calibration -w=4 -h=5 -s=0.025 -o=camera.yml -op -oe image_list.xml\n" 28 | " where image_list.xml is the standard OpenCV XML/YAML\n" 29 | " use imagelist_creator to create the xml or yaml list\n" 30 | " file consisting of the list of strings, e.g.:\n" 31 | " \n" 32 | "\n" 33 | "\n" 34 | "\n" 35 | "view000.png\n" 36 | "view001.png\n" 37 | "\n" 38 | "view003.png\n" 39 | "view010.png\n" 40 | "one_extra_view.jpg\n" 41 | "\n" 42 | "\n"; 43 | 44 | 45 | 46 | 47 | const char* liveCaptureHelp = 48 | "When the live video from camera is used as input, the following hot-keys may be used:\n" 49 | " , 'q' - quit the program\n" 50 | " 'g' - start capturing images\n" 51 | " 'u' - switch undistortion on/off\n"; 52 | 53 | static void help() 54 | { 55 | printf( "This is a camera calibration sample.\n" 56 | "Usage: calibration\n" 57 | " -w= # the number of inner corners per one of board dimension\n" 58 | " -h= # the number of inner corners per another board dimension\n" 59 | " [-pt=] # the type of pattern: chessboard or circles' grid\n" 60 | " [-n=] # the number of frames to use for calibration\n" 61 | " # (if not specified, it will be set to the number\n" 62 | " # of board views actually available)\n" 63 | " [-d=] # a minimum delay in ms between subsequent attempts to capture a next view\n" 64 | " # (used only for video capturing)\n" 65 | " [-s=] # square size in some user-defined units (1 by default)\n" 66 | " [-o=] # the output filename for intrinsic [and extrinsic] parameters\n" 67 | " [-op] # write detected feature points\n" 68 | " [-oe] # write extrinsic parameters\n" 69 | " [-zt] # assume zero tangential distortion\n" 70 | " [-a=] # fix aspect ratio (fx/fy)\n" 71 | " [-p] # fix the principal point at the center\n" 72 | " [-v] # flip the captured images around the horizontal axis\n" 73 | " [-V] # use a video file, and not an image list, uses\n" 74 | " # [input_data] string for the video file name\n" 75 | " [-su] # show undistorted images after calibration\n" 76 | " [input_data] # input data, one of the following:\n" 77 | " # - text file with a list of the images of the board\n" 78 | " # the text file can be generated with imagelist_creator\n" 79 | " # - name of video file with a video of the board\n" 80 | " # if input_data not specified, a live view from the camera is used\n" 81 | "\n" ); 82 | printf("\n%s",usage); 83 | printf( "\n%s", liveCaptureHelp ); 84 | } 85 | 86 | enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 }; 87 | enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID }; 88 | 89 | static double computeReprojectionErrors( 90 | const vector >& objectPoints, 91 | const vector >& imagePoints, 92 | const vector& rvecs, const vector& tvecs, 93 | const Mat& cameraMatrix, const Mat& distCoeffs, 94 | vector& perViewErrors ) 95 | { 96 | vector imagePoints2; 97 | int i, totalPoints = 0; 98 | double totalErr = 0, err; 99 | perViewErrors.resize(objectPoints.size()); 100 | 101 | for( i = 0; i < (int)objectPoints.size(); i++ ) 102 | { 103 | projectPoints(Mat(objectPoints[i]), rvecs[i], tvecs[i], 104 | cameraMatrix, distCoeffs, imagePoints2); 105 | err = norm(Mat(imagePoints[i]), Mat(imagePoints2), NORM_L2); 106 | int n = (int)objectPoints[i].size(); 107 | perViewErrors[i] = (float)std::sqrt(err*err/n); 108 | totalErr += err*err; 109 | totalPoints += n; 110 | } 111 | 112 | return std::sqrt(totalErr/totalPoints); 113 | } 114 | 115 | static void calcChessboardCorners(Size boardSize, float squareSize, vector& corners, Pattern patternType = CHESSBOARD) 116 | { 117 | corners.resize(0); 118 | 119 | switch(patternType) 120 | { 121 | case CHESSBOARD: 122 | case CIRCLES_GRID: 123 | for( int i = 0; i < boardSize.height; i++ ) 124 | for( int j = 0; j < boardSize.width; j++ ) 125 | corners.push_back(Point3f(float(j*squareSize), 126 | float(i*squareSize), 0)); 127 | break; 128 | 129 | case ASYMMETRIC_CIRCLES_GRID: 130 | for( int i = 0; i < boardSize.height; i++ ) 131 | for( int j = 0; j < boardSize.width; j++ ) 132 | corners.push_back(Point3f(float((2*j + i % 2)*squareSize), 133 | float(i*squareSize), 0)); 134 | break; 135 | 136 | default: 137 | CV_Error(Error::StsBadArg, "Unknown pattern type\n"); 138 | } 139 | } 140 | #include 141 | #include 142 | static bool runCalibration( vector > imagePoints, 143 | Size imageSize, Size boardSize, Pattern patternType, 144 | float squareSize, float aspectRatio, 145 | int flags, Mat& cameraMatrix, Mat& distCoeffs, 146 | vector& rvecs, vector& tvecs, 147 | vector& reprojErrs, 148 | double& totalAvgErr) 149 | { 150 | cameraMatrix = Mat::eye(3, 3, CV_64F); 151 | if( flags & CALIB_FIX_ASPECT_RATIO ) 152 | cameraMatrix.at(0,0) = aspectRatio; 153 | 154 | distCoeffs = Mat::zeros(8, 1, CV_64F); 155 | 156 | vector > objectPoints(1); 157 | calcChessboardCorners(boardSize, squareSize, objectPoints[0], patternType); 158 | 159 | objectPoints.resize(imagePoints.size(),objectPoints[0]); 160 | 161 | OcamCalibRes res; 162 | double rms_ocam = calibrateCameraOcam2(objectPoints, imagePoints, imageSize, res); 163 | 164 | ofstream res_file("calib_cpp_result.txt"); 165 | 166 | cout << "Ocam result: " << std::setprecision(15) << rms_ocam << endl; 167 | res_file << "Ocam result: " << std::setprecision(15)<< rms_ocam << endl; 168 | cout << "principal point: " << res.center_x << " : " << res.center_y << endl; 169 | res_file << "principal point: " << res.center_x << " : " << res.center_y << endl; 170 | cout << "cam to world poly: "; 171 | res_file << "cam to world poly: "; 172 | for (auto v : res.ss) 173 | { 174 | cout << v << " "; 175 | res_file << v << " "; 176 | } 177 | cout << endl; 178 | res_file << endl; 179 | 180 | cout << "world to cam: "; 181 | res_file << "world to cam: "; 182 | for (auto v : res.ss_inv) 183 | { 184 | cout << v << " "; 185 | res_file << v << " "; 186 | } 187 | cout << endl << "Rotation and translation: \n"; 188 | res_file << endl << "Rotation and translation: \n"; 189 | 190 | for (size_t k = 0; k < res.R.size(); ++k) 191 | { 192 | cout << "num : " << k << endl; 193 | res_file << "num : " << k << endl; 194 | for (int i = 0; i < 3; ++i) 195 | { 196 | for (int j = 0; j < 3; ++j) 197 | { 198 | cout << res.R[k][i * 3 + j] << " "; 199 | res_file << res.R[k][i * 3 + j] << " "; 200 | } 201 | cout << res.T[k][i] << "\n"; 202 | res_file << res.T[k][i] << "\n"; 203 | } 204 | } 205 | 206 | Eigen::Matrix3f K; 207 | std::array D_out; 208 | float xi; 209 | totalAvgErr = calib_converter::convertOcam2Mei(res.ss, res.ss_inv, 210 | Eigen::Vector2d(res.center_y, res.center_x), 211 | Eigen::Vector2d(imageSize.width, imageSize.height), 212 | K, 213 | D_out, 214 | xi); 215 | 216 | ofstream res_file_mei("mei_model_result.txt"); 217 | 218 | std::cout<< "Camera matrix: \n" << K << std::endl; 219 | res_file_mei << "Camera matrix: \n" << K << std::endl; 220 | std::cout << "xi: " << xi << std::endl; 221 | res_file_mei << "xi: " << xi << std::endl; 222 | std::cout << "distortion coeffs: " << D_out[0] << " " << D_out[1] << " " << D_out[2] << " " << D_out[3] << " " << D_out[4] << " " << std::endl; 223 | res_file_mei << "distortion coeffs: " << D_out[0] << " " << D_out[1] << " " << D_out[2] << " " << D_out[3] << " " << D_out[4] << " " << std::endl; 224 | 225 | //double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, 226 | // distCoeffs, rvecs, tvecs, 227 | // //flags|CALIB_FIX_K4|CALIB_FIX_K5 228 | // flags 229 | //); 230 | 231 | 232 | 233 | // ///*|CALIB_FIX_K3*/|CALIB_FIX_K4|CALIB_FIX_K5); 234 | //printf("RMS error reported by calibrateCamera: %g\n", rms); 235 | 236 | //bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs); 237 | 238 | //totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, 239 | // rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs); 240 | 241 | return true; 242 | } 243 | 244 | 245 | static void saveCameraParams( const string& filename, 246 | Size imageSize, Size boardSize, 247 | float squareSize, float aspectRatio, int flags, 248 | const Mat& cameraMatrix, const Mat& distCoeffs, 249 | const vector& rvecs, const vector& tvecs, 250 | const vector& reprojErrs, 251 | const vector >& imagePoints, 252 | double totalAvgErr ) 253 | { 254 | FileStorage fs( filename, FileStorage::WRITE ); 255 | 256 | time_t tt; 257 | time( &tt ); 258 | struct tm *t2 = localtime( &tt ); 259 | char buf[1024]; 260 | strftime( buf, sizeof(buf)-1, "%c", t2 ); 261 | 262 | fs << "calibration_time" << buf; 263 | 264 | if( !rvecs.empty() || !reprojErrs.empty() ) 265 | fs << "nframes" << (int)std::max(rvecs.size(), reprojErrs.size()); 266 | fs << "image_width" << imageSize.width; 267 | fs << "image_height" << imageSize.height; 268 | fs << "board_width" << boardSize.width; 269 | fs << "board_height" << boardSize.height; 270 | fs << "square_size" << squareSize; 271 | 272 | if( flags & CALIB_FIX_ASPECT_RATIO ) 273 | fs << "aspectRatio" << aspectRatio; 274 | 275 | if( flags != 0 ) 276 | { 277 | sprintf( buf, "flags: %s%s%s%s", 278 | flags & CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "", 279 | flags & CALIB_FIX_ASPECT_RATIO ? "+fix_aspectRatio" : "", 280 | flags & CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "", 281 | flags & CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : "" ); 282 | //cvWriteComment( *fs, buf, 0 ); 283 | } 284 | 285 | fs << "flags" << flags; 286 | 287 | fs << "camera_matrix" << cameraMatrix; 288 | fs << "distortion_coefficients" << distCoeffs; 289 | 290 | fs << "avg_reprojection_error" << totalAvgErr; 291 | if( !reprojErrs.empty() ) 292 | fs << "per_view_reprojection_errors" << Mat(reprojErrs); 293 | 294 | if( !rvecs.empty() && !tvecs.empty() ) 295 | { 296 | CV_Assert(rvecs[0].type() == tvecs[0].type()); 297 | Mat bigmat((int)rvecs.size(), 6, rvecs[0].type()); 298 | for( int i = 0; i < (int)rvecs.size(); i++ ) 299 | { 300 | Mat r = bigmat(Range(i, i+1), Range(0,3)); 301 | Mat t = bigmat(Range(i, i+1), Range(3,6)); 302 | 303 | CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1); 304 | CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1); 305 | //*.t() is MatExpr (not Mat) so we can use assignment operator 306 | r = rvecs[i].t(); 307 | t = tvecs[i].t(); 308 | } 309 | //cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 ); 310 | fs << "extrinsic_parameters" << bigmat; 311 | } 312 | 313 | if( !imagePoints.empty() ) 314 | { 315 | Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2); 316 | for( int i = 0; i < (int)imagePoints.size(); i++ ) 317 | { 318 | Mat r = imagePtMat.row(i).reshape(2, imagePtMat.cols); 319 | Mat imgpti(imagePoints[i]); 320 | imgpti.copyTo(r); 321 | } 322 | fs << "image_points" << imagePtMat; 323 | } 324 | } 325 | 326 | static bool readStringList( const string& filename, vector& l ) 327 | { 328 | l.resize(0); 329 | FileStorage fs(filename, FileStorage::READ); 330 | if( !fs.isOpened() ) 331 | return false; 332 | FileNode n = fs.getFirstTopLevelNode(); 333 | if( n.type() != FileNode::SEQ ) 334 | return false; 335 | FileNodeIterator it = n.begin(), it_end = n.end(); 336 | for( ; it != it_end; ++it ) 337 | l.push_back((string)*it); 338 | return true; 339 | } 340 | 341 | 342 | static bool runAndSave(const string& outputFilename, 343 | const vector >& imagePoints, 344 | Size imageSize, Size boardSize, Pattern patternType, float squareSize, 345 | float aspectRatio, int flags, Mat& cameraMatrix, 346 | Mat& distCoeffs, bool writeExtrinsics, bool writePoints ) 347 | { 348 | vector rvecs, tvecs; 349 | vector reprojErrs; 350 | double totalAvgErr = 0; 351 | 352 | bool ok = runCalibration(imagePoints, imageSize, boardSize, patternType, squareSize, 353 | aspectRatio, flags, cameraMatrix, distCoeffs, 354 | rvecs, tvecs, reprojErrs, totalAvgErr); 355 | printf("%s. avg reprojection error = %.2f\n", 356 | ok ? "Calibration succeeded" : "Calibration failed", 357 | totalAvgErr); 358 | 359 | if( ok ) 360 | saveCameraParams( outputFilename, imageSize, 361 | boardSize, squareSize, aspectRatio, 362 | flags, cameraMatrix, distCoeffs, 363 | writeExtrinsics ? rvecs : vector(), 364 | writeExtrinsics ? tvecs : vector(), 365 | writeExtrinsics ? reprojErrs : vector(), 366 | writePoints ? imagePoints : vector >(), 367 | totalAvgErr ); 368 | return ok; 369 | } 370 | 371 | 372 | int main( int argc, char** argv ) 373 | { 374 | Size boardSize, imageSize; 375 | float squareSize, aspectRatio; 376 | Mat cameraMatrix, distCoeffs; 377 | string outputFilename; 378 | string inputFilename = ""; 379 | 380 | int i, nframes; 381 | bool writeExtrinsics, writePoints; 382 | bool undistortImage = false; 383 | int flags = 0; 384 | VideoCapture capture; 385 | bool flipVertical; 386 | bool showUndistorted; 387 | bool videofile; 388 | int delay; 389 | clock_t prevTimestamp = 0; 390 | int mode = DETECTION; 391 | int cameraId = 0; 392 | vector > imagePoints; 393 | vector imageList; 394 | Pattern pattern = CHESSBOARD; 395 | 396 | cv::CommandLineParser parser(argc, argv, 397 | "{help ||}{w||}{h||}{pt|chessboard|}{n|20|}{d|1000|}{s|1|}{o|out_camera_data.yml|}" 398 | "{op||}{oe||}{zt||}{a|1|}{p||}{v||}{V||}{su||}" 399 | "{@input_data|0|}"); 400 | if (parser.has("help")) 401 | { 402 | help(); 403 | return 0; 404 | } 405 | boardSize.width = parser.get( "w" ); 406 | boardSize.height = parser.get( "h" ); 407 | if ( parser.has("pt") ) 408 | { 409 | string val = parser.get("pt"); 410 | if( val == "circles" ) 411 | pattern = CIRCLES_GRID; 412 | else if( val == "acircles" ) 413 | pattern = ASYMMETRIC_CIRCLES_GRID; 414 | else if( val == "chessboard" ) 415 | pattern = CHESSBOARD; 416 | else 417 | return fprintf( stderr, "Invalid pattern type: must be chessboard or circles\n" ), -1; 418 | } 419 | squareSize = parser.get("s"); 420 | nframes = parser.get("n"); 421 | aspectRatio = parser.get("a"); 422 | delay = parser.get("d"); 423 | writePoints = parser.has("op"); 424 | writeExtrinsics = parser.has("oe"); 425 | if (parser.has("a")) 426 | flags |= CALIB_FIX_ASPECT_RATIO; 427 | if ( parser.has("zt") ) 428 | flags |= CALIB_ZERO_TANGENT_DIST; 429 | if ( parser.has("p") ) 430 | flags |= CALIB_FIX_PRINCIPAL_POINT; 431 | flipVertical = parser.has("v"); 432 | videofile = parser.has("V"); 433 | if ( parser.has("o") ) 434 | outputFilename = parser.get("o"); 435 | showUndistorted = parser.has("su"); 436 | if ( isdigit(parser.get("@input_data")[0]) ) 437 | cameraId = parser.get("@input_data"); 438 | else 439 | inputFilename = parser.get("@input_data"); 440 | if (!parser.check()) 441 | { 442 | help(); 443 | parser.printErrors(); 444 | return -1; 445 | } 446 | if ( squareSize <= 0 ) 447 | return fprintf( stderr, "Invalid board square width\n" ), -1; 448 | if ( nframes <= 3 ) 449 | return printf("Invalid number of images\n" ), -1; 450 | if ( aspectRatio <= 0 ) 451 | return printf( "Invalid aspect ratio\n" ), -1; 452 | if ( delay <= 0 ) 453 | return printf( "Invalid delay\n" ), -1; 454 | if ( boardSize.width <= 0 ) 455 | return fprintf( stderr, "Invalid board width\n" ), -1; 456 | if ( boardSize.height <= 0 ) 457 | return fprintf( stderr, "Invalid board height\n" ), -1; 458 | 459 | if( !inputFilename.empty() ) 460 | { 461 | if( !videofile && readStringList(inputFilename, imageList) ) 462 | mode = CAPTURING; 463 | else 464 | capture.open(inputFilename); 465 | } 466 | else 467 | capture.open(cameraId); 468 | 469 | if( !capture.isOpened() && imageList.empty() ) 470 | return fprintf( stderr, "Could not initialize video (%d) capture\n",cameraId ), -2; 471 | 472 | if( !imageList.empty() ) 473 | nframes = (int)imageList.size(); 474 | 475 | if( capture.isOpened() ) 476 | printf( "%s", liveCaptureHelp ); 477 | 478 | namedWindow( "Image View", 1 ); 479 | 480 | for(i = 0;;i++) 481 | { 482 | Mat view, viewGray; 483 | bool blink = false; 484 | 485 | if( capture.isOpened() ) 486 | { 487 | Mat view0; 488 | capture >> view0; 489 | view0.copyTo(view); 490 | } 491 | else if( i < (int)imageList.size() ) 492 | view = imread(imageList[i], 1); 493 | 494 | if(view.empty()) 495 | { 496 | if( imagePoints.size() > 0 ) 497 | runAndSave(outputFilename, imagePoints, imageSize, 498 | boardSize, pattern, squareSize, aspectRatio, 499 | flags, cameraMatrix, distCoeffs, 500 | writeExtrinsics, writePoints); 501 | break; 502 | } 503 | 504 | imageSize = view.size(); 505 | 506 | if( flipVertical ) 507 | flip( view, view, 0 ); 508 | 509 | vector pointbuf; 510 | cvtColor(view, viewGray, COLOR_BGR2GRAY); 511 | 512 | bool found; 513 | switch( pattern ) 514 | { 515 | case CHESSBOARD: 516 | found = findChessboardCorners( view, boardSize, pointbuf, 517 | CALIB_CB_ADAPTIVE_THRESH); 518 | break; 519 | case CIRCLES_GRID: 520 | found = findCirclesGrid( view, boardSize, pointbuf ); 521 | break; 522 | case ASYMMETRIC_CIRCLES_GRID: 523 | found = findCirclesGrid( view, boardSize, pointbuf, CALIB_CB_ASYMMETRIC_GRID ); 524 | break; 525 | default: 526 | return fprintf( stderr, "Unknown pattern type\n" ), -1; 527 | } 528 | 529 | // improve the found corners' coordinate accuracy 530 | if( pattern == CHESSBOARD && found) cornerSubPix( viewGray, pointbuf, Size(3,3), 531 | Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 )); 532 | 533 | if( mode == CAPTURING && found && 534 | (!capture.isOpened() || clock() - prevTimestamp > delay*1e-3*CLOCKS_PER_SEC) ) 535 | { 536 | imagePoints.push_back(pointbuf); 537 | prevTimestamp = clock(); 538 | blink = capture.isOpened(); 539 | } 540 | 541 | if (found) 542 | { 543 | drawChessboardCorners(view, boardSize, Mat(pointbuf), found); 544 | imwrite("d:/img_" + to_string(i) + ".png", view); 545 | } 546 | 547 | string msg = mode == CAPTURING ? "100/100" : 548 | mode == CALIBRATED ? "Calibrated" : "Press 'g' to start"; 549 | int baseLine = 0; 550 | Size textSize = getTextSize(msg, 1, 1, 1, &baseLine); 551 | Point textOrigin(view.cols - 2*textSize.width - 10, view.rows - 2*baseLine - 10); 552 | 553 | if( mode == CAPTURING ) 554 | { 555 | if(undistortImage) 556 | msg = format( "%d/%d Undist", (int)imagePoints.size(), nframes ); 557 | else 558 | msg = format( "%d/%d", (int)imagePoints.size(), nframes ); 559 | } 560 | 561 | putText( view, msg, textOrigin, 1, 1, 562 | mode != CALIBRATED ? Scalar(0,0,255) : Scalar(0,255,0)); 563 | 564 | if( blink ) 565 | bitwise_not(view, view); 566 | 567 | if( mode == CALIBRATED && undistortImage ) 568 | { 569 | Mat temp = view.clone(); 570 | undistort(temp, view, cameraMatrix, distCoeffs); 571 | } 572 | 573 | cv::Mat view_to_show; 574 | if (view.cols > 1600) 575 | { 576 | float scale = 1600.0f / view.cols; 577 | cv::resize(view, view_to_show, cv::Size(), scale, scale); 578 | } 579 | else 580 | { 581 | view_to_show = view; 582 | } 583 | 584 | imshow("Image View", view_to_show); 585 | int key = 0xff & waitKey(capture.isOpened() ? 50 : 500); 586 | 587 | if( (key & 255) == 27 ) 588 | break; 589 | 590 | if( key == 'u' && mode == CALIBRATED ) 591 | undistortImage = !undistortImage; 592 | 593 | if( capture.isOpened() && key == 'g' ) 594 | { 595 | mode = CAPTURING; 596 | imagePoints.clear(); 597 | } 598 | 599 | if( mode == CAPTURING && imagePoints.size() >= (unsigned)nframes ) 600 | { 601 | if( runAndSave(outputFilename, imagePoints, imageSize, 602 | boardSize, pattern, squareSize, aspectRatio, 603 | flags, cameraMatrix, distCoeffs, 604 | writeExtrinsics, writePoints)) 605 | mode = CALIBRATED; 606 | else 607 | mode = DETECTION; 608 | if( !capture.isOpened() ) 609 | break; 610 | } 611 | } 612 | 613 | if( !capture.isOpened() && showUndistorted ) 614 | { 615 | Mat view, rview, map1, map2; 616 | initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(), 617 | getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), 618 | imageSize, CV_16SC2, map1, map2); 619 | 620 | for( i = 0; i < (int)imageList.size(); i++ ) 621 | { 622 | view = imread(imageList[i], 1); 623 | if(view.empty()) 624 | continue; 625 | //undistort( view, rview, cameraMatrix, distCoeffs, cameraMatrix ); 626 | remap(view, rview, map1, map2, INTER_LINEAR); 627 | imshow("Image View", rview); 628 | int c = 0xff & waitKey(); 629 | if( (c & 255) == 27 || c == 'q' || c == 'Q' ) 630 | break; 631 | } 632 | } 633 | 634 | return 0; 635 | } 636 | -------------------------------------------------------------------------------- /imagelist_creator.cpp: -------------------------------------------------------------------------------- 1 | /*this creates a yaml or xml list of files from the command line args 2 | */ 3 | 4 | #include "opencv2/core.hpp" 5 | #include "opencv2/imgcodecs.hpp" 6 | #include "opencv2/highgui.hpp" 7 | #include 8 | #include 9 | 10 | using std::string; 11 | using std::cout; 12 | using std::endl; 13 | 14 | using namespace cv; 15 | 16 | static void help(char** av) 17 | { 18 | cout << "\nThis creates a yaml or xml list of files from the command line args\n" 19 | "usage:\n./" << av[0] << " imagelist.yaml *.png\n" 20 | << "Try using different extensions.(e.g. yaml yml xml xml.gz etc...)\n" 21 | << "This will serialize this list of images or whatever with opencv's FileStorage framework" << endl; 22 | } 23 | 24 | int main(int ac, char** av) 25 | { 26 | cv::CommandLineParser parser(ac, av, "{help h||}{@output||}"); 27 | if (parser.has("help")) 28 | { 29 | help(av); 30 | return 0; 31 | } 32 | string outputname = parser.get("@output"); 33 | 34 | if (outputname.empty()) 35 | { 36 | help(av); 37 | return 1; 38 | } 39 | 40 | Mat m = imread(outputname); //check if the output is an image - prevent overwrites! 41 | if(!m.empty()){ 42 | std::cerr << "fail! Please specify an output file, don't want to overwrite you images!" << endl; 43 | help(av); 44 | return 1; 45 | } 46 | 47 | FileStorage fs(outputname, FileStorage::WRITE); 48 | fs << "images" << "["; 49 | for(int i = 2; i < ac; i++){ 50 | fs << string(av[i]); 51 | } 52 | fs << "]"; 53 | return 0; 54 | } -------------------------------------------------------------------------------- /ocam_calibration.cpp: -------------------------------------------------------------------------------- 1 | #include "ocam_calibration.h" 2 | #include 3 | #include 4 | #include 5 | #define _USE_MATH_DEFINES 6 | #include 7 | #include 8 | #include 9 | #define MAX_LOG_LEVEL -100 10 | #include 11 | //#include 12 | using namespace std; 13 | using namespace Eigen; 14 | 15 | ceres::Solver s; 16 | ceres::Problem pr; 17 | 18 | struct CalibData 19 | { 20 | vector ss; 21 | vector ss_inv; 22 | vector RRfin; 23 | double xc; 24 | double yc; 25 | MatrixXd Xt; 26 | MatrixXd Yt; 27 | vector > img_points; 28 | cv::Size img_size; 29 | int taylor_order_default; 30 | }; 31 | 32 | 33 | vector polyRootD2(double a, double b, double c) 34 | { 35 | vector ret; 36 | //Vector2d ret(std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()); 37 | double d = b * b - 4 * a * c; 38 | if (d > numeric_limits::epsilon()) 39 | { 40 | d = sqrt(d); 41 | double r = (-b + d) / (2 * a); 42 | //if (r > 0) 43 | ret.push_back(r); 44 | 45 | r = (-b - d) / (2 * a); 46 | //if (r > 0) 47 | ret.push_back(r); 48 | } 49 | else if (abs(d) < numeric_limits::epsilon()) 50 | { 51 | double r = -b / (2 * a); 52 | //if (r > 0) 53 | ret.push_back(r); 54 | } 55 | else 56 | { 57 | assert(false); 58 | } 59 | return ret; 60 | } 61 | 62 | 63 | 64 | bool vectorEqualsZero(const vector & v) 65 | { 66 | bool ret = true; 67 | 68 | for (auto e : v) 69 | { 70 | if (e != 0) 71 | return false; 72 | } 73 | 74 | return ret; 75 | } 76 | 77 | template int sgn(T val) { 78 | return (T(0) < val) - (val < T(0)); 79 | } 80 | 81 | 82 | 83 | int plot_RR(const vector & RR, 84 | const Eigen::MatrixXd & Xt, 85 | const Eigen::MatrixXd & Yt, 86 | const Eigen::MatrixXd & Xpt, 87 | const Eigen::MatrixXd & Ypt) 88 | { 89 | int index = -1; 90 | 91 | for (size_t i = 0; i < RR.size(); ++i) 92 | { 93 | Matrix3d RRdef = RR[i]; 94 | double R11 = RRdef(0, 0); 95 | double R21 = RRdef(1, 0); 96 | double R31 = RRdef(2, 0); 97 | double R12 = RRdef(0, 1); 98 | double R22 = RRdef(1, 1); 99 | double R32 = RRdef(2, 1); 100 | double T1 = RRdef(0, 2); 101 | double T2 = RRdef(1, 2); 102 | 103 | Eigen::MatrixXd MA = (R21*Xt + R22*Yt).array() + T2; 104 | Eigen::MatrixXd MB = Ypt.cwiseProduct(R31 * Xt + R32 *Yt); 105 | Eigen::MatrixXd MC = (R11*Xt + R12*Yt).array() + T1; 106 | Eigen::MatrixXd MD = Xpt.cwiseProduct(R31*Xt + R32*Yt); 107 | Eigen::MatrixXd rho = (Xpt.cwiseProduct(Xpt) + Ypt.cwiseProduct(Ypt)).cwiseSqrt(); 108 | Eigen::MatrixXd rho2 = rho.cwiseProduct(rho); 109 | //cout << MA << endl << endl; 110 | //cout << MB << endl << endl; 111 | //cout << MC << endl << endl; 112 | //cout << MD << endl << endl; 113 | //cout << rho << endl << endl; 114 | //cout << rho2 << endl << endl; 115 | 116 | Eigen::MatrixXd PP1(2 * Xt.rows(), 3); 117 | PP1.block(0, 0, Xt.rows(), 1) = MA; 118 | PP1.block(Xt.rows(), 0, Xt.rows(), 1) = MC; 119 | PP1.block(0, 1, Xt.rows(), 1) = MA.cwiseProduct(rho); 120 | PP1.block(Xt.rows(), 1, Xt.rows(), 1) = MC.cwiseProduct(rho); 121 | PP1.block(0, 2, Xt.rows(), 1) = MA.cwiseProduct(rho2); 122 | PP1.block(Xt.rows(), 2, Xt.rows(), 1) = MC.cwiseProduct(rho2); 123 | 124 | //PP1 = [MA, MA.*rho, MA.*rho2; 125 | //MC, MC.*rho, MC.*rho2]; 126 | //cout << PP1 << endl << endl; 127 | 128 | Eigen::MatrixXd PP(2 * Xt.rows(), 4); 129 | PP.block(0, 0, PP1.rows(), 3) = PP1; 130 | PP.block(0, 3, Ypt.rows(), 1) = -Ypt; 131 | PP.block(Ypt.rows(), 3, Ypt.rows(), 1) = -Xpt; 132 | //cout << PP << endl << endl; 133 | // 134 | ////PP = [PP1, [-Ypt; -Xpt]]; 135 | // 136 | Eigen::MatrixXd QQ (MB.rows() * 2, 1); 137 | QQ.block(0, 0, MB.rows(), 1) = MB; 138 | QQ.block(MB.rows(), 0, MB.rows(), 1) = MD; 139 | //cout << QQ << endl << endl; 140 | MatrixXd s = PP.jacobiSvd(ComputeThinU | ComputeThinV).solve(QQ); 141 | //cout << s << endl << endl; 142 | ////s = pinv(PP)*QQ; 143 | ////ss = s(1:3); 144 | ////if figure_number > 0 145 | //// subplot(1, size(RR, 3), i); plot(0:620, polyval([ss(3) ss(2) ss(1)], [0:620])); grid; axis equal; 146 | ////end 147 | if (s(2) >= 0) 148 | index = (int)i; 149 | 150 | } 151 | return index; 152 | } 153 | 154 | std::vector omni_find_parameters_fun(CalibData & cd) 155 | { 156 | //fel van cserélve az x és az y 157 | double xc = cd.xc; 158 | double yc = cd.yc; 159 | 160 | 161 | int count = -1; 162 | 163 | Eigen::MatrixXd PP = Eigen::MatrixXd::Zero(2 * cd.Xt.rows() * cd.img_points.size(), cd.taylor_order_default + cd.img_points.size()); 164 | Eigen::MatrixXd QQ = Eigen::MatrixXd::Zero(2 * cd.Xt.rows() * cd.img_points.size(), 1); 165 | 166 | for (size_t i = 0; i < cd.img_points.size(); ++i) 167 | { 168 | Eigen::MatrixXd Ypt(cd.img_points[i].size(), 1); 169 | Eigen::MatrixXd Xpt(cd.img_points[i].size(), 1); 170 | for (int j = 0; j < cd.img_points[i].size(); ++j) 171 | { 172 | Ypt(j, 0) = cd.img_points[i][j].x - yc; 173 | Xpt(j, 0) = cd.img_points[i][j].y - xc; 174 | } 175 | count = count + 1; 176 | 177 | Matrix3d RRdef = cd.RRfin[i]; 178 | 179 | double R11 = RRdef(0, 0); 180 | double R21 = RRdef(1, 0); 181 | double R31 = RRdef(2, 0); 182 | double R12 = RRdef(0, 1); 183 | double R22 = RRdef(1, 1); 184 | double R32 = RRdef(2, 1); 185 | double T1 = RRdef(0, 2); 186 | double T2 = RRdef(1, 2); 187 | 188 | 189 | Eigen::MatrixXd MA = (R21*cd.Xt + R22*cd.Yt).array() + T2; 190 | Eigen::MatrixXd MB = Ypt.cwiseProduct(R31 * cd.Xt + R32 *cd.Yt); 191 | Eigen::MatrixXd MC = (R11*cd.Xt + R12*cd.Yt).array() + T1; 192 | Eigen::MatrixXd MD = Xpt.cwiseProduct(R31*cd.Xt + R32*cd.Yt); 193 | vector rho(cd.taylor_order_default); 194 | 195 | //rho = []; 196 | //for j = 2:taylor_order 197 | // rho(:, : , j) = (sqrt(Xpt. ^ 2 + Ypt. ^ 2)).^j; 198 | //end 199 | 200 | MatrixXd tmp = (Xpt.cwiseProduct(Xpt) + Ypt.cwiseProduct(Ypt)).cwiseSqrt(); 201 | rho[0] = tmp; 202 | for (size_t j = 1; j < cd.taylor_order_default; ++j) 203 | { 204 | rho[j] = tmp.cwiseProduct(rho[j -1]); 205 | } 206 | rho[0] = MatrixXd::Zero(tmp.rows(), tmp.cols()); 207 | 208 | 209 | MatrixXd PP1(cd.Xt.rows() * 2, cd.taylor_order_default); 210 | PP1.block(0,0, cd.Xt.rows(), 1)= MA; 211 | PP1.block(cd.Xt.rows(), 0, cd.Xt.rows(), 1) = MC; 212 | for (int j = 1; j < cd.taylor_order_default; ++j) 213 | { 214 | PP1.block(0, j, cd.Xt.rows(), 1) = MA.cwiseProduct(rho[j]); 215 | PP1.block(cd.Xt.rows(), j, cd.Xt.rows(), 1) = MC.cwiseProduct(rho[j]); 216 | } 217 | 218 | 219 | PP.block(PP1.rows() * i, 0, PP1.rows(), PP1.cols()) = PP1; 220 | PP.block(PP1.rows() * i, cd.taylor_order_default + i, Ypt.rows(), 1) = -Ypt; 221 | PP.block(PP1.rows() * i + Ypt.rows(), cd.taylor_order_default + i, Ypt.rows(), 1) = -Xpt; 222 | //cout << PP; 223 | //PP = [PP zeros(size(PP, 1), 1); 224 | //PP1, zeros(size(PP1, 1), count - 1)[-Ypt; -Xpt]]; 225 | 226 | QQ.block(PP1.rows() * i, 0, MB.rows(), 1) = MB; 227 | QQ.block(PP1.rows() * i + MB.rows(), 0, MB.rows(), 1) = MD; 228 | //cout << QQ; 229 | } 230 | //cout << PP << endl << endl; 231 | //cout << QQ << endl << endl; 232 | 233 | MatrixXd s = PP.jacobiSvd(ComputeThinU | ComputeThinV).solve(QQ); 234 | //cout << s << endl << endl; 235 | 236 | vector ret(cd.taylor_order_default + 1); 237 | ret[0] = s(0); 238 | ret[1] = 0; 239 | for (int i = 2; i < cd.taylor_order_default + 1; ++i) 240 | ret[i] = s(i - 1); 241 | 242 | for (size_t i = 0; i < cd.img_points.size(); ++i) 243 | cd.RRfin[i](2, 2) = s(cd.taylor_order_default + i); 244 | cd.ss = ret; 245 | return ret; 246 | } 247 | 248 | template 249 | T polyval(const vector & ss, 250 | T x) 251 | { 252 | T ret = T(0); 253 | T m = T(1); 254 | for (auto & c : ss) 255 | { 256 | ret += c * m; 257 | m *= x; 258 | } 259 | return ret; 260 | } 261 | 262 | double polyval2(const vector & ss, 263 | double theta, double radius) 264 | { 265 | double m = tan(theta); 266 | std::vector poly_coeff_tmp = ss; 267 | poly_coeff_tmp[1] = poly_coeff_tmp[1] - m; 268 | vector p_deriv_tmp(poly_coeff_tmp.size() - 1); 269 | for (size_t i = 0; i < p_deriv_tmp.size(); ++i) 270 | { 271 | p_deriv_tmp[i] = (i + 1) * poly_coeff_tmp[i + 1]; 272 | } 273 | int max_iter_count = 100; 274 | double x0 = radius; 275 | double tolerance = 10e-14; 276 | double epsilon = 10e-14; 277 | bool haveWeFoundSolution = false; 278 | for (int i = 0; i < max_iter_count; ++i) 279 | { 280 | double y = polyval(poly_coeff_tmp, x0); 281 | double yprime = polyval(p_deriv_tmp, x0); 282 | 283 | if (abs(yprime) < epsilon) // Don't want to divide by too small of a number 284 | // denominator is too small 285 | break; //Leave the loop 286 | //end 287 | double x1 = x0 - y / yprime; 288 | 289 | if (abs(x1 - x0) <= tolerance * abs(x1)) 290 | { 291 | haveWeFoundSolution = true; 292 | break; 293 | } 294 | x0 = x1; 295 | } 296 | if (haveWeFoundSolution) 297 | { 298 | return x0; 299 | } 300 | else 301 | return numeric_limits::quiet_NaN(); 302 | } 303 | 304 | vector invertOcamPoly(const std::vector & p, double w, double h) 305 | { 306 | double max_err = std::numeric_limits::infinity(); 307 | vector ret; 308 | int poly_degree = 5; 309 | while (max_err > 1e-2) 310 | { 311 | 312 | vector theta; 313 | vector r; 314 | 315 | double step = 0.01; 316 | double radius = sqrt(w * w / 4 + h * h / 4); 317 | for (double x = -M_PI_2; x < 1.2; x += step) 318 | { 319 | double y = polyval2(p, x, radius); 320 | if (y != numeric_limits::quiet_NaN() && y > 0 && 321 | y < radius) 322 | { 323 | theta.push_back(x); 324 | r.push_back(y); 325 | } 326 | } 327 | 328 | 329 | 330 | MatrixXd A(theta.size(), poly_degree + 1); 331 | MatrixXd b(theta.size(), 1); 332 | 333 | for (size_t i = 0; i < theta.size(); ++i) 334 | { 335 | A(i, 0) = 1; 336 | b(i) = r[i]; 337 | for (size_t j = 1; j < poly_degree + 1; ++j) 338 | { 339 | A(i, j) = A(i, j - 1) * theta[i]; 340 | } 341 | } 342 | //cout << A << endl << endl; 343 | MatrixXd x = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b); 344 | 345 | ret.resize(poly_degree + 1); 346 | for (int i = 0; i < poly_degree + 1; ++i) 347 | ret[i] = x(i); 348 | 349 | double tmp_max_err = 0; 350 | for (size_t i = 0; i < r.size(); ++i) 351 | { 352 | double err = abs(r[i] - polyval(ret, theta[i])); 353 | if (err > tmp_max_err) 354 | tmp_max_err = err; 355 | } 356 | max_err = tmp_max_err; 357 | ++poly_degree; 358 | } 359 | return ret; 360 | } 361 | 362 | template 363 | Matrix reprojectPoint(//const std::vector & ss_inv, 364 | const std::vector & ss, 365 | const Matrix & xx, cv::Size img_size) 366 | { 367 | T d_s = ceres::sqrt(xx(0) * xx(0) + xx(1) * xx(1)); 368 | T m = ceres::atan2( xx(2) , d_s); 369 | //T rho = polyval(ss_inv, m); 370 | double radius = sqrt(img_size.width * img_size.width / 4.0 + img_size.height * img_size.height / 4.0); 371 | T rho = polyval2(ss, m, radius); 372 | return Matrix(xx(0) / d_s * rho, xx(1) / d_s * rho); 373 | } 374 | 375 | 376 | 377 | double reprojectError(CalibData & cd) 378 | { 379 | double rms2 = 0; 380 | int count = 0; 381 | for (auto k = 0; k < cd.img_points.size(); ++k) 382 | { 383 | //reprojecting points 384 | for (int i = 0; i < cd.Xt.rows(); ++i) 385 | { 386 | Vector3d w = cd.RRfin[k] * Vector3d(cd.Xt(i), cd.Yt(i), 1); 387 | Vector2d reprojected = reprojectPoint(cd.ss, w, cd.img_size); 388 | 389 | Vector2d orig(cd.img_points[k][i].y - cd.xc, cd.img_points[k][i].x - cd.yc); 390 | //cout << w << " in pixels " << reprojected << endl; 391 | rms2 += (orig - reprojected).norm(); 392 | ++count; 393 | 394 | } 395 | } 396 | 397 | return rms2 / count; 398 | } 399 | 400 | double reprojectErrorSquaredSum(CalibData & cd) 401 | { 402 | double rms2 = 0; 403 | for (auto k = 0; k < cd.img_points.size(); ++k) 404 | { 405 | //reprojecting points 406 | for (int i = 0; i < cd.Xt.rows(); ++i) 407 | { 408 | Vector3d w = cd.RRfin[k] * Vector3d(cd.Xt(i), cd.Yt(i), 1); 409 | Vector2d reprojected = reprojectPoint(cd.ss, w, cd.img_size); 410 | 411 | Vector2d orig(cd.img_points[k][i].y - cd.xc, cd.img_points[k][i].x - cd.yc); 412 | //cout << w << " in pixels " << reprojected << endl; 413 | rms2 += (orig - reprojected).squaredNorm(); 414 | 415 | } 416 | } 417 | 418 | return rms2; 419 | } 420 | 421 | bool isMatricesZeros(const vector & ms) 422 | { 423 | for (auto & m : ms) 424 | { 425 | if (m.norm() < numeric_limits::epsilon()) 426 | return true; 427 | } 428 | return false; 429 | } 430 | 431 | 432 | 433 | double calibrateCameraOcam(CalibData & calib_data) 434 | { 435 | for (int kk = 0; kk < calib_data.img_points.size(); ++kk) 436 | { 437 | Eigen::MatrixXd Ypt(calib_data.img_points[kk].size(), 1); 438 | Eigen::MatrixXd Xpt(calib_data.img_points[kk].size(), 1); 439 | for (int j = 0; j < calib_data.img_points[kk].size(); ++j) 440 | { 441 | Ypt(j, 0) = calib_data.img_points[kk][j].x - calib_data.yc; 442 | Xpt(j, 0) = calib_data.img_points[kk][j].y - calib_data.xc; 443 | } 444 | 445 | //cout << Xpt << endl; 446 | //cout << Ypt << endl; 447 | 448 | Eigen::MatrixXd A(calib_data.img_points[kk].size(), 6); 449 | 450 | A.col(0) = calib_data.Xt.cwiseProduct(Ypt); 451 | A.col(1) = calib_data.Yt.cwiseProduct(Ypt); 452 | A.col(2) = -calib_data.Xt.cwiseProduct(Xpt); 453 | A.col(3) = -calib_data.Yt.cwiseProduct(Xpt); 454 | A.col(4) = Ypt; 455 | A.col(5) = -Xpt; 456 | 457 | //cout << A << endl; 458 | 459 | Eigen::JacobiSVD svd(A, Eigen::ComputeThinV); 460 | 461 | double R11 = svd.matrixV()(0, 5); 462 | double R12 = svd.matrixV()(1, 5); 463 | double R21 = svd.matrixV()(2, 5); 464 | double R22 = svd.matrixV()(3, 5); 465 | double T1 = svd.matrixV()(4, 5); 466 | double T2 = svd.matrixV()(5, 5); 467 | 468 | double AA = ((R11*R12) + (R21*R22)) * ((R11*R12) + (R21*R22)); 469 | double BB = R11 * R11 + R21 * R21; 470 | double CC = R12 *R12 + R22 * R22; 471 | vector R32_2 = polyRootD2(1, CC - BB, -AA); 472 | 473 | 474 | vector R31; 475 | vector R32; 476 | vector sg = { 1.0, -1.0 }; 477 | for (auto r : R32_2) 478 | { 479 | if (r > 0) 480 | { 481 | for (auto c : sg) 482 | { 483 | double sqrtR32_2 = c * sqrt(r); 484 | R32.push_back(sqrtR32_2); 485 | if (vectorEqualsZero(R32_2)) 486 | { 487 | R31.push_back(sqrt(CC - BB)); 488 | R31.push_back(-sqrt(CC - BB)); 489 | R32.push_back(sqrtR32_2); 490 | } 491 | else 492 | { 493 | R31.push_back((R11*R12 + R21*R22) / -sqrtR32_2); 494 | } 495 | } 496 | } 497 | } 498 | 499 | std::vector RR(R32.size() * 2); 500 | 501 | int count = -1; 502 | for (size_t i1 = 0; i1 < R32.size(); ++i1) 503 | { 504 | for (size_t i2 = 0; i2 < 2; ++i2) 505 | { 506 | count = count + 1; 507 | double Lb = 1 / sqrt(R11 * R11 + R21 * R21 + R31[i1] * R31[i1]); 508 | RR[count] << R11, R12, T1, 509 | R21, R22, T2, 510 | R31[i1], R32[i1], 0.0; 511 | RR[count] *= sg[i2] * Lb; 512 | } 513 | } 514 | 515 | 516 | 517 | double minRR = numeric_limits::infinity(); 518 | int minRR_ind = -1; 519 | for (size_t min_count = 0; min_count < RR.size(); ++min_count) 520 | { 521 | if ((Vector2d(RR[min_count](0, 2), RR[min_count](1, 2)) - Vector2d(Xpt(0), Ypt(0))).norm() < minRR) 522 | { 523 | minRR = (Vector2d(RR[min_count](0, 2), RR[min_count](1, 2)) - Vector2d(Xpt(0), Ypt(0))).norm(); 524 | minRR_ind = (int)min_count; 525 | } 526 | } 527 | 528 | std::vector RR1; 529 | if (minRR_ind != -1) 530 | { 531 | for (size_t count = 0; count < RR.size(); ++count) 532 | { 533 | if (sgn(RR[count](0, 2)) == sgn(RR[minRR_ind](0, 2)) && 534 | sgn(RR[count](1, 2)) == sgn(RR[minRR_ind](1, 2))) 535 | { 536 | RR1.push_back(RR[count]); 537 | //cout << RR[count] << endl; 538 | } 539 | } 540 | } 541 | 542 | if (RR1.empty()) 543 | { 544 | //RRfin = 0; 545 | //ss = 0; 546 | return numeric_limits::infinity(); 547 | } 548 | 549 | //R32_2 = roots([1, CC - BB, -AA]); 550 | //R32_2 = R32_2(find(R32_2 >= 0)); 551 | int nm = plot_RR(RR1, calib_data.Xt, calib_data.Yt, Xpt, Ypt); 552 | Matrix3d RRdef = RR1[nm]; 553 | calib_data.RRfin[kk] = RRdef; 554 | } 555 | int taylor_order_default = 4; 556 | std::vector ss = omni_find_parameters_fun(calib_data); 557 | 558 | 559 | //calib_data.ss_inv = invertOcamPoly(ss); 560 | 561 | 562 | double avg_reprojection_error = reprojectError(calib_data); 563 | 564 | 565 | return avg_reprojection_error; 566 | } 567 | 568 | 569 | void findCenter(CalibData & cd) 570 | { 571 | double pxc = cd.xc; 572 | double pyc = cd.yc; 573 | double width = cd.img_size.width; 574 | double height = cd.img_size.height; 575 | double regwidth = (width / 2); 576 | double regheight = (height / 2); 577 | double yceil = 5; 578 | double xceil = 5; 579 | 580 | double xregstart = pxc - (regheight / 2); 581 | double xregstop = pxc + (regheight / 2); 582 | double yregstart = pyc - (regwidth / 2); 583 | double yregstop = pyc + (regwidth / 2); 584 | //cout << "Iteration 1." << endl; 585 | for (int glc = 0; glc < 9; ++glc) 586 | { 587 | double s = ((yregstop - yregstart) / yceil); 588 | int c = int((yregstop - yregstart) / s + 1); 589 | 590 | MatrixXd yreg(c, c); 591 | for (int i = 0; i < c; ++i) 592 | { 593 | for (int j = 0; j < c; ++j) 594 | { 595 | yreg(i, j) = yregstart + j * s; 596 | } 597 | 598 | } 599 | //cout << yreg << endl; 600 | 601 | s = ((xregstop - xregstart) / xceil); 602 | //c = (xregstop - xregstart) / s + 1; 603 | 604 | MatrixXd xreg(c, c); 605 | for (int i = 0; i < c; ++i) 606 | { 607 | for (int j = 0; j < c; ++j) 608 | { 609 | xreg(i, j) = xregstart + i * s; 610 | } 611 | 612 | } 613 | //cout << xreg << endl; 614 | 615 | 616 | int ic_proc = (int)xreg.rows(); 617 | int jc_proc = (int)xreg.cols(); 618 | //MatrixXd MSEA = numeric_limits::infinity() * MatrixXd::Ones(xreg.rows(), xreg.cols()); 619 | int min_idx1, min_idx2; 620 | double min_MSEA = numeric_limits::max(); 621 | for (int ic = 0; ic < ic_proc; ++ic) 622 | { 623 | for (int jc = 0; jc < jc_proc; ++jc) 624 | { 625 | cd.xc = xreg(ic, jc); 626 | cd.yc = yreg(ic, jc); 627 | 628 | calibrateCameraOcam(cd); 629 | if (isMatricesZeros(cd.RRfin)) 630 | { 631 | // MSEA(ic, jc) = numeric_limits::infinity(); 632 | continue; 633 | } 634 | double MSE = reprojectError(cd); 635 | 636 | if (!isnan(MSE)) 637 | { 638 | if (MSE < min_MSEA) 639 | { 640 | min_MSEA = MSE; 641 | min_idx1 = ic; 642 | min_idx2 = jc; 643 | } 644 | //MSEA(ic, jc) = MSE; 645 | } 646 | } 647 | // %obrand_end 648 | } 649 | 650 | // % drawnow; 651 | //indMSE = find(min(MSEA(:)) == MSEA); 652 | cd.xc = xreg(min_idx1, min_idx2); 653 | cd.yc = yreg(min_idx1, min_idx2); 654 | double dx_reg = abs((xregstop - xregstart) / xceil); 655 | double dy_reg = abs((yregstop - yregstart) / yceil); 656 | xregstart = cd.xc - dx_reg; 657 | xregstop = cd.xc + dx_reg; 658 | yregstart = cd.yc - dy_reg; 659 | yregstop = cd.yc + dy_reg; 660 | //cout << glc << endl; 661 | } 662 | 663 | 664 | } 665 | #include 666 | 667 | struct SnavelyReprojectionError { 668 | SnavelyReprojectionError(double o_x, double o_y, const CalibData & cd, double wx, double wy) 669 | : observed_x(o_x), observed_y(o_y), calib_data(cd), world_x(wx), world_y(wy) {} 670 | 671 | 672 | template 673 | bool operator()(const T* const camera, 674 | T* residuals 675 | ) const 676 | { 677 | // camera[0,1,2] are the angle-axis rotation. 678 | //Vector3d p; 679 | Matrix p; 680 | Matrix3d R; 681 | ceres::AngleAxisToRotationMatrix(camera, R.data()); 682 | //cout << R * R.transpose() << endl; 683 | R(0, 2) = camera[3]; 684 | R(1, 2) = camera[4]; 685 | R(2, 2) = camera[5]; 686 | p(0) = world_x; 687 | p(1) = world_y; 688 | p(2) = 1; 689 | p = R * p; 690 | 691 | 692 | Matrix ret = reprojectPoint(calib_data.ss, p, calib_data.img_size); 693 | //cout << T(observed_y - calib_data.xc - ret.x()) << endl; 694 | //cout << T(observed_x - calib_data.yc - ret.y()) << endl; 695 | // The error is the difference between the predicted and observed position. 696 | residuals[0] = T(observed_y - calib_data.xc - ret.x()); 697 | residuals[1] = T(observed_x - calib_data.yc - ret.y()); 698 | return true; 699 | } 700 | 701 | // Factory to hide the construction of the CostFunction object from 702 | // the client code. 703 | // static ceres::CostFunction* Create(const double observed_x, 704 | // const double observed_y, const CalibData & cd) { 705 | // return (new ceres::AutoDiffCostFunction( 706 | // new SnavelyReprojectionError(observed_x, observed_y,cd))); 707 | // } 708 | 709 | static ceres::CostFunction* CreateNumericDiff(const double observed_x, 710 | const double observed_y, const CalibData & cd, double wx, double wy) { 711 | return (new ceres::NumericDiffCostFunction( 712 | new SnavelyReprojectionError(observed_x, observed_y, cd, wx, wy))); 713 | } 714 | 715 | double observed_x; 716 | double observed_y; 717 | double world_x; 718 | double world_y; 719 | 720 | const CalibData & calib_data; 721 | }; 722 | 723 | 724 | struct IntinsicsReprojectionError { 725 | IntinsicsReprojectionError(const CalibData & cd) : calib_data(cd) {} 726 | 727 | 728 | template 729 | bool operator()(const T* const camera, 730 | T* residuals 731 | ) const 732 | { 733 | // camera[0,1,2] are the angle-axis rotation. 734 | //Vector3d p; 735 | calib_data.xc = camera[0]; 736 | calib_data.yc = camera[1]; 737 | for (size_t i = 0; i < calib_data.ss.size(); ++i) 738 | calib_data.ss[i] = camera[2 + i]; 739 | 740 | //calib_data.ss_inv = invertOcamPoly(calib_data.ss); 741 | 742 | //cout << T(observed_y - calib_data.xc - ret.x()) << endl; 743 | //cout << T(observed_x - calib_data.yc - ret.y()) << endl; 744 | // The error is the difference between the predicted and observed position. 745 | residuals[0] = T(reprojectErrorSquaredSum(calib_data)); 746 | return true; 747 | } 748 | 749 | // Factory to hide the construction of the CostFunction object from 750 | // the client code. 751 | // static ceres::CostFunction* Create(const double observed_x, 752 | // const double observed_y, const CalibData & cd) { 753 | // return (new ceres::AutoDiffCostFunction( 754 | // new SnavelyReprojectionError(observed_x, observed_y,cd))); 755 | // } 756 | 757 | static ceres::CostFunction* CreateNumericDiff(const CalibData & cd) { 758 | return (new ceres::NumericDiffCostFunction( 759 | new IntinsicsReprojectionError(cd))); 760 | } 761 | 762 | mutable CalibData calib_data; 763 | }; 764 | 765 | double refineParameters(CalibData & cd, int iter_count) 766 | { 767 | double MSE_tol = 1e-8; 768 | double MSE_old = 0; 769 | double MSE_new = numeric_limits::max(); 770 | for (int iter = 0; iter < iter_count && abs(MSE_new - MSE_old) > MSE_tol; ++iter) 771 | { 772 | //vector center(cd.img_points.size(), Vector2d(cd.xc, cd.yc)); 773 | //optimizing chessboard positions 774 | for (int kk = 0; kk < cd.img_points.size(); ++kk) 775 | { 776 | 777 | Matrix3d R = cd.RRfin[kk]; 778 | R.col(2) = R.col(0).cross(R.col(1)); 779 | Vector3d t = cd.RRfin[kk].col(2); 780 | Vector3d r; 781 | ceres::RotationMatrixToAngleAxis(R.data(), r.data()); 782 | 783 | 784 | //cout << tt - R << endl; 785 | //cout < refined_RT; 788 | refined_RT.block(0, 0, 3, 1) = r; 789 | refined_RT.block(3, 0, 3, 1) = t; 790 | 791 | 792 | ceres::Problem problem; 793 | for (int j = 0; j < cd.img_points[kk].size(); ++j) 794 | { 795 | Vector3d p(cd.Xt(j), cd.Yt(j), 1.0); 796 | 797 | ceres::CostFunction* cost_function = 798 | SnavelyReprojectionError::CreateNumericDiff( 799 | cd.img_points[kk][j].x, 800 | cd.img_points[kk][j].y, cd, p.x(), p.y()); 801 | problem.AddResidualBlock(cost_function, 802 | NULL /* squared loss */, 803 | refined_RT.data()); 804 | 805 | //pr.AddResidualBlock(); 806 | } 807 | 808 | 809 | ceres::Solver::Options options; 810 | options.linear_solver_type = ceres::DENSE_SCHUR; 811 | options.minimizer_progress_to_stdout = false; 812 | options.logging_type = ceres::SILENT; 813 | ceres::Solver::Summary summary; 814 | ceres::Solve(options, &problem, &summary); 815 | //std::cout << summary.FullReport() << "\n"; 816 | //std::cout << center << "\n"; 817 | 818 | r = refined_RT.block(0, 0, 3, 1); 819 | //cout << t - refined_RT.block(3, 0, 3, 1) << endl; 820 | t = refined_RT.block(3, 0, 3, 1); 821 | 822 | 823 | ceres::AngleAxisToRotationMatrix(r.data(), cd.RRfin[kk].data()); 824 | //cout << cd.RRfin[kk] * cd.RRfin[kk].transpose() << endl; 825 | cd.RRfin[kk].col(2) = t; 826 | 827 | } 828 | 829 | //optimizing the intrinsics values 830 | ceres::CostFunction* cost_function = 831 | IntinsicsReprojectionError::CreateNumericDiff(cd); 832 | ceres::Problem problem; 833 | 834 | Matrix refined_params; 835 | refined_params[0] = cd.xc; 836 | refined_params[1] = cd.yc; 837 | for (size_t i = 0; i < cd.ss.size(); ++i) 838 | refined_params[2 + i] = cd.ss[i]; 839 | 840 | problem.AddResidualBlock(cost_function, 841 | NULL /* squared loss */, 842 | refined_params.data()); 843 | ceres::Solver::Options options; 844 | options.linear_solver_type = ceres::DENSE_SCHUR; 845 | options.logging_type = ceres::SILENT; 846 | options.minimizer_progress_to_stdout = false; 847 | ceres::Solver::Summary summary; 848 | ceres::Solve(options, &problem, &summary); 849 | //ceres::Solve(options, &problem, nullptr); 850 | //std::cout << summary.FullReport() << "\n"; 851 | 852 | 853 | cd.xc = refined_params[0]; 854 | cd.yc = refined_params[1]; 855 | for (size_t i = 0; i < cd.ss.size(); ++i) 856 | cd.ss[i] = refined_params[2 + i]; 857 | 858 | //cd.ss_inv = invertOcamPoly(cd.ss); 859 | MSE_old = MSE_new; 860 | MSE_new = reprojectError(cd); 861 | } 862 | cd.ss_inv = invertOcamPoly(cd.ss, cd.img_size.width, cd.img_size.height); 863 | return MSE_new; 864 | } 865 | 866 | double calibrateCameraOcam2(const vector > & objectPoints, 867 | const vector > & imagePoints, 868 | const cv::Size & imageSize, OcamCalibRes & res) 869 | { 870 | 871 | google::InitGoogleLogging("ocam"); 872 | CalibData cd; 873 | cd.img_size = imageSize; 874 | cd.taylor_order_default = 4; 875 | cd.img_points = imagePoints; 876 | 877 | 878 | //fel van cserélve az x és az y 879 | cd.xc = imageSize.height / 2.0; 880 | cd.yc = imageSize.width / 2.0; 881 | 882 | cd.Yt = Eigen::MatrixXd(objectPoints[0].size(), 1); 883 | cd.Xt = Eigen::MatrixXd(objectPoints[0].size(), 1); 884 | for (int j = 0; j < objectPoints[0].size(); ++j) 885 | { 886 | cd.Yt(j, 0) = objectPoints[0][j].x; 887 | cd.Xt(j, 0) = objectPoints[0][j].y; 888 | } 889 | 890 | cd.RRfin.resize(objectPoints.size()); 891 | 892 | double avg_reprojection_error = calibrateCameraOcam(cd); 893 | //cout << "Ocam Average reprojection error befor find center: " << avg_reprojection_error << endl; 894 | findCenter(cd); 895 | avg_reprojection_error = calibrateCameraOcam(cd); 896 | //cout << "Ocam Average reprojection error : " << avg_reprojection_error << endl; 897 | //cout << "img center: " << cd.xc << " : " << cd.yc << endl; 898 | 899 | avg_reprojection_error = refineParameters(cd, 100); 900 | //cout << "Ocam Average reprojection error after refinement : " << avg_reprojection_error << endl; 901 | 902 | res.center_x = cd.xc; 903 | res.center_y = cd.yc; 904 | 905 | res.ss = cd.ss; 906 | res.ss_inv = cd.ss_inv; 907 | 908 | for (auto v : cd.RRfin) 909 | { 910 | Matrix3d R = v; 911 | R.col(2) = R.col(0).cross(R.col(1)); 912 | 913 | res.R.push_back({ 914 | R(0,0), R(0, 1), R(0, 2), 915 | R(1,0), R(1, 1), R(1, 2), 916 | R(2,0), R(2, 1), R(2, 2) 917 | }); 918 | 919 | Vector3d t = v.col(2); 920 | res.T.push_back({t(0), t(1), t(2)}); 921 | } 922 | 923 | return avg_reprojection_error; 924 | } 925 | -------------------------------------------------------------------------------- /ocam_calibration.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | struct OcamCalibRes 6 | { 7 | std::vector ss; 8 | std::vector ss_inv; 9 | double center_x; 10 | double center_y; 11 | std::vector> R; 12 | std::vector> T; 13 | 14 | }; 15 | 16 | 17 | double calibrateCameraOcam2(const std::vector > & objectPoints, 18 | const std::vector > & imagePoints, const cv::Size & imageSize, OcamCalibRes & res); 19 | 20 | -------------------------------------------------------------------------------- /pinhole-calib.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/core.hpp" 2 | #include 3 | #include "opencv2/imgproc.hpp" 4 | #include "opencv2/calib3d.hpp" 5 | #include "opencv2/imgcodecs.hpp" 6 | #include "opencv2/videoio.hpp" 7 | #include "opencv2/highgui.hpp" 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace cv; 15 | using namespace std; 16 | 17 | const char * usage = 18 | " \nexample command line for calibration from a live feed.\n" 19 | " calibration -w=4 -h=5 -s=0.025 -o=camera.yml -op -oe\n" 20 | " \n" 21 | " example command line for calibration from a list of stored images:\n" 22 | " imagelist_creator image_list.xml *.png\n" 23 | " calibration -w=4 -h=5 -s=0.025 -o=camera.yml -op -oe image_list.xml\n" 24 | " where image_list.xml is the standard OpenCV XML/YAML\n" 25 | " use imagelist_creator to create the xml or yaml list\n" 26 | " file consisting of the list of strings, e.g.:\n" 27 | " \n" 28 | "\n" 29 | "\n" 30 | "\n" 31 | "view000.png\n" 32 | "view001.png\n" 33 | "\n" 34 | "view003.png\n" 35 | "view010.png\n" 36 | "one_extra_view.jpg\n" 37 | "\n" 38 | "\n"; 39 | 40 | 41 | 42 | 43 | const char* liveCaptureHelp = 44 | "When the live video from camera is used as input, the following hot-keys may be used:\n" 45 | " , 'q' - quit the program\n" 46 | " 'g' - start capturing images\n" 47 | " 'u' - switch undistortion on/off\n"; 48 | 49 | static void help() 50 | { 51 | printf( "This is a camera calibration sample.\n" 52 | "Usage: calibration\n" 53 | " -w= # the number of inner corners per one of board dimension\n" 54 | " -h= # the number of inner corners per another board dimension\n" 55 | " [-pt=] # the type of pattern: chessboard or circles' grid\n" 56 | " [-n=] # the number of frames to use for calibration\n" 57 | " # (if not specified, it will be set to the number\n" 58 | " # of board views actually available)\n" 59 | " [-d=] # a minimum delay in ms between subsequent attempts to capture a next view\n" 60 | " # (used only for video capturing)\n" 61 | " [-s=] # square size in some user-defined units (1 by default)\n" 62 | " [-o=] # the output filename for intrinsic [and extrinsic] parameters\n" 63 | " [-op] # write detected feature points\n" 64 | " [-oe] # write extrinsic parameters\n" 65 | " [-zt] # assume zero tangential distortion\n" 66 | " [-a=] # fix aspect ratio (fx/fy)\n" 67 | " [-p] # fix the principal point at the center\n" 68 | " [-v] # flip the captured images around the horizontal axis\n" 69 | " [-V] # use a video file, and not an image list, uses\n" 70 | " # [input_data] string for the video file name\n" 71 | " [-su] # show undistorted images after calibration\n" 72 | " [input_data] # input data, one of the following:\n" 73 | " # - text file with a list of the images of the board\n" 74 | " # the text file can be generated with imagelist_creator\n" 75 | " # - name of video file with a video of the board\n" 76 | " # if input_data not specified, a live view from the camera is used\n" 77 | "\n" ); 78 | printf("\n%s",usage); 79 | printf( "\n%s", liveCaptureHelp ); 80 | } 81 | 82 | enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 }; 83 | enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID }; 84 | 85 | static double computeReprojectionErrors( 86 | const vector >& objectPoints, 87 | const vector >& imagePoints, 88 | const vector& rvecs, const vector& tvecs, 89 | const Mat& cameraMatrix, const Mat& distCoeffs, 90 | vector& perViewErrors ) 91 | { 92 | vector imagePoints2; 93 | int i, totalPoints = 0; 94 | double totalErr = 0, err; 95 | perViewErrors.resize(objectPoints.size()); 96 | 97 | for( i = 0; i < (int)objectPoints.size(); i++ ) 98 | { 99 | projectPoints(Mat(objectPoints[i]), rvecs[i], tvecs[i], 100 | cameraMatrix, distCoeffs, imagePoints2); 101 | err = norm(Mat(imagePoints[i]), Mat(imagePoints2), NORM_L2); 102 | int n = (int)objectPoints[i].size(); 103 | perViewErrors[i] = (float)std::sqrt(err*err/n); 104 | totalErr += err*err; 105 | totalPoints += n; 106 | } 107 | 108 | return std::sqrt(totalErr/totalPoints); 109 | } 110 | 111 | static void calcChessboardCorners(Size boardSize, float squareSize, vector& corners, Pattern patternType = CHESSBOARD) 112 | { 113 | corners.resize(0); 114 | 115 | switch(patternType) 116 | { 117 | case CHESSBOARD: 118 | case CIRCLES_GRID: 119 | for( int i = 0; i < boardSize.height; i++ ) 120 | for( int j = 0; j < boardSize.width; j++ ) 121 | corners.push_back(Point3f(float(j*squareSize), 122 | float(i*squareSize), 0)); 123 | break; 124 | 125 | case ASYMMETRIC_CIRCLES_GRID: 126 | for( int i = 0; i < boardSize.height; i++ ) 127 | for( int j = 0; j < boardSize.width; j++ ) 128 | corners.push_back(Point3f(float((2*j + i % 2)*squareSize), 129 | float(i*squareSize), 0)); 130 | break; 131 | 132 | default: 133 | CV_Error(Error::StsBadArg, "Unknown pattern type\n"); 134 | } 135 | } 136 | 137 | static bool runCalibration( vector > imagePoints, 138 | Size imageSize, Size boardSize, Pattern patternType, 139 | float squareSize, float aspectRatio, 140 | int flags, Mat& cameraMatrix, Mat& distCoeffs, 141 | vector& rvecs, vector& tvecs, 142 | vector& reprojErrs, 143 | double& totalAvgErr) 144 | { 145 | cameraMatrix = Mat::eye(3, 3, CV_64F); 146 | if( flags & CALIB_FIX_ASPECT_RATIO ) 147 | cameraMatrix.at(0,0) = aspectRatio; 148 | 149 | distCoeffs = Mat::zeros(8, 1, CV_64F); 150 | 151 | vector > objectPoints(1); 152 | calcChessboardCorners(boardSize, squareSize, objectPoints[0], patternType); 153 | 154 | objectPoints.resize(imagePoints.size(),objectPoints[0]); 155 | 156 | double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, 157 | distCoeffs, rvecs, tvecs, flags|CALIB_FIX_K4|CALIB_FIX_K5); 158 | ///*|CALIB_FIX_K3*/|CALIB_FIX_K4|CALIB_FIX_K5); 159 | printf("RMS error reported by calibrateCamera: %g\n", rms); 160 | 161 | bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs); 162 | 163 | totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, 164 | rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs); 165 | 166 | return ok; 167 | } 168 | 169 | 170 | static void saveCameraParams( const string& filename, 171 | Size imageSize, Size boardSize, 172 | float squareSize, float aspectRatio, int flags, 173 | const Mat& cameraMatrix, const Mat& distCoeffs, 174 | const vector& rvecs, const vector& tvecs, 175 | const vector& reprojErrs, 176 | const vector >& imagePoints, 177 | double totalAvgErr ) 178 | { 179 | FileStorage fs( filename, FileStorage::WRITE ); 180 | 181 | time_t tt; 182 | time( &tt ); 183 | struct tm *t2 = localtime( &tt ); 184 | char buf[1024]; 185 | strftime( buf, sizeof(buf)-1, "%c", t2 ); 186 | 187 | fs << "calibration_time" << buf; 188 | 189 | if( !rvecs.empty() || !reprojErrs.empty() ) 190 | fs << "nframes" << (int)std::max(rvecs.size(), reprojErrs.size()); 191 | fs << "image_width" << imageSize.width; 192 | fs << "image_height" << imageSize.height; 193 | fs << "board_width" << boardSize.width; 194 | fs << "board_height" << boardSize.height; 195 | fs << "square_size" << squareSize; 196 | 197 | if( flags & CALIB_FIX_ASPECT_RATIO ) 198 | fs << "aspectRatio" << aspectRatio; 199 | 200 | if( flags != 0 ) 201 | { 202 | sprintf( buf, "flags: %s%s%s%s", 203 | flags & CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "", 204 | flags & CALIB_FIX_ASPECT_RATIO ? "+fix_aspectRatio" : "", 205 | flags & CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "", 206 | flags & CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : "" ); 207 | //cvWriteComment( *fs, buf, 0 ); 208 | } 209 | 210 | fs << "flags" << flags; 211 | 212 | fs << "camera_matrix" << cameraMatrix; 213 | fs << "distortion_coefficients" << distCoeffs; 214 | 215 | fs << "avg_reprojection_error" << totalAvgErr; 216 | if( !reprojErrs.empty() ) 217 | fs << "per_view_reprojection_errors" << Mat(reprojErrs); 218 | 219 | if( !rvecs.empty() && !tvecs.empty() ) 220 | { 221 | CV_Assert(rvecs[0].type() == tvecs[0].type()); 222 | Mat bigmat((int)rvecs.size(), 6, rvecs[0].type()); 223 | for( int i = 0; i < (int)rvecs.size(); i++ ) 224 | { 225 | Mat r = bigmat(Range(i, i+1), Range(0,3)); 226 | Mat t = bigmat(Range(i, i+1), Range(3,6)); 227 | 228 | CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1); 229 | CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1); 230 | //*.t() is MatExpr (not Mat) so we can use assignment operator 231 | r = rvecs[i].t(); 232 | t = tvecs[i].t(); 233 | } 234 | //cvWriteComment( *fs, "a set of 6-tuples (rotation vector + translation vector) for each view", 0 ); 235 | fs << "extrinsic_parameters" << bigmat; 236 | } 237 | 238 | if( !imagePoints.empty() ) 239 | { 240 | Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2); 241 | for( int i = 0; i < (int)imagePoints.size(); i++ ) 242 | { 243 | Mat r = imagePtMat.row(i).reshape(2, imagePtMat.cols); 244 | Mat imgpti(imagePoints[i]); 245 | imgpti.copyTo(r); 246 | } 247 | fs << "image_points" << imagePtMat; 248 | } 249 | } 250 | 251 | static bool readStringList( const string& filename, vector& l ) 252 | { 253 | l.resize(0); 254 | FileStorage fs(filename, FileStorage::READ); 255 | if( !fs.isOpened() ) 256 | return false; 257 | FileNode n = fs.getFirstTopLevelNode(); 258 | if( n.type() != FileNode::SEQ ) 259 | return false; 260 | FileNodeIterator it = n.begin(), it_end = n.end(); 261 | for( ; it != it_end; ++it ) 262 | l.push_back((string)*it); 263 | return true; 264 | } 265 | 266 | 267 | static bool runAndSave(const string& outputFilename, 268 | const vector >& imagePoints, 269 | Size imageSize, Size boardSize, Pattern patternType, float squareSize, 270 | float aspectRatio, int flags, Mat& cameraMatrix, 271 | Mat& distCoeffs, bool writeExtrinsics, bool writePoints ) 272 | { 273 | vector rvecs, tvecs; 274 | vector reprojErrs; 275 | double totalAvgErr = 0; 276 | 277 | bool ok = runCalibration(imagePoints, imageSize, boardSize, patternType, squareSize, 278 | aspectRatio, flags, cameraMatrix, distCoeffs, 279 | rvecs, tvecs, reprojErrs, totalAvgErr); 280 | printf("%s. avg reprojection error = %.2f\n", 281 | ok ? "Calibration succeeded" : "Calibration failed", 282 | totalAvgErr); 283 | 284 | if( ok ) 285 | saveCameraParams( outputFilename, imageSize, 286 | boardSize, squareSize, aspectRatio, 287 | flags, cameraMatrix, distCoeffs, 288 | writeExtrinsics ? rvecs : vector(), 289 | writeExtrinsics ? tvecs : vector(), 290 | writeExtrinsics ? reprojErrs : vector(), 291 | writePoints ? imagePoints : vector >(), 292 | totalAvgErr ); 293 | return ok; 294 | } 295 | 296 | 297 | int main( int argc, char** argv ) 298 | { 299 | Size boardSize, imageSize; 300 | float squareSize, aspectRatio; 301 | Mat cameraMatrix, distCoeffs; 302 | string outputFilename; 303 | string inputFilename = ""; 304 | 305 | int i, nframes; 306 | bool writeExtrinsics, writePoints; 307 | bool undistortImage = false; 308 | int flags = 0; 309 | VideoCapture capture; 310 | bool flipVertical; 311 | bool showUndistorted; 312 | bool videofile; 313 | int delay; 314 | clock_t prevTimestamp = 0; 315 | int mode = DETECTION; 316 | int cameraId = 0; 317 | vector > imagePoints; 318 | vector imageList; 319 | Pattern pattern = CHESSBOARD; 320 | 321 | cv::CommandLineParser parser(argc, argv, 322 | "{help ||}{w||}{h||}{pt|chessboard|}{n|10|}{d|1000|}{s|1|}{o|out_camera_data.yml|}" 323 | "{op||}{oe||}{zt||}{a|1|}{p||}{v||}{V||}{su||}" 324 | "{@input_data|0|}"); 325 | if (parser.has("help")) 326 | { 327 | help(); 328 | return 0; 329 | } 330 | boardSize.width = parser.get( "w" ); 331 | boardSize.height = parser.get( "h" ); 332 | if ( parser.has("pt") ) 333 | { 334 | string val = parser.get("pt"); 335 | if( val == "circles" ) 336 | pattern = CIRCLES_GRID; 337 | else if( val == "acircles" ) 338 | pattern = ASYMMETRIC_CIRCLES_GRID; 339 | else if( val == "chessboard" ) 340 | pattern = CHESSBOARD; 341 | else 342 | return fprintf( stderr, "Invalid pattern type: must be chessboard or circles\n" ), -1; 343 | } 344 | squareSize = parser.get("s"); 345 | nframes = parser.get("n"); 346 | aspectRatio = parser.get("a"); 347 | delay = parser.get("d"); 348 | writePoints = parser.has("op"); 349 | writeExtrinsics = parser.has("oe"); 350 | if (parser.has("a")) 351 | flags |= CALIB_FIX_ASPECT_RATIO; 352 | if ( parser.has("zt") ) 353 | flags |= CALIB_ZERO_TANGENT_DIST; 354 | if ( parser.has("p") ) 355 | flags |= CALIB_FIX_PRINCIPAL_POINT; 356 | flipVertical = parser.has("v"); 357 | videofile = parser.has("V"); 358 | if ( parser.has("o") ) 359 | outputFilename = parser.get("o"); 360 | showUndistorted = parser.has("su"); 361 | if ( isdigit(parser.get("@input_data")[0]) ) 362 | cameraId = parser.get("@input_data"); 363 | else 364 | inputFilename = parser.get("@input_data"); 365 | if (!parser.check()) 366 | { 367 | help(); 368 | parser.printErrors(); 369 | return -1; 370 | } 371 | if ( squareSize <= 0 ) 372 | return fprintf( stderr, "Invalid board square width\n" ), -1; 373 | if ( nframes <= 3 ) 374 | return printf("Invalid number of images\n" ), -1; 375 | if ( aspectRatio <= 0 ) 376 | return printf( "Invalid aspect ratio\n" ), -1; 377 | if ( delay <= 0 ) 378 | return printf( "Invalid delay\n" ), -1; 379 | if ( boardSize.width <= 0 ) 380 | return fprintf( stderr, "Invalid board width\n" ), -1; 381 | if ( boardSize.height <= 0 ) 382 | return fprintf( stderr, "Invalid board height\n" ), -1; 383 | 384 | if( !inputFilename.empty() ) 385 | { 386 | if( !videofile && readStringList(inputFilename, imageList) ) 387 | mode = CAPTURING; 388 | else 389 | capture.open(inputFilename); 390 | } 391 | else 392 | capture.open(cameraId); 393 | 394 | if( !capture.isOpened() && imageList.empty() ) 395 | return fprintf( stderr, "Could not initialize video (%d) capture\n",cameraId ), -2; 396 | 397 | if( !imageList.empty() ) 398 | nframes = (int)imageList.size(); 399 | 400 | if( capture.isOpened() ) 401 | printf( "%s", liveCaptureHelp ); 402 | 403 | namedWindow( "Image View", 1 ); 404 | 405 | for(i = 0;;i++) 406 | { 407 | Mat view, viewGray; 408 | bool blink = false; 409 | 410 | if( capture.isOpened() ) 411 | { 412 | Mat view0; 413 | capture >> view0; 414 | view0.copyTo(view); 415 | } 416 | else if( i < (int)imageList.size() ) 417 | view = imread(imageList[i], 1); 418 | 419 | if(view.empty()) 420 | { 421 | if( imagePoints.size() > 0 ) 422 | runAndSave(outputFilename, imagePoints, imageSize, 423 | boardSize, pattern, squareSize, aspectRatio, 424 | flags, cameraMatrix, distCoeffs, 425 | writeExtrinsics, writePoints); 426 | break; 427 | } 428 | 429 | imageSize = view.size(); 430 | 431 | if( flipVertical ) 432 | flip( view, view, 0 ); 433 | 434 | vector pointbuf; 435 | cvtColor(view, viewGray, COLOR_BGR2GRAY); 436 | 437 | bool found; 438 | switch( pattern ) 439 | { 440 | case CHESSBOARD: 441 | found = findChessboardCorners( view, boardSize, pointbuf, 442 | CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE); 443 | break; 444 | case CIRCLES_GRID: 445 | found = findCirclesGrid( view, boardSize, pointbuf ); 446 | break; 447 | case ASYMMETRIC_CIRCLES_GRID: 448 | found = findCirclesGrid( view, boardSize, pointbuf, CALIB_CB_ASYMMETRIC_GRID ); 449 | break; 450 | default: 451 | return fprintf( stderr, "Unknown pattern type\n" ), -1; 452 | } 453 | 454 | // improve the found corners' coordinate accuracy 455 | if( pattern == CHESSBOARD && found) cornerSubPix( viewGray, pointbuf, Size(11,11), 456 | Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.1 )); 457 | 458 | if( mode == CAPTURING && found && 459 | (!capture.isOpened() || clock() - prevTimestamp > delay*1e-3*CLOCKS_PER_SEC) ) 460 | { 461 | imagePoints.push_back(pointbuf); 462 | prevTimestamp = clock(); 463 | blink = capture.isOpened(); 464 | } 465 | 466 | if(found) 467 | drawChessboardCorners( view, boardSize, Mat(pointbuf), found ); 468 | 469 | string msg = mode == CAPTURING ? "100/100" : 470 | mode == CALIBRATED ? "Calibrated" : "Press 'g' to start"; 471 | int baseLine = 0; 472 | Size textSize = getTextSize(msg, 1, 1, 1, &baseLine); 473 | Point textOrigin(view.cols - 2*textSize.width - 10, view.rows - 2*baseLine - 10); 474 | 475 | if( mode == CAPTURING ) 476 | { 477 | if(undistortImage) 478 | msg = format( "%d/%d Undist", (int)imagePoints.size(), nframes ); 479 | else 480 | msg = format( "%d/%d", (int)imagePoints.size(), nframes ); 481 | } 482 | 483 | putText( view, msg, textOrigin, 1, 1, 484 | mode != CALIBRATED ? Scalar(0,0,255) : Scalar(0,255,0)); 485 | 486 | if( blink ) 487 | bitwise_not(view, view); 488 | 489 | if( mode == CALIBRATED && undistortImage ) 490 | { 491 | Mat temp = view.clone(); 492 | undistort(temp, view, cameraMatrix, distCoeffs); 493 | } 494 | 495 | imshow("Image View", view); 496 | int key = 0xff & waitKey(capture.isOpened() ? 50 : 500); 497 | 498 | if( (key & 255) == 27 ) 499 | break; 500 | 501 | if( key == 'u' && mode == CALIBRATED ) 502 | undistortImage = !undistortImage; 503 | 504 | if( capture.isOpened() && key == 'g' ) 505 | { 506 | mode = CAPTURING; 507 | imagePoints.clear(); 508 | } 509 | 510 | if( mode == CAPTURING && imagePoints.size() >= (unsigned)nframes ) 511 | { 512 | if( runAndSave(outputFilename, imagePoints, imageSize, 513 | boardSize, pattern, squareSize, aspectRatio, 514 | flags, cameraMatrix, distCoeffs, 515 | writeExtrinsics, writePoints)) 516 | mode = CALIBRATED; 517 | else 518 | mode = DETECTION; 519 | if( !capture.isOpened() ) 520 | break; 521 | } 522 | } 523 | 524 | if( !capture.isOpened() && showUndistorted ) 525 | { 526 | Mat view, rview, map1, map2; 527 | initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(), 528 | getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), 529 | imageSize, CV_16SC2, map1, map2); 530 | 531 | for( i = 0; i < (int)imageList.size(); i++ ) 532 | { 533 | view = imread(imageList[i], 1); 534 | if(view.empty()) 535 | continue; 536 | //undistort( view, rview, cameraMatrix, distCoeffs, cameraMatrix ); 537 | remap(view, rview, map1, map2, INTER_LINEAR); 538 | imshow("Image View", rview); 539 | int c = 0xff & waitKey(); 540 | if( (c & 255) == 27 || c == 'q' || c == 'Q' ) 541 | break; 542 | } 543 | } 544 | 545 | return 0; 546 | } 547 | -------------------------------------------------------------------------------- /stereo-calib.cpp: -------------------------------------------------------------------------------- 1 | /* This is sample from the OpenCV book. The copyright notice is below */ 2 | 3 | /* *************** License:************************** 4 | Oct. 3, 2008 5 | Right to use this code in any way you want without warranty, support or any guarantee of it working. 6 | 7 | BOOK: It would be nice if you cited it: 8 | Learning OpenCV: Computer Vision with the OpenCV Library 9 | by Gary Bradski and Adrian Kaehler 10 | Published by O'Reilly Media, October 3, 2008 11 | 12 | AVAILABLE AT: 13 | http://www.amazon.com/Learning-OpenCV-Computer-Vision-Library/dp/0596516134 14 | Or: http://oreilly.com/catalog/9780596516130/ 15 | ISBN-10: 0596516134 or: ISBN-13: 978-0596516130 16 | 17 | OPENCV WEBSITES: 18 | Homepage: http://opencv.org 19 | Online docs: http://docs.opencv.org 20 | Q&A forum: http://answers.opencv.org 21 | Issue tracker: http://code.opencv.org 22 | GitHub: https://github.com/Itseez/opencv/ 23 | ************************************************** */ 24 | 25 | #include "opencv2/calib3d/calib3d.hpp" 26 | #include "opencv2/imgcodecs.hpp" 27 | #include "opencv2/highgui/highgui.hpp" 28 | #include "opencv2/imgproc/imgproc.hpp" 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | using namespace cv; 40 | using namespace std; 41 | 42 | static int print_help() 43 | { 44 | cout << 45 | " Given a list of chessboard images, the number of corners (nx, ny)\n" 46 | " on the chessboards, and a flag: useCalibrated for \n" 47 | " calibrated (0) or\n" 48 | " uncalibrated \n" 49 | " (1: use cvStereoCalibrate(), 2: compute fundamental\n" 50 | " matrix separately) stereo. \n" 51 | " Calibrate the cameras and display the\n" 52 | " rectified results along with the computed disparity images. \n" << endl; 53 | cout << "Usage:\n ./stereo_calib -w= -h= \n" << endl; 54 | return 0; 55 | } 56 | 57 | 58 | static void 59 | StereoCalib(const vector& imagelist, Size boardSize,bool displayCorners = false, bool useCalibrated=true, bool showRectified=true, int flags = 0, float aspectRatio = 1.0f) 60 | { 61 | if( imagelist.size() % 2 != 0 ) 62 | { 63 | cout << "Error: the image list contains odd (non-even) number of elements\n"; 64 | return; 65 | } 66 | 67 | const int maxScale = 2; 68 | const float squareSize = 1.f; // Set this to your actual square size 69 | // ARRAY AND VECTOR STORAGE: 70 | 71 | vector > imagePoints[2]; 72 | vector > singleImagePoints[2]; 73 | vector > objectPoints; 74 | Size imageSize; 75 | 76 | int i, j, k, nimages = (int)imagelist.size()/2; 77 | 78 | imagePoints[0].resize(nimages); 79 | imagePoints[1].resize(nimages); 80 | vector goodImageList; 81 | 82 | for( i = j = 0; i < nimages; i++ ) 83 | { 84 | vector found = { false, false }; 85 | for( k = 0; k < 2; k++ ) 86 | { 87 | const string& filename = imagelist[i*2+k]; 88 | Mat img = imread(filename, 0); 89 | if(img.empty()) 90 | break; 91 | if( imageSize == Size() ) 92 | imageSize = img.size(); 93 | else if( img.size() != imageSize ) 94 | { 95 | cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n"; 96 | break; 97 | } 98 | 99 | vector& corners = imagePoints[k][j]; 100 | for( int scale = 1; scale <= maxScale; scale++ ) 101 | { 102 | Mat timg; 103 | if( scale == 1 ) 104 | timg = img; 105 | else 106 | resize(img, timg, Size(), scale, scale); 107 | found[k] = findChessboardCorners(timg, boardSize, corners, 108 | CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE); 109 | if( found[k] ) 110 | { 111 | if( scale > 1 ) 112 | { 113 | Mat cornersMat(corners); 114 | cornersMat *= 1./scale; 115 | } 116 | break; 117 | } 118 | } 119 | if( displayCorners ) 120 | { 121 | cout << filename << endl; 122 | Mat cimg, cimg1; 123 | cvtColor(img, cimg, COLOR_GRAY2BGR); 124 | drawChessboardCorners(cimg, boardSize, corners, found[k]); 125 | double sf = 640./MAX(img.rows, img.cols); 126 | resize(cimg, cimg1, Size(), sf, sf); 127 | imshow("corners", cimg1); 128 | char c = (char)waitKey(500); 129 | if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit 130 | exit(-1); 131 | } 132 | else 133 | putchar('.'); 134 | if (found[k]) 135 | { 136 | cornerSubPix(img, corners, Size(11, 11), Size(-1, -1), 137 | TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 138 | 30, 0.01)); 139 | singleImagePoints[k].push_back(corners); 140 | } 141 | } 142 | if( found[0] && found[1] ) 143 | { 144 | goodImageList.push_back(imagelist[i*2]); 145 | goodImageList.push_back(imagelist[i*2+1]); 146 | j++; 147 | } 148 | } 149 | cout << j << " pairs have been successfully detected.\n"; 150 | nimages = j; 151 | if( nimages < 2 ) 152 | { 153 | cout << "Error: too little pairs to run the calibration\n"; 154 | return; 155 | } 156 | 157 | imagePoints[0].resize(nimages); 158 | imagePoints[1].resize(nimages); 159 | objectPoints.resize(nimages); 160 | 161 | for( i = 0; i < nimages; i++ ) 162 | { 163 | for( j = 0; j < boardSize.height; j++ ) 164 | for( k = 0; k < boardSize.width; k++ ) 165 | objectPoints[i].push_back(Point3f(k*squareSize, j*squareSize, 0)); 166 | } 167 | 168 | cout << "Running stereo calibration ...\n"; 169 | 170 | Mat cameraMatrix[2], distCoeffs[2]; 171 | 172 | #define PINHOLE_CALIBRATION 173 | #ifdef PINHOLE_CALIBRATION 174 | vector rvecs, tvecs; 175 | vector reprojErrs; 176 | cameraMatrix[0] = Mat::eye(3, 3, CV_64F); 177 | cameraMatrix[1] = Mat::eye(3, 3, CV_64F); 178 | if (flags & CALIB_FIX_ASPECT_RATIO) 179 | { 180 | cameraMatrix[0].at(0, 0) = aspectRatio; 181 | cameraMatrix[1].at(0, 0) = aspectRatio; 182 | } 183 | 184 | double rms_single = calibrateCamera(objectPoints, singleImagePoints[0], imageSize, cameraMatrix[0], 185 | distCoeffs[0], rvecs, tvecs, flags | CALIB_FIX_K4 | CALIB_FIX_K5); 186 | printf("[cam1]: RMS error reported by calibrateCamera : %g\n", rms_single); 187 | rms_single = calibrateCamera(objectPoints, singleImagePoints[1], imageSize, cameraMatrix[1], 188 | distCoeffs[1], rvecs, tvecs, flags | CALIB_FIX_K4 | CALIB_FIX_K5); 189 | printf("[cam2]: RMS error reported by calibrateCamera: %g\n", rms_single); 190 | 191 | #else 192 | cameraMatrix[0] = initCameraMatrix2D(objectPoints, singleImagePoints[0],imageSize,0); 193 | cameraMatrix[1] = initCameraMatrix2D(objectPoints, singleImagePoints[1],imageSize,0); 194 | #endif 195 | 196 | 197 | 198 | Mat R, T, E, F; 199 | 200 | double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1], 201 | cameraMatrix[0], distCoeffs[0], 202 | cameraMatrix[1], distCoeffs[1], 203 | imageSize, R, T, E, F, 204 | CALIB_FIX_ASPECT_RATIO + 205 | CALIB_ZERO_TANGENT_DIST + 206 | CALIB_USE_INTRINSIC_GUESS + 207 | CALIB_SAME_FOCAL_LENGTH + 208 | CALIB_RATIONAL_MODEL + 209 | CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5, 210 | TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-5) ); 211 | cout << "done with RMS error=" << rms << endl; 212 | 213 | // CALIBRATION QUALITY CHECK 214 | // because the output fundamental matrix implicitly 215 | // includes all the output information, 216 | // we can check the quality of calibration using the 217 | // epipolar geometry constraint: m2^t*F*m1=0 218 | double err = 0; 219 | int npoints = 0; 220 | vector lines[2]; 221 | for( i = 0; i < nimages; i++ ) 222 | { 223 | int npt = (int)imagePoints[0][i].size(); 224 | Mat imgpt[2]; 225 | for( k = 0; k < 2; k++ ) 226 | { 227 | imgpt[k] = Mat(imagePoints[k][i]); 228 | undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]); 229 | computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]); 230 | } 231 | for( j = 0; j < npt; j++ ) 232 | { 233 | double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] + 234 | imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) + 235 | fabs(imagePoints[1][i][j].x*lines[0][j][0] + 236 | imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]); 237 | err += errij; 238 | } 239 | npoints += npt; 240 | } 241 | cout << "average epipolar err = " << err/npoints << endl; 242 | 243 | // save intrinsic parameters 244 | FileStorage fs("../data/intrinsics.yml", FileStorage::WRITE); 245 | if( fs.isOpened() ) 246 | { 247 | fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] << 248 | "M2" << cameraMatrix[1] << "D2" << distCoeffs[1]; 249 | fs.release(); 250 | } 251 | else 252 | cout << "Error: can not save the intrinsic parameters\n"; 253 | 254 | Mat R1, R2, P1, P2, Q; 255 | Rect validRoi[2]; 256 | 257 | stereoRectify(cameraMatrix[0], distCoeffs[0], 258 | cameraMatrix[1], distCoeffs[1], 259 | imageSize, R, T, R1, R2, P1, P2, Q, 260 | CALIB_ZERO_DISPARITY, 1, imageSize, &validRoi[0], &validRoi[1]); 261 | 262 | fs.open("extrinsics.yml", FileStorage::WRITE); 263 | if( fs.isOpened() ) 264 | { 265 | fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q; 266 | fs.release(); 267 | } 268 | else 269 | cout << "Error: can not save the extrinsic parameters\n"; 270 | 271 | // OpenCV can handle left-right 272 | // or up-down camera arrangements 273 | bool isVerticalStereo = fabs(P2.at(1, 3)) > fabs(P2.at(0, 3)); 274 | 275 | // COMPUTE AND DISPLAY RECTIFICATION 276 | if( !showRectified ) 277 | return; 278 | 279 | Mat rmap[2][2]; 280 | // IF BY CALIBRATED (BOUGUET'S METHOD) 281 | if( useCalibrated ) 282 | { 283 | // we already computed everything 284 | } 285 | // OR ELSE HARTLEY'S METHOD 286 | else 287 | // use intrinsic parameters of each camera, but 288 | // compute the rectification transformation directly 289 | // from the fundamental matrix 290 | { 291 | vector allimgpt[2]; 292 | for( k = 0; k < 2; k++ ) 293 | { 294 | for( i = 0; i < nimages; i++ ) 295 | std::copy(imagePoints[k][i].begin(), imagePoints[k][i].end(), back_inserter(allimgpt[k])); 296 | } 297 | F = findFundamentalMat(Mat(allimgpt[0]), Mat(allimgpt[1]), FM_8POINT, 0, 0); 298 | Mat H1, H2; 299 | stereoRectifyUncalibrated(Mat(allimgpt[0]), Mat(allimgpt[1]), F, imageSize, H1, H2, 3); 300 | 301 | R1 = cameraMatrix[0].inv()*H1*cameraMatrix[0]; 302 | R2 = cameraMatrix[1].inv()*H2*cameraMatrix[1]; 303 | P1 = cameraMatrix[0]; 304 | P2 = cameraMatrix[1]; 305 | } 306 | 307 | //Precompute maps for cv::remap() 308 | initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]); 309 | initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]); 310 | 311 | Mat canvas; 312 | double sf; 313 | int w, h; 314 | if( !isVerticalStereo ) 315 | { 316 | sf = 600./MAX(imageSize.width, imageSize.height); 317 | w = cvRound(imageSize.width*sf); 318 | h = cvRound(imageSize.height*sf); 319 | canvas.create(h, w*2, CV_8UC3); 320 | } 321 | else 322 | { 323 | sf = 300./MAX(imageSize.width, imageSize.height); 324 | w = cvRound(imageSize.width*sf); 325 | h = cvRound(imageSize.height*sf); 326 | canvas.create(h*2, w, CV_8UC3); 327 | } 328 | 329 | for( i = 0; i < nimages; i++ ) 330 | { 331 | for( k = 0; k < 2; k++ ) 332 | { 333 | Mat img = imread(goodImageList[i*2+k], 0), rimg, cimg; 334 | remap(img, rimg, rmap[k][0], rmap[k][1], INTER_LINEAR); 335 | cvtColor(rimg, cimg, COLOR_GRAY2BGR); 336 | Mat canvasPart = !isVerticalStereo ? canvas(Rect(w*k, 0, w, h)) : canvas(Rect(0, h*k, w, h)); 337 | resize(cimg, canvasPart, canvasPart.size(), 0, 0, INTER_AREA); 338 | if( useCalibrated ) 339 | { 340 | Rect vroi(cvRound(validRoi[k].x*sf), cvRound(validRoi[k].y*sf), 341 | cvRound(validRoi[k].width*sf), cvRound(validRoi[k].height*sf)); 342 | rectangle(canvasPart, vroi, Scalar(0,0,255), 3, 8); 343 | } 344 | } 345 | 346 | if( !isVerticalStereo ) 347 | for( j = 0; j < canvas.rows; j += 16 ) 348 | line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8); 349 | else 350 | for( j = 0; j < canvas.cols; j += 16 ) 351 | line(canvas, Point(j, 0), Point(j, canvas.rows), Scalar(0, 255, 0), 1, 8); 352 | imshow("rectified", canvas); 353 | char c = (char)waitKey(); 354 | if( c == 27 || c == 'q' || c == 'Q' ) 355 | break; 356 | } 357 | } 358 | 359 | 360 | static bool readStringList( const string& filename, vector& l ) 361 | { 362 | l.resize(0); 363 | FileStorage fs(filename, FileStorage::READ); 364 | if( !fs.isOpened() ) 365 | return false; 366 | FileNode n = fs.getFirstTopLevelNode(); 367 | if( n.type() != FileNode::SEQ ) 368 | return false; 369 | FileNodeIterator it = n.begin(), it_end = n.end(); 370 | for( ; it != it_end; ++it ) 371 | l.push_back((string)*it); 372 | return true; 373 | } 374 | 375 | int main(int argc, char** argv) 376 | { 377 | Size boardSize; 378 | string imagelistfn; 379 | bool showRectified; 380 | float aspectRatio; 381 | cv::CommandLineParser parser(argc, argv, "{w|9|}{h|6|}{zt||}{a|1|}{p||}{nr||}{help||}{@input|../data/stereo_calib.xml|}"); 382 | if (parser.has("help")) 383 | return print_help(); 384 | showRectified = !parser.has("nr"); 385 | imagelistfn = parser.get("@input"); 386 | boardSize.width = parser.get("w"); 387 | boardSize.height = parser.get("h"); 388 | aspectRatio = parser.get("a"); 389 | 390 | int flags = 0; 391 | 392 | if (parser.has("a")) 393 | flags |= CALIB_FIX_ASPECT_RATIO; 394 | if (parser.has("zt")) 395 | flags |= CALIB_ZERO_TANGENT_DIST; 396 | if (parser.has("p")) 397 | flags |= CALIB_FIX_PRINCIPAL_POINT; 398 | 399 | if (!parser.check()) 400 | { 401 | parser.printErrors(); 402 | return 1; 403 | } 404 | vector imagelist; 405 | bool ok = readStringList(imagelistfn, imagelist); 406 | if(!ok || imagelist.empty()) 407 | { 408 | cout << "can not open " << imagelistfn << " or the string list is empty" << endl; 409 | return print_help(); 410 | } 411 | 412 | StereoCalib(imagelist, boardSize,false, true, showRectified, flags, aspectRatio); 413 | return 0; 414 | } 415 | --------------------------------------------------------------------------------