Merge branch 'feature/calibapp-stereo' into feature/calibapp

applications/calibration/CMakeLists.txt

@@ -3,6 +3,7 @@ set(CALIBSRC
 	src/lens.cpp
 	src/stereo.cpp
 	src/align.cpp
+	src/common.cpp
 )
 
 add_executable(ftl-calibrate ${CALIBSRC})

applications/calibration/src/common.cpp

+#include <loguru.hpp>
+#include <ftl/config.h>
+
+#include <opencv2/calib3d.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/videoio.hpp>
+#include <opencv2/highgui.hpp>
+
+#include "common.hpp"
+
+using std::vector;
+using std::map;
+using std::string;
+
+using cv::Mat;
+using cv::Vec2f, cv::Vec3f;
+using cv::Size;
+
+using cv::stereoCalibrate;
+
+namespace ftl {
+namespace calibration {
+
+// Options
+
+string getOption(map<string, string> &options, const string &opt) {
+	auto str = options[opt];
+	return str.substr(1,str.size()-2);
+}
+
+bool hasOption(const map<string, string> &options, const string &opt) {
+	return options.find(opt) != options.end();
+}
+
+// Save/load files
+
+bool saveExtrinsics(const string &ofile, Mat &R, Mat &T, Mat &R1, Mat &R2, Mat &P1, Mat &P2, Mat &Q) {
+	cv::FileStorage fs;
+	fs.open(ofile, cv::FileStorage::WRITE);
+	if (fs.isOpened()) {
+		fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1"
+			<< P1 << "P2" << P2 << "Q" << Q;
+		fs.release();
+		return true;
+	} else {
+		LOG(ERROR) << "Error: can not save the extrinsic parameters";
+	}
+	return false;
+}
+
+bool saveIntrinsics(const string &ofile, const Mat &M, const Mat& D) {
+	cv::FileStorage fs(ofile, cv::FileStorage::WRITE);
+	if (fs.isOpened()) {
+		fs << "M" << M << "D" << D;
+		fs.release();
+		return true;
+	}
+	else {
+		LOG(ERROR) << "Error: can not save the intrinsic parameters to '" << ofile << "'";
+	}
+	return false;
+}
+
+bool loadIntrinsics(const string &ifile, Mat &M1, Mat &D1) {
+	using namespace cv;
+
+	FileStorage fs;
+
+	// reading intrinsic parameters
+	fs.open((ifile).c_str(), FileStorage::READ);
+	if (!fs.isOpened()) {
+		LOG(WARNING) << "Could not open intrinsics file : " << ifile;
+		return false;
+	}
+	
+	LOG(INFO) << "Intrinsics from: " << ifile;
+
+	fs["M"] >> M1;
+	fs["D"] >> D1;
+
+	return true;
+}
+
+// Calibration classes for different patterns
+
+CalibrationChessboard::CalibrationChessboard(const map<string, string> &opt) {
+	pattern_size_ = Size(9, 6);
+	image_size_ = Size(1280, 720);
+	pattern_square_size_ = 36.0;//0.036;
+	// CALIB_CB_NORMALIZE_IMAfE | CALIB_CB_EXHAUSTIVE | CALIB_CB_ACCURACY 
+	chessboard_flags_ = cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_ACCURACY;
+}
+
+void CalibrationChessboard::objectPoints(vector<Vec3f> &out) {
+	out.reserve(pattern_size_.width * pattern_size_.height);
+	for (int row = 0; row < pattern_size_.height; ++row) {
+	for (int col = 0; col < pattern_size_.width; ++col) {
+		out.push_back(Vec3f(col * pattern_square_size_, row * pattern_square_size_, 0));
+	}}
+}
+
+bool CalibrationChessboard::findPoints(Mat &img, vector<Vec2f> &points) {
+	return cv::findChessboardCornersSB(img, pattern_size_, points, chessboard_flags_);
+}
+
+void CalibrationChessboard::drawPoints(Mat &img, const vector<Vec2f> &points) {
+	cv::drawChessboardCorners(img, pattern_size_, points, true);
+}
+
+}
+}

applications/calibration/src/common.hpp

