diff --git a/applications/calibration/src/common.cpp b/applications/calibration/src/common.cpp
index 9a44ee28ecc5241c28cc2bfed0b49b16b10a4b5e..49967754dc0dd4f6665db7dae9bd9c5e965e35d3 100644
--- a/applications/calibration/src/common.cpp
+++ b/applications/calibration/src/common.cpp
@@ -62,10 +62,10 @@ bool saveExtrinsics(const string &ofile, Mat &R, Mat &T, Mat &R1, Mat &R2, Mat &
return false;
}
-bool saveIntrinsics(const string &ofile, const Mat &M, const Mat& D) {
+bool saveIntrinsics(const string &ofile, const vector<Mat> &M, const vector<Mat>& D) {
cv::FileStorage fs(ofile, cv::FileStorage::WRITE);
if (fs.isOpened()) {
- fs << "M" << M << "D" << D;
+ fs << "K" << M << "D" << D;
fs.release();
return true;
}
@@ -75,7 +75,7 @@ bool saveIntrinsics(const string &ofile, const Mat &M, const Mat& D) {
return false;
}
-bool loadIntrinsics(const string &ifile, Mat &M1, Mat &D1) {
+bool loadIntrinsics(const string &ifile, vector<Mat> &K1, vector<Mat> &D1) {
using namespace cv;
FileStorage fs;
@@ -89,7 +89,7 @@ bool loadIntrinsics(const string &ifile, Mat &M1, Mat &D1) {
LOG(INFO) << "Intrinsics from: " << ifile;
- fs["M"] >> M1;
+ fs["M"] >> K1;
fs["D"] >> D1;
return true;
@@ -188,7 +188,14 @@ CalibrationChessboard::CalibrationChessboard(const map<string, string> &opt) {
LOG(INFO) << " square_size: " << pattern_square_size_;
LOG(INFO) << "-----------------------------------";
- // CALIB_CB_NORMALIZE_IMAGE |Â CALIB_CB_EXHAUSTIVE |Â CALIB_CB_ACCURACY
+ // From OpenCV (4.1.0) Documentation
+ //
+ // CALIB_CB_NORMALIZE_IMAGE Normalize the image gamma with equalizeHist before detection.
+ // CALIB_CB_EXHAUSTIVE Run an exhaustive search to improve detection rate.
+ // CALIB_CB_ACCURACY Up sample input image to improve sub-pixel accuracy due to
+ // aliasing effects. This should be used if an accurate camera
+ // calibration is required.
+
chessboard_flags_ = cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_ACCURACY;
}
diff --git a/applications/calibration/src/common.hpp b/applications/calibration/src/common.hpp
index ad904a5eaea58afc760bdc1b97e86671182304ff..274698b32b51ae9b741b94e25b492ab059637e37 100644
--- a/applications/calibration/src/common.hpp
+++ b/applications/calibration/src/common.hpp
@@ -15,8 +15,8 @@ int getOptionInt(const std::map<std::string, std::string> &options, const std::s
double getOptionDouble(const std::map<std::string, std::string> &options, const std::string &opt, double default_value);
std::string getOptionString(const std::map<std::string, std::string> &options, const std::string &opt, std::string default_value);
-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);
+bool loadIntrinsics(const std::string &ifile, std::vector<cv::Mat> &K, std::vector<cv::Mat> &D);
+bool saveIntrinsics(const std::string &ofile, const std::vector<cv::Mat> &K, const std::vector<cv::Mat> &D);
// 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);
diff --git a/applications/calibration/src/lens.cpp b/applications/calibration/src/lens.cpp
index db7fc810b46b4aef95edd7161a9d032cbe8f6cfd..924787a740ce46e0d4caf31c6630f38d15a02244 100644
--- a/applications/calibration/src/lens.cpp
+++ b/applications/calibration/src/lens.cpp
@@ -1,6 +1,8 @@
#include "common.hpp"
#include "lens.hpp"
+#include <ftl/config.h>
+
#include <loguru.hpp>
#include <opencv2/core.hpp>
@@ -28,88 +30,126 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
LOG(INFO) << "Begin intrinsic calibration";
// TODO PARAMETERS TO CONFIG FILE
- Size image_size = Size( getOptionInt(opt, "width", 1280),
+ const Size image_size = Size( getOptionInt(opt, "width", 1280),
getOptionInt(opt, "height", 720));
- int iter = getOptionInt(opt, "iter", 60);
- int delay = getOptionInt(opt, "delay", 750);
- string filename_intrinsics = getOptionString(opt, "profile", "./panasonic.yml");
+ const int n_cameras = getOptionInt(opt, "n_cameras", 2);
+ const int iter = getOptionInt(opt, "iter", 60);
+ const int delay = getOptionInt(opt, "delay", 750);
+ const double aperture_width = getOptionDouble(opt, "aperture_width", 6.2);
+ const double aperture_height = getOptionDouble(opt, "aperture_height", 4.6);
+ const string filename_intrinsics = getOptionString(opt, "profile", FTL_LOCAL_CONFIG_ROOT "/intrinsics.yml");
CalibrationChessboard calib(opt);
LOG(INFO) << "Intrinsic calibration parameters";
- LOG(INFO) << " profile: " << filename_intrinsics;
- LOG(INFO) << " width: " << image_size.width;
- LOG(INFO) << " height: " << image_size.height;
- LOG(INFO) << " iter: " << iter;
- LOG(INFO) << " delay: " << delay;
+ LOG(INFO) << " profile: " << filename_intrinsics;
+ LOG(INFO) << " n_cameras: " << n_cameras;
+ LOG(INFO) << " width: " << image_size.width;
+ LOG(INFO) << " height: " << image_size.height;
+ LOG(INFO) << " iter: " << iter;
+ LOG(INFO) << " delay: " << delay;
+ LOG(INFO) << " aperture_width: " << aperture_width;
+ LOG(INFO) << " aperture_height: " << aperture_height;
LOG(INFO) << "-----------------------------------";
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;
+ vector<cv::VideoCapture> cameras;
+ cameras.reserve(n_cameras);
+ for (int c = 0; c < n_cameras; c++) { cameras.emplace_back(c); }
+ 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);
}
- 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); }
+ vector<vector<vector<Vec2f>>> image_points(n_cameras);
+ vector<vector<vector<Vec3f>>> object_points(n_cameras);
+ vector<Mat> img(n_cameras);
+ vector<int> count(n_cameras, 0);
- cv::imshow("Camera", img);
- cv::waitKey(delay);
+ while (iter > *std::min_element(count.begin(), count.end())) {
- if (!found) { continue; }
+ for (auto &camera : cameras) { camera.grab(); }
- vector<Vec3f> points_ref;
- calib.objectPoints(points_ref);
+ for (int c = 0; c < n_cameras; c++) {
+ vector<Vec2f> points;
+ cameras[c].retrieve(img[c]);
+
+ if (calib.findPoints(img[c], points)) {
+ calib.drawPoints(img[c], points);
+ count[c]++;
+ }
+ else { continue; }
+
+ vector<Vec3f> points_ref;
+ calib.objectPoints(points_ref);
- Mat camera_matrix, dist_coeffs;
- vector<Mat> rvecs, tvecs;
+ Mat camera_matrix, dist_coeffs;
+ vector<Mat> rvecs, tvecs;
- image_points.push_back(points);
- object_points.push_back(points_ref);
+ image_points[c].push_back(points);
+ object_points[c].push_back(points_ref);
+ }
+
+ for (int c = 0; c < n_cameras; c++) {
+ cv::imshow("Camera " + std::to_string(c), img[c]);
+ }
- iter--;
+ cv::waitKey(delay);
}
- 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
- );
+ vector<Mat> camera_matrix(n_cameras), dist_coeffs(n_cameras);
- LOG(INFO) << "final reprojection RMS error: " << rms;
+ for (int c = 0; c < n_cameras; c++) {
+ LOG(INFO) << "Calculating intrinsic paramters for camera " << std::to_string(c);
+ vector<Mat> rvecs, tvecs;
+
+ double rms = cv::calibrateCamera(
+ object_points[c], image_points[c],
+ image_size, camera_matrix[c], dist_coeffs[c],
+ rvecs, tvecs, calibrate_flags
+ );
+
+ LOG(INFO) << "final reprojection RMS error: " << rms;
+
+ double fovx, fovy, focal_length, aspect_ratio;
+ cv::Point2d principal_point;
+
+ // TODO: check for valid aperture width/height; do not run if not valid values
+ cv::calibrationMatrixValues(camera_matrix[c], image_size, aperture_width, aperture_height,
+ fovx, fovy, focal_length, principal_point, aspect_ratio);
+ LOG(INFO) << "";
+ LOG(INFO) << " fovx (deg): " << fovx;
+ LOG(INFO) << " fovy (deg): " << fovy;
+ LOG(INFO) << " focal length (mm): " << focal_length;
+ LOG(INFO) << " principal point (mm): " << principal_point;
+ LOG(INFO) << " aspect ratio: " << aspect_ratio;
+ LOG(INFO) << "";
+ LOG(INFO) << "Camera matrix:\n" << camera_matrix[c];
+ LOG(INFO) << "Distortion coefficients:\n" << dist_coeffs[c];
+ LOG(INFO) << "";
+ }
+
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);
-
+ vector<Mat> map1(n_cameras), map2(n_cameras);
+ for (int c = 0; c < n_cameras; c++) {
+ cv::initUndistortRectifyMap(camera_matrix[c], dist_coeffs[c], Mat::eye(3,3, CV_64F), camera_matrix[c],
+ image_size, CV_16SC2, map1[c], map2[c]);
+ }
+
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);
+ for (auto &camera : cameras ) { camera.grab(); }
+ for (int c = 0; c < n_cameras; c++) {
+ cameras[c].retrieve(img[c]);
+ cv::remap(img[c], img[c], map1[c], map2[c], cv::INTER_CUBIC);
+ cv::imshow("Camera " + std::to_string(c), img[c]);
+ }
}
}
diff --git a/applications/calibration/src/stereo.cpp b/applications/calibration/src/stereo.cpp
index 59b37bc1f70601a37365099bd1cc750db9c3df5c..c009d6dd0311c27bc952e7daba18afc76e30288f 100644
--- a/applications/calibration/src/stereo.cpp
+++ b/applications/calibration/src/stereo.cpp
@@ -28,7 +28,7 @@ void ftl::calibration::stereo(map<string, string> &opt) {
Size image_size = Size( getOptionInt(opt, "width", 1280),
getOptionInt(opt, "height", 720));
// iterations
- int iter = getOptionInt(opt, "iter", 3);
+ int iter = getOptionInt(opt, "iter", 50);
// delay between images
double delay = getOptionInt(opt, "delay", 250);
// max_error for a single image; if error larger image discarded
@@ -38,7 +38,7 @@ void ftl::calibration::stereo(map<string, string> &opt) {
// intrinsics filename
string filename_intrinsics = getOptionString(opt, "profile", "./panasonic.yml");
- bool use_grid = (bool) getOptionInt(opt, "use_grid", 1);
+ bool use_grid = (bool) getOptionInt(opt, "use_grid", 0);
LOG(INFO) << "Stereo calibration parameters";
LOG(INFO) << " profile: " << filename_intrinsics;
@@ -94,9 +94,7 @@ void ftl::calibration::stereo(map<string, string> &opt) {
vector<Mat> dist_coeffs(2);
vector<Mat> camera_matrices(2);
- // assume identical cameras; load intrinsic parameters
- if (!( loadIntrinsics(filename_intrinsics, camera_matrices[0], dist_coeffs[0]) &&
- loadIntrinsics(filename_intrinsics, camera_matrices[1], dist_coeffs[1]))) {
+ if (!loadIntrinsics(filename_intrinsics, camera_matrices, dist_coeffs)) {
LOG(FATAL) << "Failed to load intrinsic camera parameters from file.";
}
diff --git a/components/common/cpp/include/ftl/configuration.hpp b/components/common/cpp/include/ftl/configuration.hpp
index 3333ed5764818f487323d688944950a11cde359f..c060b70382d0174d351d836d5476fa5a1f29db61 100644
--- a/components/common/cpp/include/ftl/configuration.hpp
+++ b/components/common/cpp/include/ftl/configuration.hpp
@@ -116,66 +116,70 @@ using config::configure;
template <typename T, typename... ARGS>
T *ftl::config::create(json_t &link, ARGS ...args) {
- //auto &r = link; // = ftl::config::resolve(link);
-
- if (!link["$id"].is_string()) {
- LOG(FATAL) << "Entity does not have $id or parent: " << link;
- return nullptr;
- }
-
- ftl::Configurable *cfg = ftl::config::find(link["$id"].get<std::string>());
- if (!cfg) {
- try {
- cfg = new T(link, args...);
- } catch(...) {
- LOG(FATAL) << "Could not construct " << link;
- }
- } else {
- // Make sure configurable has newest object pointer
- cfg->patchPtr(link);
- }
-
- try {
- return dynamic_cast<T*>(cfg);
- } catch(...) {
- LOG(FATAL) << "Configuration URI object is of wrong type: " << link;
- return nullptr;
- }
+ //auto &r = link; // = ftl::config::resolve(link);
+
+ if (!link["$id"].is_string()) {
+ LOG(FATAL) << "Entity does not have $id or parent: " << link;
+ return nullptr;
+ }
+
+ ftl::Configurable *cfg = ftl::config::find(link["$id"].get<std::string>());
+ if (!cfg) {
+ try {
+ cfg = new T(link, args...);
+ } catch (std::exception &ex) {
+ LOG(ERROR) << ex.what();
+ LOG(FATAL) << "Could not construct " << link;
+ } catch(...) {
+ LOG(ERROR) << "Unknown exception";
+ LOG(FATAL) << "Could not construct " << link;
+ }
+ } else {
+ // Make sure configurable has newest object pointer
+ cfg->patchPtr(link);
+ }
+
+ try {
+ return dynamic_cast<T*>(cfg);
+ } catch(...) {
+ LOG(FATAL) << "Configuration URI object is of wrong type: " << link;
+ return nullptr;
+ }
}
template <typename T, typename... ARGS>
T *ftl::config::create(ftl::Configurable *parent, const std::string &name, ARGS ...args) {
- nlohmann::json &entity = (!parent->getConfig()[name].is_null())
+ nlohmann::json &entity = (!parent->getConfig()[name].is_null())
? parent->getConfig()[name]
: ftl::config::resolve(parent->getConfig())[name];
- if (entity.is_object()) {
- if (!entity["$id"].is_string()) {
- std::string id_str = *parent->get<std::string>("$id");
- if (id_str.find('#') != std::string::npos) {
- entity["$id"] = id_str + std::string("/") + name;
- } else {
- entity["$id"] = id_str + std::string("#") + name;
- }
- }
-
- return create<T>(entity, args...);
- } else if (entity.is_null()) {
- // Must create the object from scratch...
- std::string id_str = *parent->get<std::string>("$id");
- if (id_str.find('#') != std::string::npos) {
- id_str = id_str + std::string("/") + name;
- } else {
- id_str = id_str + std::string("#") + name;
- }
- parent->getConfig()[name] = {
+ if (entity.is_object()) {
+ if (!entity["$id"].is_string()) {
+ std::string id_str = *parent->get<std::string>("$id");
+ if (id_str.find('#') != std::string::npos) {
+ entity["$id"] = id_str + std::string("/") + name;
+ } else {
+ entity["$id"] = id_str + std::string("#") + name;
+ }
+ }
+
+ return create<T>(entity, args...);
+ } else if (entity.is_null()) {
+ // Must create the object from scratch...
+ std::string id_str = *parent->get<std::string>("$id");
+ if (id_str.find('#') != std::string::npos) {
+ id_str = id_str + std::string("/") + name;
+ } else {
+ id_str = id_str + std::string("#") + name;
+ }
+ parent->getConfig()[name] = {
// cppcheck-suppress constStatement
- {"$id", id_str}
- };
+ {"$id", id_str}
+ };
- nlohmann::json &entity2 = parent->getConfig()[name];
- return create<T>(entity2, args...);
- }
+ nlohmann::json &entity2 = parent->getConfig()[name];
+ return create<T>(entity2, args...);
+ }
LOG(ERROR) << "Unable to create Configurable entity '" << name << "'";
return nullptr;
@@ -183,7 +187,7 @@ T *ftl::config::create(ftl::Configurable *parent, const std::string &name, ARGS
template <typename T, typename... ARGS>
std::vector<T*> ftl::config::createArray(ftl::Configurable *parent, const std::string &name, ARGS ...args) {
- nlohmann::json &base = (!parent->getConfig()[name].is_null())
+ nlohmann::json &base = (!parent->getConfig()[name].is_null())
? parent->getConfig()[name]
: ftl::config::resolve(parent->getConfig())[name];
diff --git a/components/rgbd-sources/src/calibrate.cpp b/components/rgbd-sources/src/calibrate.cpp
index cfe164baacf16ad722188aa1ecd72564a4db19f0..6a1ce062085f0833c58aa308d0579fd707aceb02 100644
--- a/components/rgbd-sources/src/calibrate.cpp
+++ b/components/rgbd-sources/src/calibrate.cpp
@@ -78,11 +78,21 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
return false;
}
- fs["M"] >> M1_;
- fs["D"] >> D1_;
+ {
+ vector<Mat> K, D;
+ fs["K"] >> K;
+ fs["D"] >> D;
+
+ K[0].copyTo(M1_);
+ K[1].copyTo(M2_);
+ D[0].copyTo(D1_);
+ D[1].copyTo(D2_);
+ }
- M2_ = M1_;
- D2_ = D1_;
+ CHECK(M1_.size() == Size(3, 3));
+ CHECK(M2_.size() == Size(3, 3));
+ CHECK(D1_.size() == Size(5, 1));
+ CHECK(D2_.size() == Size(5, 1));
auto efile = ftl::locateFile("extrinsics.yml");
if (efile) {
@@ -106,8 +116,6 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
fs["P2"] >> P2_;
fs["Q"] >> Q_;
- P_ = P1_;
-
img_size_ = img_size;
// cv::cuda::remap() works only with CV_32FC1
diff --git a/components/rgbd-sources/src/calibrate.hpp b/components/rgbd-sources/src/calibrate.hpp
index a621c3dab9c5c54020ce6bbaf3a10c50bdeb81b6..c5f4786831edb77435a4d205702e7909517829a9 100644
--- a/components/rgbd-sources/src/calibrate.hpp
+++ b/components/rgbd-sources/src/calibrate.hpp
@@ -63,7 +63,6 @@ private:
std::pair<cv::cuda::GpuMat, cv::cuda::GpuMat> map1_gpu_;
std::pair<cv::cuda::GpuMat, cv::cuda::GpuMat> map2_gpu_;
- cv::Mat P_;
cv::Mat Q_;
cv::Mat R_, T_, R1_, P1_, R2_, P2_;
cv::Mat M1_, D1_, M2_, D2_;