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

multicalibration saves resolution for intrinsic scaling

parent 58cfefa3
No related branches found
No related tags found
1 merge request!87Bug/intrinsic scaling
Pipeline #12693 passed
Stage: all
Hide whitespace changes
Inline Side-by-side
applications/calibration-multi/src/main.cpp
+ 4
− 3
View file @ dd1431d2
......@@ -174,8 +174,8 @@ void visualizeCalibration( MultiCameraCalibrationNew &calib, Mat &out,
vector<int> markers = {cv::MARKER_SQUARE, cv::MARKER_DIAMOND};
for (size_t c = 0; c < rgb.size(); c++) {
cv::rectangle(rgb[c], roi[c], Scalar(24, 224, 24), 2);
cv::remap(rgb[c], rgb[c], map1[c], map2[c], cv::INTER_CUBIC);
cv::rectangle(rgb[c], roi[c], Scalar(24, 224, 24), 2);
for (int r = 50; r < rgb[c].rows; r = r+50) {
cv::line(rgb[c], cv::Point(0, r), cv::Point(rgb[c].cols-1, r), cv::Scalar(0,0,255), 1);
......@@ -241,7 +241,8 @@ void calibrate( MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
size_t pos1 = uri_cameras[c/2].find("node");
size_t pos2 = uri_cameras[c/2].find("#", pos1);
node_name = uri_cameras[c/2].substr(pos1, pos2 - pos1);
//LOG(INFO) << c << ":" << calib.getCameraMatNormalized(c, 1280, 720);
//LOG(INFO) << c + 1 << ":" << calib.getCameraMatNormalized(c + 1, 1280, 720);
if (params.save_extrinsic) {
saveExtrinsics(params.output_path + node_name + "-extrinsic.yml", R_c1c2, T_c1c2, R1, R2, P1, P2, Q);
LOG(INFO) << "Saved: " << params.output_path + node_name + "-extrinsic.yml";
......@@ -295,7 +296,7 @@ void calibrateFromPath( const string &path,
cv::FileStorage fs(path + filename, cv::FileStorage::READ);
fs["uri"] >> uri_cameras;
//params.idx_cameras = {6, 7};
//params.idx_cameras = {2, 3};//{0, 1, 4, 5, 6, 7, 8, 9, 10, 11};
params.idx_cameras.resize(uri_cameras.size() * 2);
std::iota(params.idx_cameras.begin(), params.idx_cameras.end(), 0);
......
applications/calibration-multi/src/multicalibrate.cpp
+ 54
− 20
View file @ dd1431d2
......@@ -81,16 +81,18 @@ double CalibrationTarget::estimateScale(vector<Point3d> points) {
}
MultiCameraCalibrationNew::MultiCameraCalibrationNew(
size_t n_cameras, size_t reference_camera, CalibrationTarget target, int fix_intrinsics) :
size_t n_cameras, size_t reference_camera, Size resolution,
CalibrationTarget target, int fix_intrinsics) :
target_(target),
visibility_graph_(n_cameras),
is_calibrated_(false),
n_cameras_(n_cameras),
reference_camera_(reference_camera),
min_visible_points_(25),
min_visible_points_(50),
fix_intrinsics_(fix_intrinsics == 1 ? 5 : 0),
resolution_(resolution),
K_(n_cameras),
dist_coeffs_(n_cameras),
R_(n_cameras),
......@@ -116,6 +118,23 @@ Mat MultiCameraCalibrationNew::getCameraMat(size_t idx) {
return K;
}
Mat MultiCameraCalibrationNew::getCameraMatNormalized(size_t idx, double scale_x, double scale_y)
{
Mat K = getCameraMat(idx);
CHECK((scale_x != 0.0 && scale_y != 0.0) || ((scale_x == 0.0) && scale_y == 0.0));
scale_x = scale_x / (double) resolution_.width;
scale_y = scale_y / (double) resolution_.height;
Mat scale(Size(3, 3), CV_64F, 0.0);
scale.at<double>(0, 0) = scale_x;
scale.at<double>(1, 1) = scale_y;
scale.at<double>(2, 2) = 1.0;
return (scale * K);
}
Mat MultiCameraCalibrationNew::getDistCoeffs(size_t idx) {
DCHECK(idx < n_cameras_);
Mat D;
......@@ -149,6 +168,7 @@ void MultiCameraCalibrationNew::addPoints(vector<vector<Point2d>> points, vector
void MultiCameraCalibrationNew::reset() {
is_calibrated_ = false;
weights_ = vector(n_cameras_, vector(points2d_[0].size(), 0.0));
inlier_ = vector(n_cameras_, vector(points2d_[0].size(), 0));
points3d_ = vector(n_cameras_, vector(points2d_[0].size(), Point3d()));
points3d_optimized_ = vector(points2d_[0].size(), Point3d());
......@@ -163,6 +183,7 @@ void MultiCameraCalibrationNew::saveInput(const string &filename) {
}
void MultiCameraCalibrationNew::saveInput(cv::FileStorage &fs) {
fs << "resolution" << resolution_;
fs << "K" << K_;
fs << "points2d" << points2d_;
fs << "visible" << visible_;
......@@ -182,6 +203,7 @@ void MultiCameraCalibrationNew::loadInput(const std::string &filename, const vec
fs["K"] >> K;
fs["points2d"] >> points2d;
fs["visible"] >> visible;
fs["resolution"] >> resolution_;
fs.release();
vector<size_t> cameras;
......@@ -287,13 +309,13 @@ void MultiCameraCalibrationNew::updatePoints3D(size_t camera, Point3d new_point,
// instead store all triangulations and handle outliers
// (perhaps inverse variance weighted mean?)
if (euclideanDistance(point, new_point) > 1.0) {
if (euclideanDistance(point, new_point) > 10.0) {
LOG(ERROR) << "bad value (skipping) " << "(" << point << " vs " << new_point << ")";
f = -1;
}
else {
point = (point * f + new_point) / (double) (f + 1);
f = f + 1;
f++;
}
}
else {
......@@ -365,8 +387,8 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
// cameras possibly lack line of sight?
DCHECK(points1.size() > 8);
Mat K1 = K_[camera_from];
Mat K2 = K_[camera_to];
Mat &K1 = K_[camera_from];
Mat &K2 = K_[camera_to];
vector<uchar> inliers;
Mat F, E;
......@@ -462,12 +484,12 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
// Bundle Adjustment
// vector<Point3d> points3d_triangulated;
// points3d_triangulated.insert(points3d_triangulated.begin(), points3d.begin(), points3d.end());
LOG(INFO) << K1;
double err;
cvsba::Sba sba;
{
sba.setParams(cvsba::Sba::Params(cvsba::Sba::TYPE::MOTION, 200, 1.0e-30, fix_intrinsics_, 5, false));
sba.setParams(cvsba::Sba::Params(cvsba::Sba::TYPE::MOTIONSTRUCTURE, 200, 1.0e-30, 5, 5, false));
Mat rvec1, rvec2;
cv::Rodrigues(R1, rvec1);
cv::Rodrigues(R2, rvec2);
......@@ -488,16 +510,21 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
cv::Rodrigues(r[1], R2);
t1 = t[0];
t2 = t[1];
err = sba.getFinalReprjError();
// intrinsic parameters should only be optimized at final BA
//K1 = K[0];
//K2 = K[1];
err = sba.getFinalReprjError();
LOG(INFO) << "SBA reprojection error before BA " << sba.getInitialReprjError();
LOG(INFO) << "SBA reprojection error after BA " << err;
}
calculateTransform(R2, t2, R1, t1, rmat, tvec);
// Store and average 3D points for both cameras (skip garbage)
if (err < 10.0) {
Mat rmat1, tvec1;
updatePoints3D(camera_from, points3d, idx, R1, t1);
updatePoints3D(camera_to, points3d, idx, R2, t2);
}
......@@ -507,11 +534,8 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
<< "), not updating points!";
}
// LOG(INFO) << "Final error: " << reprojectionError(points3d, points2, K2, rmat, tvec);
//if (reprojectionError(points3d, points2, K2, rmat, tvec) > 10.0) {
// TODO: should ignore results
// LOG(ERROR) << "pairwise calibration failed! RMS: " << reprojectionError(points3d, points2, K2, rmat, tvec);
//};
//LOG(INFO) << reprojectionError(points3d, points1, K1, R1, t1);
//LOG(INFO) << reprojectionError(points3d, points2, K2, R2, t2);
return err;
}
......@@ -598,16 +622,26 @@ double MultiCameraCalibrationNew::calibrateAll(int reference_camera) {
);
continue;
}
LOG(INFO) << "Running pairwise calibration for cameras " << c1 << " and " << c2;
size_t n_visible = getVisiblePointsCount({c1, c2});
if (n_visible < min_visible_points_) continue;
if (n_visible < min_visible_points_) {
LOG(INFO) << "Not enough (" << min_visible_points_ << ") points between "
<< "cameras " << c1 << " and " << c2 << " (" << n_visible << " points), "
<< "skipping";
continue;
}
LOG(INFO) << "Running pairwise calibration for cameras "
<< c1 << " and " << c2 << "(" << n_visible << " points)";
if (transformations.find(make_pair(c2, c1)) != transformations.end()) {
continue;
}
Mat R, t, R_i, t_i;
if (calibratePair(c1, c2, R, t) > 10.0) {
// TODO: threshold parameter, 16.0 possibly too high
if (calibratePair(c1, c2, R, t) > 16.0) {
LOG(ERROR) << "Pairwise calibration failed, skipping cameras "
<< c1 << " and " << c2;
continue;
......
applications/calibration-multi/src/multicalibrate.hpp
+ 6
− 1
View file @ dd1431d2
......@@ -34,7 +34,8 @@ private:
class MultiCameraCalibrationNew {
public:
MultiCameraCalibrationNew( size_t n_cameras, size_t reference_camera,
CalibrationTarget target, int fix_intrinsics=1);
Size resolution, CalibrationTarget target,
int fix_intrinsics=1);
void setCameraParameters(size_t idx, const Mat &K, const Mat &distCoeffs);
void setCameraParameters(size_t idx, const Mat &K);
......@@ -56,6 +57,8 @@ public:
void saveInput(const std::string &filename);
Mat getCameraMat(size_t idx);
Mat getCameraMatNormalized(size_t idx, double scale_x = 1.0, double scale_y = 1.0);
Mat getDistCoeffs(size_t idx);
double calibrateAll(int reference_camera = -1);
......@@ -133,6 +136,7 @@ private:
size_t min_visible_points_;
int fix_intrinsics_;
Size resolution_;
vector<Mat> K_;
vector<Mat> dist_coeffs_;
vector<Mat> R_;
......@@ -143,6 +147,7 @@ private:
vector<vector<Point2d>> points2d_;
vector<vector<int>> visible_;
vector<vector<int>> inlier_; // "inlier"
vector<vector<double>> weights_;
int fm_method_;
double fm_ransac_threshold_;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment