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

iteration count

parent eb0fcd02
No related branches found
No related tags found
1 merge request!44Feature/calibapp improvements
Pipeline #11731 passed
......@@ -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", 30);
int iter = getOptionInt(opt, "iter", 4);
// delay between images
double delay = getOptionInt(opt, "delay", 250);
// max_error for a single image; if error larger image discarded
......@@ -38,6 +38,8 @@ 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);
LOG(INFO) << "Stereo calibration parameters";
LOG(INFO) << " profile: " << filename_intrinsics;
LOG(INFO) << " width: " << image_size.width;
......@@ -46,6 +48,7 @@ void ftl::calibration::stereo(map<string, string> &opt) {
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);
......@@ -94,6 +97,7 @@ void ftl::calibration::stereo(map<string, string> &opt) {
Mat R, T, E, F, per_view_errors;
// capture calibration patterns
while (iter > 0) {
int res = 0;
int grid_pos = -1;
......@@ -102,8 +106,9 @@ void ftl::calibration::stereo(map<string, string> &opt) {
vector<vector<Vec2f>> new_points(2);
int delay_remaining = delay;
for (; delay_remaining > 50; delay_remaining -= 50) {
cv::waitKey(50);
for (; delay_remaining > 50; delay_remaining -= 10) {
for (size_t i = 0; i < 2; i++) {
auto &camera = cameras[i];
auto &img = new_img[i];
......@@ -111,11 +116,9 @@ void ftl::calibration::stereo(map<string, string> &opt) {
camera.grab();
camera.retrieve(img);
if (i == 0) grid.drawGrid(img);
if (use_grid && i == 0) grid.drawGrid(img);
cv::imshow("Camera " + std::to_string(i), img);
}
cv::waitKey(10);
}
for (size_t i = 0; i < 2; i++) {
......@@ -130,13 +133,16 @@ void ftl::calibration::stereo(map<string, string> &opt) {
cv::imshow("Camera " + std::to_string(i), img);
}
if (res != 2) { LOG(WARNING) << "Input not detected on all inputs"; }
else {
grid_pos = grid.checkGrid(cv::Point(new_points[0][0]), cv::Point(new_points[0][new_points[0].size()-1]));
if (res != 2) { LOG(WARNING) << "Input not detected on all inputs"; continue; }
if (grid_pos == -1) {
LOG(WARNING) << "Captured pattern not inside grid cell";
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;
......@@ -146,7 +152,7 @@ void ftl::calibration::stereo(map<string, string> &opt) {
// reject it if RMS reprojection error too high
int flags = stereocalibrate_flags;
double rms = stereoCalibrate(
double rms_iter = stereoCalibrate(
vector<vector<Vec3f>> { points_ref },
vector<vector<Vec2f>> { new_points[0] },
vector<vector<Vec2f>> { new_points[1] },
......@@ -155,29 +161,36 @@ void ftl::calibration::stereo(map<string, string> &opt) {
image_size, R, T, E, F, per_view_errors,
flags);
LOG(INFO) << "rms for pattern: " << rms;
if (rms > max_error) {
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]; }
grid.updateGrid(grid_pos);
LOG(INFO) << 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_grid.push_back(points_ref);
for (size_t i = 0; i < 2; i++) { image_points[i].push_back(new_points[i]); }
iter--;
}
}
......@@ -185,6 +198,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],
......@@ -214,6 +232,9 @@ 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";
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]);
......@@ -238,7 +259,6 @@ void ftl::calibration::stereo(map<string, string> &opt) {
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), in[i]);
cv::imshow("Camera " + std::to_string(i) + " (rectified)", out[i]);
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment