Resolves #76 calibration rework

.gitlab-ci.yml

@@ -44,4 +44,5 @@ windows:
     - devenv ftl.utu.fi.sln /build Release
     - rmdir /q /s "%DEPLOY_DIR%/%CI_COMMIT_REF_SLUG%"
     - mkdir "%DEPLOY_DIR%/%CI_COMMIT_REF_SLUG%"
-    - 'copy "applications\vision\Release\ftl-vision.exe" "%DEPLOY_DIR%\%CI_COMMIT_REF_SLUG%"'
\ No newline at end of file
+    - 'copy "applications\vision\Release\ftl-vision.exe" "%DEPLOY_DIR%\%CI_COMMIT_REF_SLUG%"'
+    - 'copy "applications\calibration\Release\ftl-calibrate.exe" "%DEPLOY_DIR%\%CI_COMMIT_REF_SLUG%"'
\ No newline at end of file

CMakeLists.txt

@@ -163,6 +163,7 @@ add_subdirectory(components/common/cpp)
 add_subdirectory(components/net)
 add_subdirectory(components/rgbd-sources)
 add_subdirectory(components/control/cpp)
+add_subdirectory(applications/calibration)
 
 if (BUILD_RENDERER)
 	add_subdirectory(components/renderers)

applications/calibration/CMakeLists.txt

+set(CALIBSRC
+	src/main.cpp
+	src/lens.cpp
+	src/stereo.cpp
+	src/align.cpp
+	src/common.cpp
+)
+
+add_executable(ftl-calibrate ${CALIBSRC})
+
+target_include_directories(ftl-calibrate PRIVATE src)
+
+target_link_libraries(ftl-calibrate ftlcommon Threads::Threads ${OpenCV_LIBS})
+
+

applications/calibration/src/align.cpp

+#include "align.hpp"
+
+#include <loguru.hpp>
+
+#include <opencv2/core.hpp>
+#include <opencv2/core/utility.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/videoio.hpp>
+#include <opencv2/highgui.hpp>
+
+using std::map;
+using std::string;
+using cv::Mat;
+using std::vector;
+using cv::Point2f;
+using cv::Size;
+
+struct Rec4f {
+	float left;
+	float right;
+	float top;
+	float bottom;
+};
+
+bool loadIntrinsicsMap(const std::string &ifile, const cv::Size &imageSize, Mat &map1, Mat &map2, Mat &cameraMatrix, float scale) {
+    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;
+
+
+    Mat D1;
+    fs["M"] >> cameraMatrix;
+    fs["D"] >> D1;
+
+    //cameraMatrix *= scale;
+
+	initUndistortRectifyMap(cameraMatrix, D1, Mat::eye(3,3, CV_64F), cameraMatrix, imageSize, CV_16SC2,
+    		map1, map2);
+
+    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);
+}
+
+static const float kPI = 3.14159f;
+
+static float calculateZRotation(const vector<Point2f> &points, Size &boardSize) {
+    Point2f tl = points[boardSize.width * (boardSize.height / 2)];
+    Point2f tr = points[boardSize.width * (boardSize.height / 2) + boardSize.width-1];
+
+    float dx = tr.x - tl.x;
+    float dy = tr.y - tl.y;
+    float angle = atan2(dy,  dx) * (180.0f / kPI);
+    return angle;
+}
+
+static Point2f parallaxDistortion(const vector<Point2f> &points, Size &boardSize) {
+    Point2f tl = points[0];
+    Point2f tr = points[boardSize.width-1];
+    Point2f bl = points[(boardSize.height-1)*boardSize.width];
+    Point2f br = points[points.size()-1];
+
+    float dx1 = tr.x - tl.x;
+    float dx2 = br.x - bl.x;
+    float ddx = dx1 - dx2;
+
+    float dy1 = bl.y - tl.y;
+    float dy2 = br.y - tr.y;
+    float ddy = dy1 - dy2;
+
+    return Point2f(ddx, ddy);
+}
+
+static float distanceTop(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
+    Point2f tl = points[0];
+    Point2f tr = points[boardSize.width-1];
+
+    float pixSize = tr.x - tl.x;
+    float mmSize = boardSize.width * squareSize;
+    float focal = camMatrix.at<double>(0,0);
+
+    return ((mmSize / pixSize) * focal) / 1000.0f;
+}
+
+static float distanceBottom(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
+    Point2f bl = points[(boardSize.height-1)*boardSize.width];
+    Point2f br = points[points.size()-1];
+
+    float pixSize = br.x - bl.x;
+    float mmSize = boardSize.width * squareSize;
+    float focal = camMatrix.at<double>(0,0);
+
+    return ((mmSize / pixSize) * focal) / 1000.0f;
+}
+
+static float distanceLeft(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
+    Point2f bl = points[(boardSize.height-1)*boardSize.width];
+    Point2f tl = points[0];
+
+    float pixSize = bl.y - tl.y;
+    float mmSize = boardSize.height * squareSize;
+    float focal = camMatrix.at<double>(0,0);
+
+    return ((mmSize / pixSize) * focal) / 1000.0f;
+}
+
+static float distanceRight(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
+    Point2f tr = points[boardSize.width-1];
+    Point2f br = points[points.size()-1];
+
+    float pixSize = br.y - tr.y;
+    float mmSize = boardSize.height * squareSize;
+    float focal = camMatrix.at<double>(0,0);
+
+    return ((mmSize / pixSize) * focal) / 1000.0f;
+}
+
+static Rec4f distances(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
+	return {
+		-distanceLeft(camMatrix, points, boardSize, squareSize),
+		-distanceRight(camMatrix, points, boardSize, squareSize),
+		-distanceTop(camMatrix, points, boardSize, squareSize),
+		-distanceBottom(camMatrix, points, boardSize, squareSize)
+	};
+}
+
+static float distance(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
+    Point2f tl = points[boardSize.width * (boardSize.height / 2)];
+    Point2f tr = points[boardSize.width * (boardSize.height / 2) + boardSize.width-1];
+
+    float pixSize = tr.x - tl.x;
+    float mmSize = boardSize.width * squareSize;
+    float focal = camMatrix.at<double>(0,0);
+
+    return ((mmSize / pixSize) * focal) / 1000.0f;
+}
+
+static Point2f diffY(const vector<Point2f> &pointsA, const vector<Point2f> &pointsB, Size &boardSize) {
+    Point2f tlA = pointsA[boardSize.width * (boardSize.height / 2)];
+    Point2f trA = pointsA[boardSize.width * (boardSize.height / 2) + boardSize.width-1];
+
+    Point2f tlB = pointsB[boardSize.width * (boardSize.height / 2)];
+    Point2f trB = pointsB[boardSize.width * (boardSize.height / 2) + boardSize.width-1];
+
+    float d1 = tlA.y - tlB.y;
+    float d2 = trA.y - trB.y;
+
+    return Point2f(d1,d2);
+}
+
+static const float kDistanceThreshold = 0.005f;
+
+
+static void showAnaglyph(const Mat &frame_l, const Mat &frame_r, Mat &img3d) {
+    using namespace cv;
+
+    float data[] = {0.299f, 0.587f, 0.114f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.299, 0.587, 0.114};
+    Mat m(2, 9, CV_32FC1, data);
+
+    //Mat img3d;
+
+    img3d = Mat(frame_l.size(), CV_8UC3);
+  
+    for (int y=0; y<img3d.rows; y++) {
+        unsigned char *row3d = img3d.ptr(y);
+        const unsigned char *rowL = frame_l.ptr(y);
+        const unsigned char *rowR = frame_r.ptr(y);
+
+        for (int x=0; x<img3d.cols*3; x+=3) {
+            const uchar lb = rowL[x+0];
+            const uchar lg = rowL[x+1];
+            const uchar lr = rowL[x+2];
+            const uchar rb = rowR[x+0];
+            const uchar rg = rowR[x+1];
+            const uchar rr = rowR[x+2];
+
+            row3d[x+0] = lb*m.at<float>(0,6) + lg*m.at<float>(0,7) + lr*m.at<float>(0,8) + rb*m.at<float>(1,6) + rg*m.at<float>(1,7) + rr*m.at<float>(1,8);
+            row3d[x+1] = lb*m.at<float>(0,3) + lg*m.at<float>(0,4) + lr*m.at<float>(0,5) + rb*m.at<float>(1,3) + rg*m.at<float>(1,4) + rr*m.at<float>(1,5);
+            row3d[x+2] = lb*m.at<float>(0,0) + lg*m.at<float>(0,1) + lr*m.at<float>(0,2) + rb*m.at<float>(1,0) + rg*m.at<float>(1,1) + rr*m.at<float>(1,2);
+        }
+    }
+
+    //imshow("Anaglyph", img3d);
+    //return img3d;
+}
+
+void ftl::calibration::align(map<string, string> &opt) {
+    using namespace cv;
+
+    float squareSize = 36.0f;
+
+    VideoCapture camA(0);
+    VideoCapture camB(1);
+
+    if (!camA.isOpened() || !camB.isOpened()) {
+        LOG(ERROR) << "Could not open a camera device";
+        return;
+    }
+
+    camA.set(cv::CAP_PROP_FRAME_WIDTH, 1280);  // TODO Use settings
+	camA.set(cv::CAP_PROP_FRAME_HEIGHT, 720);
+    camB.set(cv::CAP_PROP_FRAME_WIDTH, 1280);
+	camB.set(cv::CAP_PROP_FRAME_HEIGHT, 720);
+
+    Mat map1, map2, cameraMatrix;
+    Size imgSize(1280,720);
+    loadIntrinsicsMap((hasOption(opt, "profile")) ? getOption(opt,"profile") : "./panasonic.yml", imgSize, map1, map2, cameraMatrix, 1.0f);
+
+    Size boardSize(9,6);
+
+#if CV_VERSION_MAJOR >= 4
+	int chessBoardFlags = CALIB_CB_NORMALIZE_IMAGE;
+#else
+	int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
+
+	if (!settings.useFisheye) {
+		// fast check erroneously fails with high distortions like fisheye
+		chessBoardFlags |= CALIB_CB_FAST_CHECK;
+	}
+#endif
+
+    int step = 0;
+    bool anaglyph = true;
+
+    while (true) {
+        Mat frameA, fA;
+        Mat frameB, fB;
+
+        camA.grab();
+        camB.grab();
+        camA.retrieve(frameA);
+        camB.retrieve(frameB);
+
+        remap(frameA, fA, map1, map2, INTER_LINEAR);
+        remap(frameB, fB, map1, map2, INTER_LINEAR);
+
+        // Get the chessboard
+        vector<Point2f> pointBufA;
+        vector<Point2f> pointBufB;
+		bool foundA, foundB;
+        foundA = findChessboardCornersSB(fA, boardSize,
+				pointBufA, chessBoardFlags);
+        foundB = findChessboardCornersSB(fB, boardSize,
+				pointBufB, chessBoardFlags);
+
+    // Step 1: Position cameras correctly with respect to chessboard
+    //      - print distance estimate etc
+
+    // Step 2: Show individual camera tilt degrees with left / right indicators
+
+    // Show also up down tilt perspective error
+
+    // Step 3: Display current baseline in mm
+
+        if (foundA) {
+            // Draw the corners.
+			//drawChessboardCorners(fA, boardSize,
+			//		Mat(pointBufA), foundA);
+        }
+
+        if (foundB) {
+            // Draw the corners.
+			//drawChessboardCorners(fB, boardSize,
+			//		Mat(pointBufB), foundB);
+        }
+
+        Mat anag;
+        showAnaglyph(fA, fB, anag);
+
+        if (foundA) {
+			Rec4f dists = distances(cameraMatrix, pointBufA, boardSize, squareSize);
+			//Rec4f angs = angles(pointBufA, boardSize);
+
+			// TODO Check angles also...
+			bool lrValid = std::abs(dists.left-dists.right) <= kDistanceThreshold;
+			bool tbValid = std::abs(dists.top-dists.bottom) <= kDistanceThreshold;
+			bool distValid = lrValid & tbValid;
+			bool tiltUp = dists.top < dists.bottom && !tbValid;
+			bool tiltDown = dists.top > dists.bottom && !tbValid;
+			bool rotLeft = dists.left > dists.right && !lrValid;
+			bool rotRight = dists.left < dists.right && !lrValid;
+
+			float d = (dists.left + dists.right + dists.top + dists.bottom) / 4.0f;
+
+			// TODO Draw lines
+            Point2f bl = pointBufA[(boardSize.height-1)*boardSize.width];
+            Point2f tl = pointBufA[0];
+            Point2f tr = pointBufA[boardSize.width-1];
+            Point2f br = pointBufA[pointBufA.size()-1];
+
+
+            line(anag, tl, tr, (!lrValid && tiltUp) ? Scalar(0,0,255) : Scalar(0,255,0));
+            line(anag, bl, br, (!lrValid && tiltDown) ? Scalar(0,0,255) : Scalar(0,255,0));
+            line(anag, tl, bl, (!tbValid && rotLeft) ? Scalar(0,0,255) : Scalar(0,255,0));
+            line(anag, tr, br, (!tbValid && rotRight) ? Scalar(0,0,255) : Scalar(0,255,0));
+
+            //fA = fA(Rect(tl.x - 100, tl.y- 100, br.x-tl.x + 200, br.y-tl.y + 200));
+
+			// Show distance error between cameras
+
+			// Show estimated baseline
+
+            //if (step == 0) {
+            //    Point2f pd = parallaxDistortion(pointBufA, boardSize);
+            //    putText(fA, string("Distort: ") + std::to_string(pd.x) + string(",") + std::to_string(pd.y), Point(10,50), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
+            //} else if (step == 1) {
+                //float d = distance(cameraMatrix, pointBufA, boardSize, squareSize);
+                //putText(anag, string("Distance: ") + std::to_string(-d) + string("m"), Point(10,50), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
+           // } else if (step == 2) {
+                //float angle = calculateZRotation(pointBufA, boardSize) - 180.0f;
+                //putText(anag, string("Angle: ") + std::to_string(angle), Point(10,150), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
+            //} else if (step == 3) {
+            //    Point2f vd = diffY(pointBufA, pointBufB, boardSize);
+            //    putText(fA, string("Vertical: ") + std::to_string(vd.x) + string(",") + std::to_string(vd.y), Point(10,200), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
+           // }
+
+            if (foundB) {
+                //if (step == 0) {
+                    //Point2f pd = parallaxDistortion(pointBufB, boardSize);
+                    //putText(fB, string("Distort: ") + std::to_string(pd.x) + string(",") + std::to_string(pd.y), Point(10,50), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
+                //} else if (step == 1) {
+                    //float d = distance(cameraMatrix, pointBufB, boardSize, squareSize);
+                    //putText(fB, string("Distance: ") + std::to_string(-d) + string("m"), Point(10,100), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
+                //} else if (step == 2) {
+                    //float angle = calculateZRotation(pointBufB, boardSize) - 180.0f;
+                    //putText(fB, string("Angle: ") + std::to_string(angle), Point(10,150), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
+                //}
+
+                Rec4f dists = distances(cameraMatrix, pointBufB, boardSize, squareSize);
+                //Rec4f angs = angles(pointBufA, boardSize);
+
+                // TODO Check angles also...
+                bool lrValid = std::abs(dists.left-dists.right) <= kDistanceThreshold;
+                bool tbValid = std::abs(dists.top-dists.bottom) <= kDistanceThreshold;
+                bool distValid = lrValid & tbValid;
+                bool tiltUp = dists.top < dists.bottom && !tbValid;
+                bool tiltDown = dists.top > dists.bottom && !tbValid;
+                bool rotLeft = dists.left > dists.right && !lrValid;
+                bool rotRight = dists.left < dists.right && !lrValid;
+
+                float d = (dists.left + dists.right + dists.top + dists.bottom) / 4.0f;
+
+                // TODO Draw lines
+                Point2f bbl = pointBufB[(boardSize.height-1)*boardSize.width];
+                Point2f btl = pointBufB[0];
+                Point2f btr = pointBufB[boardSize.width-1];
+                Point2f bbr = pointBufB[pointBufB.size()-1];
+
+
+                line(anag, btl, btr, (!lrValid && tiltUp) ? Scalar(0,0,255) : Scalar(0,255,0));
+                line(anag, bbl, bbr, (!lrValid && tiltDown) ? Scalar(0,0,255) : Scalar(0,255,0));
+                line(anag, btl, bbl, (!tbValid && rotLeft) ? Scalar(0,0,255) : Scalar(0,255,0));
+                line(anag, btr, bbr, (!tbValid && rotRight) ? Scalar(0,0,255) : Scalar(0,255,0));
+
+                float baseline1 = std::abs(tl.x - btl.x);
+                float baseline2 = std::abs(tr.x - btr.x);
+                float baseline3 = std::abs(bl.x - bbl.x);
+                float baseline4 = std::abs(br.x - bbr.x);
+                float boardWidth = (std::abs(tl.x-tr.x) + std::abs(btl.x - btr.x)) / 2.0f;
+                float baseline = ((baseline1 + baseline2 + baseline3 + baseline4) / 4.0f) / boardWidth * (boardSize.width*squareSize);
+
+                putText(anag, string("Baseline: ") + std::to_string(baseline) + string("mm"), Point(10,150), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,255,0), 2);
+            }
+
+        }
+
+        /*if (anaglyph) {
+            showAnaglyph(fA,fB);
+        } else {
+            imshow("Left", fA);
+            imshow("Right", fB);
+        }*/
+        imshow("Anaglyph", anag);
+
+		char key = static_cast<char>(waitKey(20));
+		if (key  == 27)
+			break;
+        if (key == 32) anaglyph = !anaglyph;
+    }
+}
\ No newline at end of file

applications/calibration/src/align.hpp

+#ifndef _FTL_CALIBRATION_ALIGN_HPP_
+#define _FTL_CALIBRATION_ALIGN_HPP_
+
+#include <map>
+#include <string>
+
+namespace ftl {
+namespace calibration {
+
+void align(std::map<std::string, std::string> &opt);
+
+}
+}
+
+#endif  // _FTL_CALIBRATION_ALIGN_HPP_

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>
+
+#include <opencv2/core.hpp>
+#include <opencv2/core/utility.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/videoio.hpp>
+#include <opencv2/highgui.hpp>
+
+#include <vector>
+
+using std::map;
+using std::string;
+using std::vector;
+
+using cv::Mat;
+using cv::Vec2f;
+using cv::Vec3f;
+using cv::Size;
+
+using namespace ftl::calibration;
+
+void ftl::calibration::intrinsic(map<string, string> &opt) {
+	LOG(INFO) << "Begin intrinsic calibration";
+
+	// 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
+	
+
+	int calibrate_flags = cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_ASPECT_RATIO;
+	// PARAMETERS
+
+	cv::VideoCapture camera = cv::VideoCapture(0);
+
+	if (!camera.isOpened()) {
+		LOG(ERROR) << "Could not open camera device";
+		return;
+	}
+	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);
+
+		iter--;
+	}
+	
+	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;
+	cv::initUndistortRectifyMap(camera_matrix, dist_coeffs, Mat::eye(3,3, CV_64F), camera_matrix,
+								image_size, CV_16SC2, map1, map2);
+
+	while (cv::waitKey(25) != 27) {
+		Mat img, img_out;
+		
+		camera.grab();
+		camera.retrieve(img);
+		cv::remap(img, img_out, map1, map2, cv::INTER_CUBIC);
+
+		cv::imshow("Camera", img_out);
+	}
+}

