diff --git a/applications/calibration/src/common.cpp b/applications/calibration/src/common.cpp
index a29bb8f76a05793526854b9f4958f9a04c4b1add..28b6f442b64d997d69e25bc473ec0eee723ddd89 100644
--- a/applications/calibration/src/common.cpp
+++ b/applications/calibration/src/common.cpp
@@ -22,16 +22,30 @@ 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);
+string getOption(const map<string, string> &options, const string &opt) {
+ const string str = options.at(opt);
+ return str.substr(1, str.size() - 2);
}
bool hasOption(const map<string, string> &options, const string &opt) {
return options.find(opt) != options.end();
}
+int getOptionInt(const map<string, string> &options, const string &opt, int default_value) {
+ if (!hasOption(options, opt)) return default_value;
+ return std::stoi(getOption(options, opt));
+}
+
+double getOptionDouble(const map<string, string> &options, const string &opt, double default_value) {
+ if (!hasOption(options, opt)) return default_value;
+ return std::stod(getOption(options, opt));
+}
+
+string getOptionString(const map<string, string> &options, const string &opt, string default_value) {
+ if (!hasOption(options, opt)) return default_value;
+ return getOption(options, opt);
+}
+
// Save/load files
bool saveExtrinsics(const string &ofile, Mat &R, Mat &T, Mat &R1, Mat &R2, Mat &P1, Mat &P2, Mat &Q) {
@@ -81,13 +95,100 @@ bool loadIntrinsics(const string &ifile, Mat &M1, Mat &D1) {
return true;
}
+Grid::Grid(int rows, int cols, int width, int height,
+ int offset_x, int offset_y) {
+ rows_ = rows;
+ cols_ = cols;
+ width_ = width;
+ height_ = height;
+ offset_x_ = offset_x;
+ offset_y_ = offset_y;
+ cell_width_ = width_ / cols_;
+ cell_height_ = height_ / rows_;
+ reset();
+
+ corners_ = vector<std::pair<cv::Point, cv::Point>>();
+
+ for (int r = 0; r < rows_; r++) {
+ for (int c = 0; c < cols_; c++) {
+ int x1 = offset_x_ + c * cell_width_;
+ int y1 = offset_y_ + r * cell_height_;
+ int x2 = offset_x_ + (c + 1) * cell_width_ - 1;
+ int y2 = offset_y_ + (r + 1) * cell_height_ - 1;
+ corners_.push_back(std::pair(cv::Point(x1, y1), cv::Point(x2, y2)));
+ }}
+}
+
+void Grid::drawGrid(Mat &rgb) {
+ for (size_t i = 0; i < rows_ * cols_; ++i) {
+ bool visited = visited_[i];
+ cv::Scalar color = visited ? cv::Scalar(24, 255, 24) : cv::Scalar(24, 24, 255);
+ cv::rectangle(rgb, corners_[i].first, corners_[i].second, color, 2);
+ }
+}
+
+int Grid::checkGrid(cv::Point p1, cv::Point p2) {
+ // TODO calculate directly
+
+ for (size_t i = 0; i < rows_ * cols_; ++i) {
+ auto &corners = corners_[i];
+ if (p1.x >= corners.first.x &&
+ p1.x <= corners.second.x &&
+ p1.y >= corners.first.y &&
+ p1.y <= corners.second.y &&
+ p2.x >= corners.first.x &&
+ p2.x <= corners.second.x &&
+ p2.y >= corners.first.y &&
+ p2.y <= corners.second.y) {
+ return i;
+ }
+ }
+
+ return -1;
+}
+
+void Grid::updateGrid(int i) {
+ if (i >= 0 && i < visited_.size() && !visited_[i]) {
+ visited_[i] = true;
+ visited_count_ += 1;
+ }
+}
+
+bool Grid::isVisited(int i) {
+ if (i >= 0 && i < visited_.size()) {
+ return visited_[i];
+ }
+ return false;
+}
+
+bool Grid::isComplete() {
+ return visited_count_ == visited_.size();
+}
+
+void Grid::reset() {
+ visited_count_ = 0;
+ visited_ = vector<bool>(rows_ * cols_, false);
+ // reset visited
+}
+
// 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
+ pattern_size_ = Size( getOptionInt(opt, "cols", 9),
+ getOptionInt(opt, "rows", 6));
+ image_size_ = Size( getOptionInt(opt, "width", 1280),
+ getOptionInt(opt, "height", 720));
+ pattern_square_size_ = getOptionDouble(opt, "square_size", 36.0);
+
+ LOG(INFO) << "Chessboard calibration parameters";
+ LOG(INFO) << " rows: " << pattern_size_.width;
+ LOG(INFO) << " cols: " << pattern_size_.height;
+ LOG(INFO) << " width: " << image_size_.width;
+ LOG(INFO) << " height: " << image_size_.height;
+ LOG(INFO) << " square_size: " << pattern_square_size_;
+ LOG(INFO) << "-----------------------------------";
+
+ // CALIB_CB_NORMALIZE_IMAGE |Â CALIB_CB_EXHAUSTIVE |Â CALIB_CB_ACCURACY
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 4bc3d1e4b8c5e7f734821cce9b5af7b4f05a2605..ad904a5eaea58afc760bdc1b97e86671182304ff 100644
--- a/applications/calibration/src/common.hpp
+++ b/applications/calibration/src/common.hpp
@@ -9,8 +9,11 @@
namespace ftl {
namespace calibration {
-std::string getOption(std::map<std::string, std::string> &options, const std::string &opt);
+std::string getOption(const 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);
+int getOptionInt(const std::map<std::string, std::string> &options, const std::string &opt, int default_value);
+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);
@@ -18,6 +21,31 @@ bool saveIntrinsics(const std::string &ofile, const cv::Mat &M1, const cv::Mat &
// 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);
+class Grid {
+private:
+ int rows_;
+ int cols_;
+ int width_;
+ int height_;
+ int cell_width_;
+ int cell_height_;
+ int offset_x_;
+ int offset_y_;
+ int visited_count_;
+
+ std::vector<std::pair<cv::Point, cv::Point>> corners_;
+ std::vector<bool> visited_;
+
+public:
+ Grid(int rows, int cols, int width, int height, int offset_x, int offset_y);
+ void drawGrid(cv::Mat &rgb);
+ int checkGrid(cv::Point p1, cv::Point p2);
+ void updateGrid(int i);
+ bool isVisited(int i);
+ bool isComplete();
+ void reset();
+};
+
/**
* @brief Wrapper for OpenCV's calibration methods. Paramters depend on
* implementation (different types of patterns).
@@ -54,11 +82,11 @@ public:
* 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
+ * All parameters (command line parameters):
+ * - rows, cols: pattern size (inner corners)
+ * - square_size: millimeters (TODO: meters)
+ * - width, height: image size, pixels
+ * - flags: see ChessboardCornersSB documentation (TODO: not implemented)
*/
class CalibrationChessboard : Calibration {
public:
diff --git a/applications/calibration/src/lens.cpp b/applications/calibration/src/lens.cpp
index afc54fd96def762cb61ff4f3ea9da5e0c087e5d7..db7fc810b46b4aef95edd7161a9d032cbe8f6cfd 100644
--- a/applications/calibration/src/lens.cpp
+++ b/applications/calibration/src/lens.cpp
@@ -28,11 +28,20 @@ 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
-
+ 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");
+ 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) << "-----------------------------------";
int calibrate_flags = cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_ASPECT_RATIO;
// PARAMETERS
@@ -60,7 +69,7 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
if (found) { calib.drawPoints(img, points); }
cv::imshow("Camera", img);
- cv::waitKey(750);
+ cv::waitKey(delay);
if (!found) { continue; }
@@ -70,21 +79,6 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
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);
diff --git a/applications/calibration/src/stereo.cpp b/applications/calibration/src/stereo.cpp
index 65b93e18d306ec00d10298755a4ef6accbab01ff..aedb197152c10f7c7d033bb9b0e62c71d9150cf0 100644
--- a/applications/calibration/src/stereo.cpp
+++ b/applications/calibration/src/stereo.cpp
@@ -24,12 +24,46 @@ 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
+ // image size, also used by CalibrationChessboard
+ Size image_size = Size( getOptionInt(opt, "width", 1280),
+ getOptionInt(opt, "height", 720));
+ // iterations
+ int iter = getOptionInt(opt, "iter", 3);
+ // delay between images
+ double delay = getOptionInt(opt, "delay", 250);
+ // max_error for a single image; if error larger image discarded
+ double max_error = getOptionDouble(opt, "max_error", 1.0);
+ // scaling/cropping (see OpenCV stereoRectify())
+ float alpha = getOptionDouble(opt, "alpha", 0);
+ // intrinsics filename
+ string filename_intrinsics = getOptionString(opt, "profile", "./panasonic.yml");
+
+ bool use_grid = (bool) getOptionInt(opt, "use_grid", 1);
+
+ LOG(INFO) << "Stereo 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) << " max_error: " << max_error;
+ LOG(INFO) << " alpha: " << alpha;
+ LOG(INFO) << " use_grid: " << use_grid;
+ LOG(INFO) << "-----------------------------------";
+
+ CalibrationChessboard calib(opt);
+ vector<Grid> grids;
+ int grid_i = 0;
+
+ // grid parameters, 3x3 grid; one small grid and one large grid. Grids are cycled until
+ // iter reaches zero
+ grids.push_back(Grid(3, 3,
+ (3.0f/4.0f) * image_size.width, (3.0f/4.0f) * image_size.height,
+ ((1.0f/4.0f) * image_size.width) / 2, ((1.0f/4.0f) * image_size.height) / 2));
+
+ grids.push_back(Grid(3, 3, image_size.width, image_size.height, 0, 0));
+ Grid grid = grids[grid_i];
+
// PARAMETERS
int stereocalibrate_flags =
@@ -48,73 +82,122 @@ void ftl::calibration::stereo(map<string, string> &opt) {
camera.set(cv::CAP_PROP_FRAME_HEIGHT, image_size.height);
}
+ // image points to calculate the parameters after all input data is captured
vector<vector<vector<Vec2f>>> image_points(2);
vector<vector<Vec3f>> object_points;
+
+ // image points for each grid, updated to image_points and object_points
+ // after is grid complete
+ vector<vector<vector<Vec2f>>> image_points_grid(9, vector<vector<Vec2f>>(2));
+ vector<vector<Vec3f>> object_points_grid(9);
+
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]);
+ if (!( loadIntrinsics(filename_intrinsics, camera_matrices[0], dist_coeffs[0]) &&
+ loadIntrinsics(filename_intrinsics, camera_matrices[1], dist_coeffs[1]))) {
+ LOG(FATAL) << "Failed to load intrinsic camera parameters from file.";
+ }
Mat R, T, E, F, per_view_errors;
-
+
+ // capture calibration patterns
while (iter > 0) {
int res = 0;
+ int grid_pos = -1;
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];
+ int delay_remaining = delay;
+ for (; delay_remaining > 50; delay_remaining -= 50) {
+ cv::waitKey(50);
- camera.grab();
- camera.retrieve(img);
+ for (size_t i = 0; i < 2; i++) {
+ auto &camera = cameras[i];
+ auto &img = new_img[i];
+
+ camera.grab();
+ camera.retrieve(img);
+
+ if (use_grid && i == 0) grid.drawGrid(img);
+ cv::imshow("Camera " + std::to_string(i), img);
+ }
}
for (size_t i = 0; i < 2; i++) {
auto &img = new_img[i];
auto &points = new_points[i];
-
+
+ // TODO move to "findPoints"-thread
if (calib.findPoints(img, points)) {
calib.drawPoints(img, points);
res++;
}
- cv::imshow("Camera: " + std::to_string(i), img);
+ 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);
+ if (res != 2) { LOG(WARNING) << "Input not detected on all inputs"; continue; }
+
+ if (use_grid) {
+ // top left and bottom right corners; not perfect but good enough
+ grid_pos = grid.checkGrid(
+ cv::Point(new_points[0][0]),
+ cv::Point(new_points[0][new_points[0].size()-1])
+ );
+
+ if (grid_pos == -1) { LOG(WARNING) << "Captured pattern not inside grid cell"; continue; }
+ }
+
+ 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;
+
+ // TODO move to "findPoints"-thread
+ double rms_iter = 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_iter;
+ if (rms_iter > max_error) {
+ LOG(WARNING) << "RMS reprojection error too high, maximum allowed error: " << max_error;
+ continue;
+ }
+
+ if (use_grid) {
+ // store results in result grid
+ object_points_grid[grid_pos] = points_ref;
+ for (size_t i = 0; i < 2; i++) { image_points_grid[grid_pos][i] = new_points[i]; }
- LOG(INFO) << "rms for pattern: " << rms;
- if (rms > max_error) {
- LOG(WARNING) << "RMS reprojection error too high, maximum allowed error: " << max_error;
- continue;
+ grid.updateGrid(grid_pos);
+
+ if (grid.isComplete()) {
+ LOG(INFO) << "Grid complete";
+ grid.reset();
+ grid_i = (grid_i + 1) % grids.size();
+ grid = grids[grid_i];
+
+ // copy results
+ object_points.insert(object_points.end(), object_points_grid.begin(), object_points_grid.end());
+ for (size_t i = 0; i < image_points_grid.size(); i++) {
+ for (size_t j = 0; j < 2; j++) { image_points[j].push_back(image_points_grid[i][j]); }
+ }
+ iter--;
}
-
+ }
+ else {
object_points.push_back(points_ref);
- for (size_t i = 0; i < 2; i++) {
- image_points[i].push_back(new_points[i]);
- }
-
+ for (size_t i = 0; i < 2; i++) { image_points[i].push_back(new_points[i]); }
iter--;
}
}
@@ -122,6 +205,11 @@ void ftl::calibration::stereo(map<string, string> &opt) {
// calculate stereoCalibration using all input images (which have low enough
// RMS error in previous step)
+ LOG(INFO) << "Calculating extrinsic stereo parameters using " << object_points.size() << " samples.";
+
+ CHECK(object_points.size() == image_points[0].size());
+ CHECK(object_points.size() == image_points[1].size());
+
double rms = stereoCalibrate(object_points,
image_points[0], image_points[1],
camera_matrices[0], dist_coeffs[0],
@@ -151,28 +239,39 @@ void ftl::calibration::stereo(map<string, string> &opt) {
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
+ for (size_t i = 0; i < 2; i++) { cv::destroyWindow("Camera " + std::to_string(i)); }
+
+ // Visualize 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);
+ // vector<Mat> out_gray(2);
+ // Mat diff, diff_color;
- while(cv::waitKey(50) == -1) {
+ while(cv::waitKey(25) == -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);
+ // cv::cvtColor(out[i], out_gray[i], cv::COLOR_BGR2GRAY);
// draw lines
- for (int r = 0; r < image_size.height; r = r+50) {
+ for (int r = 50; 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]);
}
+
+ /* not useful
+ cv::absdiff(out_gray[0], out_gray[1], diff);
+ cv::applyColorMap(diff, diff_color, cv::COLORMAP_JET);
+ cv::imshow("Difference", diff_color);
+ */
}
}