Skip to content
Snippets Groups Projects
Commit 1b90993a authored by Sebastian Hahta's avatar Sebastian Hahta
Browse files

Merge branch 'feature/camera-parameters' into 'master'

calibration: cameras to use their own intrinsics

See merge request nicolas.pope/ftl!73
parents 566507ea 9163e86e
No related branches found
No related tags found
1 merge request!73calibration: cameras to use their own intrinsics
Pipeline #12427 passed
......@@ -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;
}
......
......@@ -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);
......
#include "common.hpp"
#include "lens.hpp"
#include <ftl/config.h>
#include <loguru.hpp>
#include <opencv2/core.hpp>
......@@ -28,50 +30,60 @@ 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) << " 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);
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);
}
vector<vector<Vec2f>> image_points;
vector<vector<Vec3f>> object_points;
while (iter > 0) {
Mat img;
vector<Vec2f> 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);
camera.grab();
camera.retrieve(img);
while (iter > *std::min_element(count.begin(), count.end())) {
bool found = calib.findPoints(img, points);
if (found) { calib.drawPoints(img, points); }
for (auto &camera : cameras) { camera.grab(); }
cv::imshow("Camera", img);
cv::waitKey(delay);
for (int c = 0; c < n_cameras; c++) {
vector<Vec2f> points;
cameras[c].retrieve(img[c]);
if (!found) { continue; }
if (calib.findPoints(img[c], points)) {
calib.drawPoints(img[c], points);
count[c]++;
}
else { continue; }
vector<Vec3f> points_ref;
calib.objectPoints(points_ref);
......@@ -79,37 +91,65 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
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);
}
iter--;
for (int c = 0; c < n_cameras; c++) {
cv::imshow("Camera " + std::to_string(c), img[c]);
}
Mat camera_matrix, dist_coeffs;
cv::waitKey(delay);
}
vector<Mat> camera_matrix(n_cameras), dist_coeffs(n_cameras);
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, image_points,
image_size, camera_matrix, dist_coeffs,
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]);
}
}
}
......@@ -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.";
}
......
......@@ -127,7 +127,11 @@ T *ftl::config::create(json_t &link, ARGS ...args) {
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 {
......
......@@ -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
......
......@@ -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_;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment