ざっくりincalibしとく

incalib.cpp

#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <sstream>

std::string gen_fpath(int i, std::string ext)
{
    std::stringstream ss;
    ss << "./data/chessboard/";
//    ss << "../../data/chessboard/";
// fpath tmp    ss << "./data/chessboard/samples/";
    if (i < 100) ss << "0";
    if (i < 10) ss << "0";
    ss << i << ext;
    return ss.str();
}

namespace util {
    static const int DISPLAY_LIMIT_MAXCOL = 500;
    int str2int(std::string s) { int r = 0; std::istringstream ss(s); ss >> r; return r; }
    void imshow(const cv::Mat& img, const std::string& window_name, const int waiting_time)
    {
        if(img.cols > DISPLAY_LIMIT_MAXCOL){
            cv::Mat disp_img;
            double scale = (double)DISPLAY_LIMIT_MAXCOL/(double)img.rows;
            cv::resize(img, disp_img, cv::Size((int)(img.cols*scale), (int)(img.rows*scale)));

            cv::imshow(window_name.c_str(), disp_img);
            cv::waitKey(waiting_time);
            disp_img.release();
        } else {
            cv::imshow(window_name.c_str(), img);
            cv::waitKey(waiting_time);
        }
    }
};

namespace wname {
    const std::string original_img = "Original Image";
    const std::string undistorted_img = "Undistorted Image";
};

int main( int argc, char** argv )
{
    assert(argc == 6 && "Usage: ./incalib chessboard_col chessboard_row unit_size chessnum_of_images ext Example: ./incalib 10 7 10 20 .png");
    const int chessboard_col = util::str2int(std::string(argv[1]));
    const int chessboard_row = util::str2int(std::string(argv[2]));
	const cv::Size chessboard_size( chessboard_col, chessboard_row );

    const int unit_size      = util::str2int(std::string(argv[3]));
	const uint num_of_chessboard_imgs = util::str2int(std::string(argv[4]));
	const std::string fext = std::string(argv[5]);

    // load chessboard_img
	cv::vector<cv::Mat> chessboard_imgs;
	for( uint i = 0; i < num_of_chessboard_imgs; i++ ) {
        std::string fname = gen_fpath(i, fext);
        std::cerr << "Loading: " << fname << "...";
        cv::Mat load_img = cv::imread( fname, 0 ); // gray scale
        if ( load_img.empty() ) { std::cerr << "FAIL" << std::endl; return -1; }
		chessboard_imgs.push_back( load_img );
		std::cerr << "OK!" << std::endl;
	}

    cv::TermCriteria criteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.001 );

    // find chessboard corners and save them into image_points
	cv::vector< cv::vector<cv::Point2f> > image_points;
	for( uint i = 0; i < chessboard_imgs.size(); i++ ) {
		std::cerr << "Find corners from image " << i;

        cv::vector<cv::Point2f> corners;
		if( cv::findChessboardCorners( chessboard_imgs[i], chessboard_size, corners ) ) {
            image_points.push_back(corners);
			std::cerr << " ... All corners found." << std::endl;

            // render image_points
			cv::drawChessboardCorners( chessboard_imgs[i], chessboard_size, (cv::Mat)corners, true );
            util::imshow(chessboard_imgs[i], wname::original_img, 10);
		} else {
			std::cerr << " ... at least 1 corner not found." << std::endl;
		}
	}

    // world points corresponds to chessboard points
	cv::vector< cv::vector<cv::Point3f> > world_points;
	// save world points
    std::cerr << image_points.size() << std::endl;
	for( uint i=0; i < image_points.size(); i++ ) {
        cv::vector<cv::Point3f> coordinate_points;
		for( int j = 0 ; j < chessboard_size.area(); j++ ) { // row*col
			coordinate_points.push_back( cv::Point3f( static_cast<float>( j % chessboard_size.width * unit_size ),
						                              static_cast<float>( j / chessboard_size.width * unit_size ),
						                              0.0 ));
		}
        world_points.push_back(coordinate_points);
	}

    assert(image_points.size() == world_points.size() && "image points and world points not equal");

	// intrinsic params
	cv::Mat intrinsic_param_matrix;
	cv::Mat	distortion_coeffs;
    // extrinsic params (unique to each image)
	cv::vector<cv::Mat>	rotation_vectors;
	cv::vector<cv::Mat>	translation_vectors;
    // incalib
    std::cerr << "Calcurating..." << std::endl;
	cv::calibrateCamera( world_points, image_points, chessboard_imgs[0].size(), intrinsic_param_matrix, distortion_coeffs, rotation_vectors, translation_vectors);
	std::cerr << "Camera parameters have been estimated" << std::endl << std::endl;

	// undistortion
	std::cerr << "Undistorted images" << std::endl;
	cv::Mat	undistorted_img;
	cv::namedWindow( wname::undistorted_img );
	for( uint i = 0; i < chessboard_imgs.size(); i++ ) {
		cv::undistort( chessboard_imgs[i], undistorted_img, intrinsic_param_matrix, distortion_coeffs);
        cv::Mat src_img[2];
        src_img[0] = chessboard_imgs[i];
        src_img[1] = undistorted_img;
        cv::Mat combined_img;
        cv::hconcat(src_img, 2, combined_img);

        util::imshow(combined_img, wname::undistorted_img, 200);
        combined_img.release();
	}

    // filio::output
    int system_return = system("mkdir data");
    cv::FileStorage cvfs("./data/camera.xml", CV_STORAGE_WRITE);
    cv::write(cvfs,"intrinsicMat", intrinsic_param_matrix);
    cv::write(cvfs,"distCoeffs", distortion_coeffs);

    // fileio::input
    /*
    cv::FileStorage cvfs("./data/camera.xml", CV_STORAGE_READ);
    cv::FileNode node(cvfs.fs, NULL);
    cv::Mat in_mat, dist_coeff;
    cv::read(node["intrinsicMat"], in_mat);
    cv::read(node["distCoeffs"], dist_coeff);
    std::cerr << ma << std::endl;
    std::cerr << mb << std::endl;
    */

	return 0;
}