applications/calibration/src/lens.hpp

+#ifndef _FTL_CALIBRATION_LENS_HPP_
+#define _FTL_CALIBRATION_LENS_HPP_
+
+#include <map>
+#include <string>
+
+namespace ftl {
+namespace calibration {
+
+void intrinsic(std::map<std::string, std::string> &opt);
+
+}
+}
+
+#endif  // _FTL_CALIBRATION_LENS_HPP_

applications/calibration/src/main.cpp

+#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_uptime = false;
+	loguru::g_preamble_thread = false;
+	loguru::init(argc, argv, "--verbosity");
+	argc--;
+	argv++;
+
+	// 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";
+	}
+
+	return 0;
+}

applications/calibration/src/stereo.cpp

+#include <loguru.hpp>
+#include <ftl/config.h>
+
+#include <opencv2/imgproc.hpp>
+#include <opencv2/videoio.hpp>
+#include <opencv2/highgui.hpp>
+
+#include "common.hpp"
+#include "stereo.hpp"
+
+using std::vector;
+using std::map;
+using std::string;
+
+using cv::Mat;
+using cv::Vec2f, cv::Vec3f;
+using cv::Size;
+
+using cv::stereoCalibrate;
+
+using namespace ftl::calibration;
+
+void ftl::calibration::stereo(map<string, string> &opt) {
+	LOG(INFO) << "Begin stereo calibration";
+
+	// TODO PARAMETERS TO CONFIG FILE
+	Size image_size = Size(1280, 720);
+	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) };
+
+	for (auto &camera : cameras ) {
+		if (!camera.isOpened()) {
+			LOG(ERROR) << "Could not open camera device";
+			return;
+		}
+		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;
+
+		vector<Mat> new_img(2);
+		vector<vector<Vec2f>> new_points(2);
+
+		for (size_t i = 0; i < 2; i++) {
+			auto &camera = cameras[i];
+			auto &img = new_img[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);
+				res++;
+			}
+
+			cv::imshow("Camera: " + std::to_string(i), img);
+		}
+		cv::waitKey(750);
+
+		if (res != 2) { LOG(WARNING) << "Input not detected on all inputs"; }
+		else {
+			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--;
+		}
+	}
+
+	// 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[1], dist_coeffs[1],
+		image_size, R, T, E, F, per_view_errors,
+		stereocalibrate_flags,
+		cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 120, 1e-6)
+	);
+
+	LOG(INFO) << "Final extrinsic calibration RMS (reprojection error): " << rms;
+	for (int i = 0; i < per_view_errors.rows * per_view_errors.cols; 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, 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> in(2);
+	vector<Mat> out(2);
+
+	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], 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

