diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 65b620505d8ce22b9e84c5c6785b76896a83deef..da5fd16da6596671e3441b510943609e8881c50b 100644
--- a/.gitlab-ci.yml
+++ b/.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
diff --git a/CMakeLists.txt b/CMakeLists.txt
index efb6f222f6593725042c08e3d74b4a00a611ca8f..dabffe4e20021aa110011ac312973528a653252b 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/applications/calibration/CMakeLists.txt b/applications/calibration/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0efa6e95c7c66e4480e2965e0cccffae39d5cb09
--- /dev/null
+++ b/applications/calibration/CMakeLists.txt
@@ -0,0 +1,15 @@
+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})
+
+
diff --git a/applications/calibration/src/align.cpp b/applications/calibration/src/align.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bbf6f0f2e475457bb0ae3d6ee9de683deb67b105
--- /dev/null
+++ b/applications/calibration/src/align.cpp
@@ -0,0 +1,398 @@
+#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
diff --git a/applications/calibration/src/align.hpp b/applications/calibration/src/align.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..8a2097c14c9b541299fd112c1f6f279387a1aede
--- /dev/null
+++ b/applications/calibration/src/align.hpp
@@ -0,0 +1,15 @@
+#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_
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
new file mode 100644
index 0000000000000000000000000000000000000000..afc54fd96def762cb61ff4f3ea9da5e0c087e5d7
--- /dev/null
+++ b/applications/calibration/src/lens.cpp
@@ -0,0 +1,121 @@
+#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);
+ }
+}
diff --git a/applications/calibration/src/lens.hpp b/applications/calibration/src/lens.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..ec455b6bd7b3de6085074e2ea6b6ca7cca3fee9b
--- /dev/null
+++ b/applications/calibration/src/lens.hpp
@@ -0,0 +1,15 @@
+#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_
diff --git a/applications/calibration/src/main.cpp b/applications/calibration/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..698c7c9027513d8cea532f0b324f329adf5a6a08
--- /dev/null
+++ b/applications/calibration/src/main.cpp
@@ -0,0 +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_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;
+}
diff --git a/applications/calibration/src/stereo.cpp b/applications/calibration/src/stereo.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..65b93e18d306ec00d10298755a4ef6accbab01ff
--- /dev/null
+++ b/applications/calibration/src/stereo.cpp
@@ -0,0 +1,178 @@
+#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]);
+ }
+ }
+}
diff --git a/applications/calibration/src/stereo.hpp b/applications/calibration/src/stereo.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..0175ea366251fc9003d4571c97b06c9e38ad5a83
--- /dev/null
+++ b/applications/calibration/src/stereo.hpp
@@ -0,0 +1,18 @@
+#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_
diff --git a/components/common/cpp/include/ftl/configuration.hpp b/components/common/cpp/include/ftl/configuration.hpp
index 9b832753eac89b737562bcee24b3b58eba8b8bd4..9010858c4a0c268c4b7603880eef79ad2d2a974d 100644
--- a/components/common/cpp/include/ftl/configuration.hpp
+++ b/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 &);
diff --git a/components/common/cpp/src/configuration.cpp b/components/common/cpp/src/configuration.cpp
index dc6e59b977412729dd658de6fb49238b5c39cdfa..4990a35f3a3de0ad90c130fe9fe0dc56570fa73b 100644
--- a/components/common/cpp/src/configuration.cpp
+++ b/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) {
diff --git a/components/rgbd-sources/src/calibrate.cpp b/components/rgbd-sources/src/calibrate.cpp
index 7fe953c03963951ca2ebe5cb3fa1228274fc2434..4addc38e33b3dba8afaf86952818f66cb0ea1895 100644
--- a/components/rgbd-sources/src/calibrate.cpp
+++ b/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) {