+#ifndef _FTL_CALIBRATION_COMMON_HPP_
+#define _FTL_CALIBRATION_COMMON_HPP_
+
+#include <map>
+#include <string>
+
+#include <opencv2/core.hpp>
+
+namespace ftl {
+namespace calibration {
+
+std::string getOption(std::map<std::string, std::string> &options, const std::string &opt);
+bool hasOption(const std::map<std::string, std::string> &options, const std::string &opt);
+
+bool loadIntrinsics(const std::string &ifile, cv::Mat &M1, cv::Mat &D1);
+bool saveIntrinsics(const std::string &ofile, const cv::Mat &M1, const cv::Mat &D1);
+
+// TODO loadExtrinsics()
+bool saveExtrinsics(const std::string &ofile, cv::Mat &R, cv::Mat &T, cv::Mat &R1, cv::Mat &R2, cv::Mat &P1, cv::Mat &P2, cv::Mat &Q);
+
+/**
+ * @brief	Wrapper for OpenCV's calibration methods. Paramters depend on
+ * 			implementation (different types of patterns).
+ *
+ * Calibration objects may store state; eg. from previous views of calibration
+ * images.
+ */
+class Calibration {
+public:
+	/**
+	 * @brief	Calculate reference points for given pattern
+	 * @param	Output parameter
+	 */
+	void objectPoints(std::vector<cv::Vec3f> &out);
+
+	/**
+	 * @brief	Try to find calibration pattern in input image
+	 * @param	Input image
+	 * @param	Output parameter for found point image coordinates
+	 * @returns	true if pattern found, otherwise false
+	 */
+	bool findPoints(cv::Mat &in, std::vector<cv::Vec2f> &out);
+
+	/**
+	 * @brief	Draw points to image
+	 * @param	Image to draw to
+	 * @param	Pattern points (in image coordinates)
+	 */
+	void drawPoints(cv::Mat &img, const std::vector<cv::Vec2f> &points);
+};
+
+/**
+ * @brief	Chessboard calibration pattern. Uses OpenCV's
+ * 			findChessboardCornersSB function.
+ * @todo	Parameters hardcoded in constructor
+ *
+ * All parameters:
+ * 	- pattern size (inner corners)
+ * 	- square size, millimeters (TODO: meters)
+ * 	- image size, pixels
+ * 	- flags, see ChessboardCornersSB documentation
+ */
+class CalibrationChessboard : Calibration {
+public:
+	CalibrationChessboard(const std::map<std::string, std::string> &opt);
+	void objectPoints(std::vector<cv::Vec3f> &out);
+	bool findPoints(cv::Mat &in, std::vector<cv::Vec2f> &out);
+	void drawPoints(cv::Mat &img, const std::vector<cv::Vec2f> &points);
+
+private:
+	int chessboard_flags_ = 0;
+	float pattern_square_size_;
+	cv::Size pattern_size_;
+	cv::Size image_size_;
+};
+
+// TODO other patterns, circles ...
+
+}
+}
+
+#endif // _FTL_CALIBRATION_COMMON_HPP_

applications/calibration/src/lens.cpp

+#include "common.hpp"
 #include "lens.hpp"
 
 #include <loguru.hpp>
@@ -17,324 +18,104 @@ using std::string;
 using std::vector;
 
 using cv::Mat;
-using cv::Point2f;
-using cv::Point3f;
+using cv::Vec2f;
+using cv::Vec3f;
 using cv::Size;
 