+#ifndef _FTL_CALIBRATION_STEREO_HPP_
+#define _FTL_CALIBRATION_STEREO_HPP_
+
+#include <map>
+#include <string>
+
+#include <opencv2/core.hpp>
+#include <opencv2/calib3d.hpp>
+
+namespace ftl {
+namespace calibration {
+
+void stereo(std::map<std::string, std::string> &opt);
+
+}
+}
+
+#endif  // _FTL_CALIBRATION_STEREO_HPP_

components/common/cpp/include/ftl/configuration.hpp

@@ -29,6 +29,8 @@ typedef nlohmann::json json_t;
 
 std::optional<std::string> locateFile(const std::string &name);
 
+std::map<std::string, std::string> read_options(char ***argv, int *argc);
+
 Configurable *configure(int argc, char **argv, const std::string &root);
 
 Configurable *configure(json_t &);

components/common/cpp/src/configuration.cpp

@@ -353,7 +353,7 @@ static bool findConfiguration(const string &file, const vector<string> &paths) {
 /**
  * Generate a map from command line option to value
  */
-static map<string, string> read_options(char ***argv, int *argc) {
+map<string, string> ftl::config::read_options(char ***argv, int *argc) {
 	map<string, string> opts;
 
 	while (*argc > 0) {
@@ -461,7 +461,7 @@ Configurable *ftl::config::configure(int argc, char **argv, const std::string &r
 #endif  // WIN32
 
 	// Process Arguments
-	auto options = read_options(&argv, &argc);
+	auto options = ftl::config::read_options(&argv, &argc);
 	
 	vector<string> paths;
 	while (argc-- > 0) {

components/rgbd-sources/src/calibrate.cpp

@@ -219,7 +219,7 @@ bool Calibrate::_loadCalibration() {
     FileStorage fs;
 
     // reading intrinsic parameters
-    auto ifile = ftl::locateFile("intrinsics.yml");
+    auto ifile = ftl::locateFile(value("intrinsics", std::string("intrinsics.yml")));
     if (ifile) {
 		fs.open((*ifile).c_str(), FileStorage::READ);
 		if (!fs.isOpened()) {
@@ -234,13 +234,16 @@ bool Calibrate::_loadCalibration() {
     }
 
     Mat M1, D1, M2, D2;
-    fs["M1"] >> M1;
-    fs["D1"] >> D1;
-    fs["M2"] >> M2;
-    fs["D2"] >> D2;
+    fs["M"] >> M1;
+    fs["D"] >> D1;
+    //fs["M2"] >> M2;
+    //fs["D2"] >> D2;
 
     M1 *= scale;
-    M2 *= scale;
+    //M2 *= scale;
+
+	M2 = M1;
+	D2 = D1;
 
 	auto efile = ftl::locateFile("extrinsics.yml");
 	if (efile) {