前々回の記事でOpenCVの魚眼レンズのキャリブレーション関数がうまく動作しなかったため，fisheye::calibrate()関数をちょこちょこ改造したものを貼っていましたが，あとで必要になりそうな気がするので，キャリブレーションプログラム全体のソースを置いておきます．
fisheye::calibrate()に加えた変更点は，以下のような感じです．
1. ヤコビアンの逆行列の計算をEigen::fullPivLUに変更
2. 最急降下法の学習率を外部から設定できるように変更
プログラム全体としては，チェスコーナーの検出後，一度，カメラ内部パラメータのみの推定を行った後，それを初期値として歪みパラメータを含めた推定を行うようにしています．
学習率(α)は特にセンシティブなので，何回か変更しながら実行して，いい具合に収束する値を使うといいと思います．手持ちのPCでは0.4~0.6あたりが誤差の収束が良かったです．うまくいくと，下左の画像のように再投影誤差(error)がぐいぐい下がっていきます．収束は画像の枚数にもかなり依存しているようなので，多すぎない適当な枚数(30~40くらい?)で試すと良さそうです．
#include <string>
#include <vector>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <boost/filesystem.hpp>
#include "fisheye.hpp"
// reads images in a directory
std::vector<cv::Mat> read_images(const std::string& directory_path) {
std::vector<cv::Mat> images;
boost::filesystem::directory_iterator dir(directory_path);
boost::filesystem::directory_iterator end;
for(dir; dir != end; dir++) {
std::cout << dir->path().string() << std::endl;
cv::Mat image = cv::imread(dir->path().string());
if(image.data) {
images.push_back(image);
} else {
std::cerr << "couldn't read the image file!!" << dir->path().string() << std::endl;
}
}
return images;
}
// finds chessboard corners on an image
std::vector<cv::Point2f> find_chessboard_corners(const cv::Mat& image, int rows, int cols) {
std::vector<cv::Point2f> corners;
cv::Mat gray;
cv::cvtColor(image, gray, CV_BGR2GRAY);
bool found = cv::findChessboardCorners(gray, cv::Size(cols, rows), corners);
if(!found || corners.size() != rows * cols) {
std::cerr << "failed to find the corners!!" << std::endl;
return std::vector<cv::Point2f>();
}
cv::find4QuadCornerSubpix(gray, corners, cv::Size(3, 3));
cv::Mat canvas = image.clone();
cv::drawChessboardCorners(canvas, cv::Size(cols, rows), corners, found);
cv::imshow("image", canvas);
cv::waitKey(1);
return corners;
}
// undistorts an fisheye image
// @ref http://stackoverflow.com/questions/38983164/opencv-fisheye-undistort-issues
cv::Mat undistort(const cv::Mat& K, const cv::Mat& D, const cv::Mat& frame) {
cv::Mat newK, map1, map2;
cv::Mat rview(frame.size(), frame.type());
cv::fisheye::estimateNewCameraMatrixForUndistortRectify(K, D, frame.size(), cv::Matx33d::eye(), newK, 1);
cv::fisheye::initUndistortRectifyMap(K, D, cv::Matx33d::eye(), newK, frame.size(), CV_16SC2, map1, map2);
cv::remap(frame, rview, map1, map2, cv::INTER_LINEAR);
return rview;
}
// main
int main(int argc, char** argv) {
// chessboard parameters
const int chess_rows = 6;
const int chess_cols = 9;
const double chess_size = 21.5 / 1000.0;
// gradient decent parameter
// carefully choose a GOOD value between 0.0 and 1.0 (OpenCV default = 0.4)
const double alpha_smooth = 0.6;
std::cout << "reading images..." << std::endl;
const auto& images = read_images("imgs");
std::cout << "detecting the chessboard corners..." << std::endl;
std::cout << "# of images : " << images.size() << std::endl;
std::vector<std::vector<cv::Point2f>> image_points;
for(const auto& image : images) {
auto corners = find_chessboard_corners(image, chess_rows, chess_cols);
if(!corners.empty()){
image_points.push_back(corners);
}
}
std::cout << "calculating the point coordinates in the object space..." << std::endl;
std::vector<std::vector<cv::Point3f>> object_points;
std::vector<cv::Point3f> points;
for(int i=0; i<chess_rows; i++) {
for(int j=0; j<chess_cols; j++) {
points.push_back(cv::Point3f(i*chess_size, j*chess_size, 0.0f));
}
}
object_points.assign(image_points.size(), points);
std::cout << "calibrating..." << std::endl;
cv::Mat intrinsic, distortion;
cv::Mat rvecs, tvecs;
std::cout << "STEP 1: intrinsic estimation" << std::endl;
cv::TermCriteria criteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 1024, 1e-9);
int flags = cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC | cv::fisheye::CALIB_FIX_SKEW |
cv::fisheye::CALIB_FIX_K1 | cv::fisheye::CALIB_FIX_K2 |
cv::fisheye::CALIB_FIX_K3 | cv::fisheye::CALIB_FIX_K4;
// cv::fisheye::calibrate(object_points, image_points, cv::Size(images[0].cols, images[0].rows), intrinsic, distortion, rvecs, tvecs, flags, criteria);
cv::fisheye::calibrate_(object_points, image_points, cv::Size(images[0].cols, images[0].rows), intrinsic, distortion, rvecs, tvecs, flags, criteria, alpha_smooth);
std::cout << "--- intrinsic ---\n" << intrinsic << std::endl;
std::cout << "--- distortion ---\n" << distortion << std::endl;
std::cout << "STEP 2: intrinsic & distortion estimation" << std::endl;
flags = cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC | cv::fisheye::CALIB_USE_INTRINSIC_GUESS | cv::fisheye::CALIB_FIX_SKEW;
// cv::fisheye::calibrate(object_points, image_points, cv::Size(images[0].cols, images[0].rows), intrinsic, distortion, rvecs, tvecs, flags, criteria);
cv::fisheye::calibrate_(object_points, image_points, cv::Size(images[0].cols, images[0].rows), intrinsic, distortion, rvecs, tvecs, flags, criteria, alpha_smooth);
std::cout << "--- intrinsic ---\n" << intrinsic << std::endl;
std::cout << "--- distortion ---\n" << distortion << std::endl;
cv::FileStorage fs("camera.xml", cv::FileStorage::WRITE);
fs << "intrinsic" << intrinsic;
fs << "distortion" << distortion;
fs.release();
for(const cv::Mat& image : images) {
cv::Mat undistorted = undistort(intrinsic, distortion, image);
cv::imshow("undistorted", undistorted);
cv::waitKey(0);
}
return 0;
}
※以前の記事に貼ってあるfisheye.hpp, fisheye.cppが必要です．下のzipにも含まれています．(コメントアウトしてあるOpenCV標準のfisheye::calibrate()を使っても条件が良ければ動くと思います．)
fisheye_calib.zip
(Ubuntu 14.04 + OpenCV 2.4.13 + gcc4.8, Win7 + OpenCV 2.4.10 + VC++2013で一応動作確認)
実行ファイルと同じ場所のimgsという名前のディレクトリ内の画像を読み込んでカメラパラメータをxmlファイルに保存します．ソースと一緒にテスト用の画像を置いているので，動作確認にどうぞ．※いつものことですが，結果に責任は負いません．
2021/02/20：エンコード＆リンクを直した
ピンバック: fisheye calibration | tetro