前々回の記事で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ファイルに保存します.ソースと一緒にテスト用の画像を置いているので,動作確認にどうぞ.※いつものことですが,結果に責任は負いません.

1件のコメント

  1. […] (17/04/10)プログラム全体を上げました. […]

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です