diff --git a/applications/calibration/CMakeLists.txt b/applications/calibration/CMakeLists.txt
index 70b8aa449a2e8b5aa97a9e70cac574ba626e8ba7..0efa6e95c7c66e4480e2965e0cccffae39d5cb09 100644
--- a/applications/calibration/CMakeLists.txt
+++ b/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})
diff --git a/applications/calibration/src/common.cpp b/applications/calibration/src/common.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a29bb8f76a05793526854b9f4958f9a04c4b1add
--- /dev/null
+++ b/applications/calibration/src/common.cpp
@@ -0,0 +1,111 @@
+#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);
+}
+
+}
+}
diff --git a/applications/calibration/src/common.hpp b/applications/calibration/src/common.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..4bc3d1e4b8c5e7f734821cce9b5af7b4f05a2605
--- /dev/null
+++ b/applications/calibration/src/common.hpp
@@ -0,0 +1,82 @@
+#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_
diff --git a/applications/calibration/src/lens.cpp b/applications/calibration/src/lens.cpp
index e7fe47b758e62267f0932594cd563ad2bc3ff4e0..afc54fd96def762cb61ff4f3ea9da5e0c087e5d7 100644
--- a/applications/calibration/src/lens.cpp
+++ b/applications/calibration/src/lens.cpp
@@ -1,3 +1,4 @@
+#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;
-};
+using namespace ftl::calibration;
-//! [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();
-
- 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;
-}
-
-//! [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;
-
- 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;
-}
-
-static bool recalibrate(const Settings &settings, vector<vector<Point2f>> &imagePoints,
- Mat &cameraMatrix, Mat &distCoeffs, Size &imageSize) {
- using namespace cv;
-
- VideoCapture camera(settings.device);
- if (!camera.isOpened()) {
- LOG(ERROR) << "Could not open camera device";
- return false;
- }
+void ftl::calibration::intrinsic(map<string, string> &opt) {
+ LOG(INFO) << "Begin intrinsic calibration";
- camera.set(cv::CAP_PROP_FRAME_WIDTH, 1280); // TODO Use settings
- camera.set(cv::CAP_PROP_FRAME_HEIGHT, 720);
+ // 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
+
- 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);
+ int calibrate_flags = cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_ASPECT_RATIO;
+ // PARAMETERS
-#if CV_VERSION_MAJOR >= 4
- int chessBoardFlags = CALIB_CB_NORMALIZE_IMAGE;
-#else
- int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
+ cv::VideoCapture camera = cv::VideoCapture(0);
- if (!settings.useFisheye) {
- // fast check erroneously fails with high distortions like fisheye
- chessBoardFlags |= CALIB_CB_FAST_CHECK;
+ if (!camera.isOpened()) {
+ LOG(ERROR) << "Could not open camera device";
+ return;
}
-#endif
-
- auto timestamp = std::chrono::high_resolution_clock::now();
-
- for (;;) {
- Mat view;
- LOG(INFO) << "Grabbing calibration image...";
- camera.grab();
- camera.retrieve(view);
-
- if (imagePoints.size() >= (size_t)settings.nrFrames) {
- bool r = runCalibration(settings, imageSize,
- cameraMatrix, distCoeffs, imagePoints,
- grid_width, release_object);
-
- if (r) {
- LOG(INFO) << "Calibration successful";
- break;
- }
+ camera.set(cv::CAP_PROP_FRAME_WIDTH, image_size.width);
+ camera.set(cv::CAP_PROP_FRAME_HEIGHT, image_size.height);
+
+ vector<vector<Vec2f>> image_points;
+ vector<vector<Vec3f>> object_points;
+
+ while (iter > 0) {
+ Mat img;
+ vector<Vec2f> points;
+
+ camera.grab();
+ camera.retrieve(img);
+
+ bool found = calib.findPoints(img, points);
+ if (found) { calib.drawPoints(img, points); }
+
+ cv::imshow("Camera", img);
+ cv::waitKey(750);
+
+ if (!found) { continue; }
+
+ vector<Vec3f> points_ref;
+ calib.objectPoints(points_ref);
+
+ Mat camera_matrix, dist_coeffs;
+ vector<Mat> rvecs, tvecs;
+
+ /* slow
+ double rms = cv::calibrateCamera(
+ vector<vector<Vec3f>> { points_ref },
+ vector<vector<Vec2f>> { points },
+ image_size, camera_matrix, dist_coeffs,
+ rvecs, tvecs, calibrate_flags
+ );
+
+ LOG(INFO) << "rms for pattern: " << rms;
+ if (rms > max_error) {
+ LOG(WARNING) << "RMS reprojection error too high, maximum allowed error: " << max_error;
+ continue;
}
+ */
+
+ image_points.push_back(points);
+ object_points.push_back(points_ref);
- 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);
- }
-
- if (found) {
- // Draw the corners.
- drawChessboardCorners(view, settings.boardSize,
- Mat(pointBuf), found);
- } else {
- LOG(WARNING) << "No calibration pattern found";
- }
-
- imshow("Left", view);
- char key = static_cast<char>(waitKey(10));
-
- if (key == 27)
- break;
+ iter--;
}
-
- return true;
-}
-
-inline bool hasOption(const map<string, string> &options, const std::string &opt) {
- return options.find(opt) != options.end();
-}
-
-inline std::string getOption(map<string, string> &options, const std::string &opt) {
- auto str = options[opt];
- return str.substr(1,str.size()-2);
-}
-
-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
- }
-
+
+ Mat camera_matrix, dist_coeffs;
+ vector<Mat> rvecs, tvecs;
+
+ double rms = cv::calibrateCamera(
+ object_points, image_points,
+ image_size, camera_matrix, dist_coeffs,
+ rvecs, tvecs, calibrate_flags
+ );
+
+ LOG(INFO) << "final reprojection RMS error: " << rms;
+
+ 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);
+ cv::initUndistortRectifyMap(camera_matrix, dist_coeffs, Mat::eye(3,3, CV_64F), camera_matrix,
+ image_size, CV_16SC2, map1, map2);
- VideoCapture camera(settings.device);
- if (!camera.isOpened()) {
- LOG(ERROR) << "Could not open camera device";
- return;
- }
-
- camera.set(cv::CAP_PROP_FRAME_WIDTH, 1280); // TODO Use settings
- camera.set(cv::CAP_PROP_FRAME_HEIGHT, 720);
-
- while (true) {
- Mat img, img2;
+ while (cv::waitKey(25) != 27) {
+ Mat img, img_out;
+
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);
}
}
diff --git a/applications/calibration/src/main.cpp b/applications/calibration/src/main.cpp
index 395257f75d712b5190d459692da7d4beeea11954..698c7c9027513d8cea532f0b324f329adf5a6a08 100644
--- a/applications/calibration/src/main.cpp
+++ b/applications/calibration/src/main.cpp
@@ -1,29 +1,30 @@
#include <loguru.hpp>
#include <ftl/configuration.hpp>
+
#include "lens.hpp"
#include "stereo.hpp"
#include "align.hpp"
int main(int argc, char **argv) {
- loguru::g_preamble_date = false;
+ loguru::g_preamble_date = false;
loguru::g_preamble_uptime = false;
loguru::g_preamble_thread = false;
loguru::init(argc, argv, "--verbosity");
- argc--;
+ argc--;
argv++;
- // Process Arguments
+ // Process Arguments
auto options = ftl::config::read_options(&argv, &argc);
- if (options.find("intrinsic") != options.end()) {
- ftl::calibration::intrinsic(options);
- } else if (options.find("stereo") != options.end()) {
- ftl::calibration::stereo(options);
- } else if (options.find("align") != options.end()) {
- ftl::calibration::align(options);
- } else {
- LOG(ERROR) << "Must have one of: --intrinsic --stereo or --align";
- }
+ if (options.find("intrinsic") != options.end()) {
+ ftl::calibration::intrinsic(options);
+ } else if (options.find("stereo") != options.end()) {
+ ftl::calibration::stereo(options);
+ } else if (options.find("align") != options.end()) {
+ ftl::calibration::align(options);
+ } else {
+ LOG(ERROR) << "Must have one of: --intrinsic --stereo or --align";
+ }
- return 0;
-}
\ No newline at end of file
+ return 0;
+}
diff --git a/applications/calibration/src/stereo.cpp b/applications/calibration/src/stereo.cpp
index d43fa61b5aafaa70788909b1310fe40138b1bdc8..65b93e18d306ec00d10298755a4ef6accbab01ff 100644
--- a/applications/calibration/src/stereo.cpp
+++ b/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()) {
@@ -93,7 +47,18 @@ void ftl::calibration::stereo(map<string, string> &opt) {
camera.set(cv::CAP_PROP_FRAME_WIDTH, image_size.width);
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]);
}
}
diff --git a/applications/calibration/src/stereo.hpp b/applications/calibration/src/stereo.hpp
index 1db23f5d5fbeed7ea1ce7f8b988b0dad4306ba0e..0175ea366251fc9003d4571c97b06c9e38ad5a83 100644
--- a/applications/calibration/src/stereo.hpp
+++ b/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);
}