-struct Settings {
-    float squareSize;
-    cv::Size boardSize;
-    int nrFrames;
-    float delay;
-    std::string outputfile;
-    int device;
-    bool useFisheye;
-    double aspectRatio;
-    int flag;
-};
-
-//! [compute_errors]
-static double computeReprojectionErrors(
-		const vector<vector<Point3f> >& objectPoints,
-		const vector<vector<Point2f> >& imagePoints,
-		const vector<Mat>& rvecs, const vector<Mat>& tvecs,
-		const Mat& cameraMatrix , const Mat& distCoeffs,
-		vector<float>& perViewErrors, bool fisheye) {
-    using namespace cv;
-
-	vector<Point2f> imagePoints2;
-	size_t totalPoints = 0;
-	double totalErr = 0;
-	perViewErrors.resize(objectPoints.size());
-
-	for (size_t i = 0; i < objectPoints.size(); ++i) {
-		if (fisheye) {
-			cv::fisheye::projectPoints(objectPoints[i], imagePoints2, rvecs[i],
-					tvecs[i], cameraMatrix, distCoeffs);
-		} else {
-			projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix,
-					distCoeffs, imagePoints2);
-		}
-		double err = norm(imagePoints[i], imagePoints2, NORM_L2);
-
-		size_t n = objectPoints[i].size();
-		perViewErrors[i] = static_cast<float>(std::sqrt(err*err/n));
-		totalErr        += err*err;
-		totalPoints     += n;
-	}
-
-	return std::sqrt(totalErr/totalPoints);
-}
-
-//! [board_corners]
-static void calcBoardCornerPositions(Size boardSize, float squareSize,
-		vector<Point3f>& corners) {
-	corners.clear();
+using namespace ftl::calibration;
 
-    for (int i = 0; i < boardSize.height; ++i)
-        for (int j = 0; j < boardSize.width; ++j)
-            corners.push_back(Point3f(j*squareSize, i*squareSize, 0));
-
-}
-
-//! [board_corners]
-static bool _runCalibration(const Settings& s, Size& imageSize,
-		Mat& cameraMatrix, Mat& distCoeffs,
-		vector<vector<Point2f> > imagePoints, vector<Mat>& rvecs,
-		vector<Mat>& tvecs, vector<float>& reprojErrs,  double& totalAvgErr,
-		vector<Point3f>& newObjPoints, float grid_width, bool release_object) {
-
-    using namespace cv;
-
-	cameraMatrix = Mat::eye(3, 3, CV_64F);
-	if (s.flag & CALIB_FIX_ASPECT_RATIO)
-		cameraMatrix.at<double>(0, 0) = s.aspectRatio;
-
-	//if (s.useFisheye) {
-	//	distCoeffs = Mat::zeros(4, 1, CV_64F);
-	//} else {
-		distCoeffs = Mat::zeros(8, 1, CV_64F);
-	//}
-
-	vector<vector<Point3f> > objectPoints(1);
-	calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0]);
-	objectPoints[0][s.boardSize.width - 1].x = objectPoints[0][0].x +
-			grid_width;
-	newObjPoints = objectPoints[0];
-
-	objectPoints.resize(imagePoints.size(), objectPoints[0]);
-
-	// Find intrinsic and extrinsic camera parameters
-	double rms;
-
-	if (s.useFisheye) {
-		Mat _rvecs, _tvecs;
-		rms = cv::fisheye::calibrate(objectPoints, imagePoints, imageSize,
-				cameraMatrix, distCoeffs, _rvecs, _tvecs, s.flag);
-
-		rvecs.reserve(_rvecs.rows);
-		tvecs.reserve(_tvecs.rows);
-		for (int i = 0; i < static_cast<int>(objectPoints.size()); i++) {
-			rvecs.push_back(_rvecs.row(i));
-			tvecs.push_back(_tvecs.row(i));
-		}
-	} else {
-		rms = calibrateCamera(objectPoints, imagePoints, imageSize,
-								cameraMatrix, distCoeffs, rvecs, tvecs,
-								s.flag | CALIB_USE_LU);
-	}
-
-	LOG(INFO) << "Re-projection error reported by calibrateCamera: "<< rms;
-
-	bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
-
-	objectPoints.clear();
-	objectPoints.resize(imagePoints.size(), newObjPoints);
-	totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, rvecs,
-			tvecs, cameraMatrix, distCoeffs, reprojErrs, s.useFisheye);
-
-	return ok;
-}
+void ftl::calibration::intrinsic(map<string, string> &opt) {
+	LOG(INFO) << "Begin intrinsic calibration";
 
-//! [run_and_save]
-bool runCalibration(const Settings& s, Size imageSize,
-		Mat& cameraMatrix, Mat& distCoeffs,
-		vector<vector<Point2f> > imagePoints, float grid_width,
-		bool release_object) {
-	vector<Mat> rvecs, tvecs;
-	vector<float> reprojErrs;
-	double totalAvgErr = 0;
-	vector<Point3f> newObjPoints;
+	// TODO PARAMETERS TO CONFIG FILE
+	Size image_size = Size(1280, 720);
+	int iter = 60;
+	string filename_intrinsics = (hasOption(opt, "profile")) ? getOption(opt, "profile") : "./panasonic.yml";
+	CalibrationChessboard calib(opt); // TODO paramters hardcoded in constructor
 	
-	bool ok = _runCalibration(s, imageSize, cameraMatrix, distCoeffs,
-			imagePoints, rvecs, tvecs, reprojErrs, totalAvgErr, newObjPoints,
-			grid_width, release_object);
-	LOG(INFO) << (ok ? "Calibration succeeded" : "Calibration failed")
-		 << ". avg re projection error = " << totalAvgErr;
 
-	return ok;
-}
+	int calibrate_flags = cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_ASPECT_RATIO;
+	// PARAMETERS
 
-static bool recalibrate(const Settings &settings, vector<vector<Point2f>> &imagePoints,
-		Mat &cameraMatrix, Mat &distCoeffs, Size &imageSize) {
-    using namespace cv;
+	cv::VideoCapture camera = cv::VideoCapture(0);
 
-    VideoCapture camera(settings.device);
 	if (!camera.isOpened()) {
 		LOG(ERROR) << "Could not open camera device";
-        return false;
+		return;
 	}
+	camera.set(cv::CAP_PROP_FRAME_WIDTH, image_size.width); 
+	camera.set(cv::CAP_PROP_FRAME_HEIGHT, image_size.height);
 
-    camera.set(cv::CAP_PROP_FRAME_WIDTH, 1280);  // TODO Use settings
-	camera.set(cv::CAP_PROP_FRAME_HEIGHT, 720);
+	vector<vector<Vec2f>> image_points;
+	vector<vector<Vec3f>> object_points;
 	
-	float grid_width = settings.squareSize * (settings.boardSize.width - 1);
-	bool release_object = false;
-	double prevTimestamp = 0.0;
-	const Scalar RED(0, 0, 255), GREEN(0, 255, 0);
+	while (iter > 0) {
+		Mat img;
+		vector<Vec2f> points;
 		
-#if CV_VERSION_MAJOR >= 4
-	int chessBoardFlags = CALIB_CB_NORMALIZE_IMAGE;
-#else
-	int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
+		camera.grab();
+		camera.retrieve(img);
 		
-	if (!settings.useFisheye) {
-		// fast check erroneously fails with high distortions like fisheye
-		chessBoardFlags |= CALIB_CB_FAST_CHECK;
-	}
-#endif
+		bool found = calib.findPoints(img, points);
+		if (found) { calib.drawPoints(img, points); }
 
-    auto timestamp = std::chrono::high_resolution_clock::now();
+		cv::imshow("Camera", img);
+		cv::waitKey(750);
 
-	for (;;) {
-		Mat view;
-		LOG(INFO) << "Grabbing calibration image...";
-        camera.grab();
-        camera.retrieve(view);
+		if (!found) { continue; }
 
-		if (imagePoints.size() >= (size_t)settings.nrFrames) {
-			bool r = runCalibration(settings, imageSize,
-							cameraMatrix, distCoeffs, imagePoints,
-							grid_width, release_object);
+		vector<Vec3f> points_ref;
+		calib.objectPoints(points_ref);
 
-			if (r) {
-				LOG(INFO) << "Calibration successful";
-				break;
-			}
-		}
+		Mat camera_matrix, dist_coeffs;
+		vector<Mat> rvecs, tvecs;
 		
-		imageSize = view.size();  // Format input image.
-
-		vector<Point2f> pointBuf;
-		bool found;
-
-#if CV_VERSION_MAJOR >= 4
-		LOG(INFO) << "Using findChessboardCornersSB()";
-		found = findChessboardCornersSB(view, settings.boardSize,
-				pointBuf, chessBoardFlags);
-#else
-		LOG(INFO) << "Using findChessboardCorners()";
-		found = findChessboardCorners(view, settings_.boardSize,
-				pointBuf, chessBoardFlags);
-#endif
-
-        auto now = std::chrono::high_resolution_clock::now();
-        std::chrono::duration<double> elapsed = now - timestamp;
-
-		if (found && elapsed.count() > settings.delay) {
-			timestamp = now;
-
-#if CV_VERSION_MAJOR >=4
-			// findChessboardCornersSB() does not require subpixel step.
-#else
-			// improve the found corners' coordinate accuracy for chessboard.
-			Mat viewGray;
-			cvtColor(view, viewGray, COLOR_BGR2GRAY);
-			cornerSubPix(viewGray, pointBuf, Size(winSize, winSize),
-				Size(-1, -1), TermCriteria(
-					TermCriteria::EPS+TermCriteria::COUNT, 30, 0.0001));
-#endif
-			imagePoints.push_back(pointBuf);
-        }
+		/* slow
+		double rms = cv::calibrateCamera(
+							vector<vector<Vec3f>> { points_ref }, 
+							vector<vector<Vec2f>> { points },
+							image_size, camera_matrix, dist_coeffs,
+							rvecs, tvecs, calibrate_flags
+		);
 		
-        if (found) {
-			// Draw the corners.
-			drawChessboardCorners(view, settings.boardSize,
-					Mat(pointBuf), found);
-        } else {
-            LOG(WARNING) << "No calibration pattern found";
+		LOG(INFO) << "rms for pattern: " << rms;
+		if (rms > max_error) {
+			LOG(WARNING) << "RMS reprojection error too high, maximum allowed error: " << max_error;
+			continue;
 		}
+		*/
 	
-		imshow("Left", view);
-		char key = static_cast<char>(waitKey(10));
+		image_points.push_back(points);
+		object_points.push_back(points_ref);
 
-		if (key  == 27)
-			break;
+		iter--;
 	}
 	
-	return true;
-}
+	Mat camera_matrix, dist_coeffs;
+	vector<Mat> rvecs, tvecs;
 	
-inline bool hasOption(const map<string, string> &options, const std::string &opt) {
-    return options.find(opt) != options.end();
-}
+	double rms = cv::calibrateCamera(
+						object_points, image_points,
+						image_size, camera_matrix, dist_coeffs,
+						rvecs, tvecs, calibrate_flags
+	);
 
-inline std::string getOption(map<string, string> &options, const std::string &opt) {
-    auto str = options[opt];
-    return str.substr(1,str.size()-2);
-}
+	LOG(INFO) << "final reprojection RMS error: " << rms;
 		
-void ftl::calibration::intrinsic(map<string, string> &opt) {
-    using namespace cv;
-
-    Settings settings;
-    vector<vector<Point2f>> imagePoints;
-	Mat cameraMatrix;
-    Mat distCoeffs;
-    Size imageSize;
-
-    settings.squareSize = 36;
-    settings.boardSize = Size(9,6);
-    settings.nrFrames = 30;
-    settings.delay = 0.5f;
-    settings.outputfile = "./panasonic.yml";
-    settings.device = 0;
-    settings.useFisheye = false;
-    settings.aspectRatio = 1.0;
-
-    if (hasOption(opt, "output")) settings.outputfile = getOption(opt,"output");
-    if (hasOption(opt, "device")) settings.device = std::stoi(opt["device"]);
-
-    int flag = 0;
-    flag |= CALIB_FIX_PRINCIPAL_POINT;
-    flag |= CALIB_ZERO_TANGENT_DIST;
-    flag |= CALIB_FIX_ASPECT_RATIO;
-    //flag |= CALIB_FIX_K1;
-    //flag |= CALIB_FIX_K2;
-    //flag |= CALIB_FIX_K3;
-    flag |= CALIB_FIX_K4;
-    flag |= CALIB_FIX_K5;
-
-    settings.flag = flag;
-
-    if (recalibrate(settings, imagePoints, cameraMatrix, distCoeffs, imageSize)) {
-        // Yay, calibrated. Write out...
-        // save intrinsic parameters
-		FileStorage fs(settings.outputfile, FileStorage::WRITE);
-		if (fs.isOpened()) {
-		    fs << "M" << cameraMatrix << "D" << distCoeffs;
-		    fs.release();
-		} else {
-		    LOG(ERROR) << "Error: can not save the intrinsic parameters to '" << settings.outputfile << "'";
-		}
-    } else {
-        // Nay, failed
-    }
+	saveIntrinsics(filename_intrinsics, camera_matrix, dist_coeffs);
+	LOG(INFO) << "intrinsic paramaters saved to: " << filename_intrinsics;
 	
 	Mat map1, map2;
-	initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat::eye(3,3, CV_64F), cameraMatrix, imageSize, CV_16SC2,
-    		map1, map2);
-
-	VideoCapture camera(settings.device);
-    if (!camera.isOpened()) {
-        LOG(ERROR) << "Could not open camera device";
-        return;
-    }
+	cv::initUndistortRectifyMap(camera_matrix, dist_coeffs, Mat::eye(3,3, CV_64F), camera_matrix,
+								image_size, CV_16SC2, map1, map2);
 
-    camera.set(cv::CAP_PROP_FRAME_WIDTH, 1280);  // TODO Use settings
-	camera.set(cv::CAP_PROP_FRAME_HEIGHT, 720);
+	while (cv::waitKey(25) != 27) {
+		Mat img, img_out;
 		
-	while (true) {
-		Mat img, img2;
 		camera.grab();
 		camera.retrieve(img);
-		remap(img, img2, map1, map2, INTER_LINEAR);
-
-		imshow("Left", img2);
-
-		char key = static_cast<char>(waitKey(20));
+		cv::remap(img, img_out, map1, map2, cv::INTER_CUBIC);
 
-		if (key  == 27)
-			break;
+		cv::imshow("Camera", img_out);
 	}
 }

applications/calibration/src/main.cpp

 #include <loguru.hpp>
 #include <ftl/configuration.hpp>
+
 #include "lens.hpp"
 #include "stereo.hpp"
 #include "align.hpp"

applications/calibration/src/stereo.cpp

@@ -5,6 +5,7 @@
 #include <opencv2/videoio.hpp>
 #include <opencv2/highgui.hpp>
 
+#include "common.hpp"
 #include "stereo.hpp"
 
 using std::vector;
@@ -19,71 +20,24 @@ using cv::stereoCalibrate;
 
 using namespace ftl::calibration;
 
-inline std::string getOption(map<string, string> &options, const std::string &opt) {
-    auto str = options[opt];
-    return str.substr(1,str.size()-2);
-}
-
-inline bool hasOption(const map<string, string> &options, const std::string &opt) {
-	return options.find(opt) != options.end();
-}
-
-bool loadIntrinsics(const string &ifile, Mat &M1, Mat &D1) {
-	using namespace cv;
-
-	FileStorage fs;
-
-	// reading intrinsic parameters
-	fs.open((ifile).c_str(), FileStorage::READ);
-	if (!fs.isOpened()) {
-		LOG(WARNING) << "Could not open intrinsics file : " << ifile;
-		return false;
-	}
-	
-	LOG(INFO) << "Intrinsics from: " << ifile;
-
-	fs["M"] >> M1;
-	fs["D"] >> D1;
-
-	return true;
-}
-
-CalibrationChessboard::CalibrationChessboard(const map<string, string> &opt) {
-	pattern_size_ = Size(9, 6);
-	image_size_ = Size(1280, 720);
-	pattern_square_size_ = 0.036;
-	// CALIB_CB_NORMALIZE_IMAGE | CALIB_CB_EXHAUSTIVE | CALIB_CB_ACCURACY 
-	chessboard_flags_ = cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_ACCURACY;
-}
-
-void CalibrationChessboard::objectPoints(vector<Vec3f> &out) {
-	out.reserve(pattern_size_.width * pattern_size_.height);
-	for (int row = 0; row < pattern_size_.width; ++row) {
-	for (int col = 0; col < pattern_size_.height; ++col) {
-		out.push_back(Vec3f(row * pattern_square_size_, col * pattern_square_size_, 0));
-	}}
-}
-
-bool CalibrationChessboard::findPoints(Mat &img, vector<Vec2f> &points) {
-	return cv::findChessboardCornersSB(img, pattern_size_, points, chessboard_flags_);
-}
-
-void CalibrationChessboard::drawPoints(Mat &img, const vector<Vec2f> &points) {
-	cv::drawChessboardCorners(img, pattern_size_, points, true);
-}
-
 void ftl::calibration::stereo(map<string, string> &opt) {
 	LOG(INFO) << "Begin stereo calibration";
-	CalibrationChessboard calib(opt);
-
-	Mat M1, D1;
 
+	// TODO PARAMETERS TO CONFIG FILE
 	Size image_size = Size(1280, 720);
-	int iter = 35;
+	int iter = 30;
+	double max_error = 1.0;
+	float alpha = 0;
+	string filename_intrinsics = (hasOption(opt, "profile")) ? getOption(opt, "profile") : "./panasonic.yml";
+	CalibrationChessboard calib(opt); // TODO paramters hardcoded in constructor
+	// PARAMETERS
+
+	int stereocalibrate_flags =
+		cv::CALIB_FIX_INTRINSIC | cv::CALIB_FIX_PRINCIPAL_POINT | cv::CALIB_FIX_ASPECT_RATIO |
+		cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_SAME_FOCAL_LENGTH | cv::CALIB_RATIONAL_MODEL |
+		cv::CALIB_FIX_K3 | cv::CALIB_FIX_K4 | cv::CALIB_FIX_K5;
 
 	vector<cv::VideoCapture> cameras { cv::VideoCapture(0), cv::VideoCapture(1) };
-	vector<vector<vector<Vec2f>>> image_points(2);
-	vector<vector<Vec3f>> object_points;
 
 	for (auto &camera : cameras ) {
 		if (!camera.isOpened()) {
@@ -94,6 +48,17 @@ void ftl::calibration::stereo(map<string, string> &opt) {
 		camera.set(cv::CAP_PROP_FRAME_HEIGHT, image_size.height);
 	}
 
+	vector<vector<vector<Vec2f>>> image_points(2);
+	vector<vector<Vec3f>> object_points;
+	vector<Mat> dist_coeffs(2);
+	vector<Mat> camera_matrices(2);
+
+	// assume identical cameras; load intrinsic parameters
+	loadIntrinsics(filename_intrinsics, camera_matrices[0], dist_coeffs[0]);
+	loadIntrinsics(filename_intrinsics, camera_matrices[1], dist_coeffs[1]);
+	
+	Mat R, T, E, F, per_view_errors;
+
 	while (iter > 0) {
 		int res = 0;
 
@@ -103,10 +68,14 @@ void ftl::calibration::stereo(map<string, string> &opt) {
 		for (size_t i = 0; i < 2; i++) {
 			auto &camera = cameras[i];
 			auto &img = new_img[i];
-			auto &points = new_points[i];
 
 			camera.grab();
 			camera.retrieve(img);
+		}
+
+		for (size_t i = 0; i < 2; i++) {
+			auto &img = new_img[i];
+			auto &points = new_points[i];
 			
 			if (calib.findPoints(img, points)) {
 				calib.drawPoints(img, points);
@@ -115,78 +84,94 @@ void ftl::calibration::stereo(map<string, string> &opt) {
 
 			cv::imshow("Camera: " + std::to_string(i), img);
 		}
-		cv::waitKey(500);
+		cv::waitKey(750);
 
 		if (res != 2) { LOG(WARNING) << "Input not detected on all inputs"; }
 		else {
-			for (size_t i = 0; i < 2; i++) {
-				image_points[i].push_back(new_points[i]);
-			}
 			vector<Vec3f> points_ref;
 			calib.objectPoints(points_ref);
+			
+			// calculate reprojection error with single pair of images
+			// reject it if RMS reprojection error too high
+			int flags = stereocalibrate_flags;
+
+			double rms = stereoCalibrate(
+						vector<vector<Vec3f>> { points_ref }, 
+						vector<vector<Vec2f>> { new_points[0] },
+						vector<vector<Vec2f>> { new_points[1] },
+						camera_matrices[0], dist_coeffs[0],
+						camera_matrices[1], dist_coeffs[1],
+						image_size, R, T, E, F, per_view_errors,
+						flags);
+			
+			LOG(INFO) << "rms for pattern: " << rms;
+			if (rms > max_error) {
+				LOG(WARNING) << "RMS reprojection error too high, maximum allowed error: " << max_error;
+				continue;
+			}
+
 			object_points.push_back(points_ref);
+			for (size_t i = 0; i < 2; i++) {
+				image_points[i].push_back(new_points[i]);
+			}
+
 			iter--;
 		}
 	}
 
-	vector<Mat> dist_coeffs(2);
-	vector<Mat> camera_matrices(2);
-	string filename = (hasOption(opt, "profile")) ? getOption(opt, "profile") : "./panasonic.yml";
-	
-	loadIntrinsics(filename, camera_matrices[0], dist_coeffs[0]);
-	loadIntrinsics(filename, camera_matrices[1], dist_coeffs[1]);
-	Mat per_view_errors, R, T, E, F;
+	// calculate stereoCalibration using all input images (which have low enough
+	// RMS error in previous step)
 
 	double rms = stereoCalibrate(object_points,
 		image_points[0], image_points[1],
 		camera_matrices[0], dist_coeffs[0],
-		camera_matrices[0], dist_coeffs[1],
+		camera_matrices[1], dist_coeffs[1],
 		image_size, R, T, E, F, per_view_errors,
-				cv::CALIB_FIX_INTRINSIC +  cv::CALIB_FIX_ASPECT_RATIO +
-				cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_USE_INTRINSIC_GUESS +
-				cv::CALIB_SAME_FOCAL_LENGTH + cv::CALIB_RATIONAL_MODEL +
-				cv::CALIB_FIX_K3 + cv::CALIB_FIX_K4 + cv::CALIB_FIX_K5
-		//,cv::TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6)
+		stereocalibrate_flags,
+		cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 120, 1e-6)
 	);
 
-	LOG(INFO) << "extrinsic calibration RMS: " << rms;
+	LOG(INFO) << "Final extrinsic calibration RMS (reprojection error): " << rms;
 	for (int i = 0; i < per_view_errors.rows * per_view_errors.cols; i++) {
-		LOG(INFO) << "error: " << ((double*) per_view_errors.data)[i];
+		LOG(4) << "error for sample " << i << ": " << ((double*) per_view_errors.data)[i];
 	}
 
 	Mat R1, R2, P1, P2, Q;
 	cv::Rect validRoi[2];
 
+	// calculate extrinsic parameters
 	stereoRectify(
 		camera_matrices[0], dist_coeffs[0],
 		camera_matrices[1], dist_coeffs[1],
 		image_size, R, T, R1, R2, P1, P2, Q,
-		cv::CALIB_ZERO_DISPARITY, 1, image_size,
-		&validRoi[0], &validRoi[1]);
-
-	cv::FileStorage fs;
-	fs.open(FTL_LOCAL_CONFIG_ROOT "/extrinsics.yml", cv::FileStorage::WRITE);
-	if (fs.isOpened()) {
-		fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1"
-			<< P1 << "P2" << P2 << "Q" << Q;
-		fs.release();
-	} else {
-		LOG(ERROR) << "Error: can not save the extrinsic parameters";
-	}
+		cv::CALIB_ZERO_DISPARITY, alpha, image_size,
+		&validRoi[0], &validRoi[1]
+	);
 
+	saveExtrinsics(FTL_LOCAL_CONFIG_ROOT "/extrinsics.yml", R, T, R1, R2, P1, P2, Q);
+	LOG(INFO) << "Stereo camera extrinsics saved to: " << FTL_LOCAL_CONFIG_ROOT "/extrinsics.yml";
+
+	// display results
+	vector<Mat> map1(2), map2(2);
+	cv::initUndistortRectifyMap(camera_matrices[0], dist_coeffs[0], R1, P1, image_size, CV_16SC2, map1[0], map2[0]);
+	cv::initUndistortRectifyMap(camera_matrices[1], dist_coeffs[1], R2, P2, image_size, CV_16SC2, map1[1], map2[1]);
 
-	vector<Mat> mapx(2), mapy(2);
-	cv::initUndistortRectifyMap(camera_matrices[0], dist_coeffs[0], R1, P1, image_size, CV_32FC2, mapx[0], mapy[0]);
-	cv::initUndistortRectifyMap(camera_matrices[1], dist_coeffs[1], R2, P2, image_size, CV_32FC2, mapx[1], mapy[1]);
 	vector<Mat> in(2);
 	vector<Mat> out(2);
-	while(cv::waitKey(50)) {
+
+	while(cv::waitKey(50) == -1) {
 		for(size_t i = 0; i < 2; i++) {
 			auto &camera = cameras[i];
 			camera.grab();
 			camera.retrieve(in[i]);
 			cv::imshow("Camera: " + std::to_string(i), in[i]);
-			cv::remap(in[i], out[i], mapx[i], mapy[i], cv::INTER_CUBIC);
+			cv::remap(in[i], out[i], map1[i], map2[i], cv::INTER_CUBIC);
+
+			// draw lines
+			for (int r = 0; r < image_size.height; r = r+50) {
+				cv::line(out[i], cv::Point(0, r), cv::Point(image_size.width-1, r), cv::Scalar(0,0,255), 1);
+			}
+
 			cv::imshow("Camera " + std::to_string(i) + " (rectified)", out[i]);
 		}
 	}

applications/calibration/src/stereo.hpp

@@ -10,34 +10,6 @@
 namespace ftl {
 namespace calibration {
 
-class Calibration {
-public:
-	void objectPoints(std::vector<cv::Vec3f> &out);
-	bool findPoints(cv::Mat &in, std::vector<cv::Vec2f> &out);
-	void drawPoints(cv::Mat &img, const std::vector<cv::Vec2f> &points);
-};
-
-/**
- * Parameters:
- * 	- pattern size, inner corners
- * 	- square size, millimeters (TODO: meters)
- * 	- image size, pixels
- * 	- flags, ChessboardCornersSB
- */
-class CalibrationChessboard : Calibration {
-public:
-	CalibrationChessboard(const std::map<std::string, std::string> &opt);
-	void objectPoints(std::vector<cv::Vec3f> &out);
-	bool findPoints(cv::Mat &in, std::vector<cv::Vec2f> &out);
-	void drawPoints(cv::Mat &img, const std::vector<cv::Vec2f> &points);
-
-private:
-	int chessboard_flags_ = 0;
-	float pattern_square_size_;
-	cv::Size pattern_size_;
-	cv::Size image_size_;
-};
-
 void stereo(std::map<std::string, std::string> &opt);
 
 }