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
This commit is part of merge request !87. Comments created here will be created in the context of that merge request.
...@@ -174,8 +174,8 @@ void visualizeCalibration( MultiCameraCalibrationNew &calib, Mat &out, ...@@ -174,8 +174,8 @@ void visualizeCalibration( MultiCameraCalibrationNew &calib, Mat &out,
vector<int> markers = {cv::MARKER_SQUARE, cv::MARKER_DIAMOND}; vector<int> markers = {cv::MARKER_SQUARE, cv::MARKER_DIAMOND};
for (size_t c = 0; c < rgb.size(); c++) { 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::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) { 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); 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, ...@@ -241,7 +241,8 @@ void calibrate( MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
size_t pos1 = uri_cameras[c/2].find("node"); size_t pos1 = uri_cameras[c/2].find("node");
size_t pos2 = uri_cameras[c/2].find("#", pos1); size_t pos2 = uri_cameras[c/2].find("#", pos1);
node_name = uri_cameras[c/2].substr(pos1, pos2 - 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) { if (params.save_extrinsic) {
saveExtrinsics(params.output_path + node_name + "-extrinsic.yml", R_c1c2, T_c1c2, R1, R2, P1, P2, Q); 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"; LOG(INFO) << "Saved: " << params.output_path + node_name + "-extrinsic.yml";
...@@ -295,7 +296,7 @@ void calibrateFromPath( const string &path, ...@@ -295,7 +296,7 @@ void calibrateFromPath( const string &path,
cv::FileStorage fs(path + filename, cv::FileStorage::READ); cv::FileStorage fs(path + filename, cv::FileStorage::READ);
fs["uri"] >> uri_cameras; 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); params.idx_cameras.resize(uri_cameras.size() * 2);
std::iota(params.idx_cameras.begin(), params.idx_cameras.end(), 0); std::iota(params.idx_cameras.begin(), params.idx_cameras.end(), 0);
......
...@@ -81,16 +81,18 @@ double CalibrationTarget::estimateScale(vector<Point3d> points) { ...@@ -81,16 +81,18 @@ double CalibrationTarget::estimateScale(vector<Point3d> points) {
} }
MultiCameraCalibrationNew::MultiCameraCalibrationNew( 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), target_(target),
visibility_graph_(n_cameras), visibility_graph_(n_cameras),
is_calibrated_(false), is_calibrated_(false),
n_cameras_(n_cameras), n_cameras_(n_cameras),
reference_camera_(reference_camera), reference_camera_(reference_camera),
min_visible_points_(25), min_visible_points_(50),
fix_intrinsics_(fix_intrinsics == 1 ? 5 : 0), fix_intrinsics_(fix_intrinsics == 1 ? 5 : 0),
resolution_(resolution),
K_(n_cameras), K_(n_cameras),
dist_coeffs_(n_cameras), dist_coeffs_(n_cameras),
R_(n_cameras), R_(n_cameras),
...@@ -116,6 +118,23 @@ Mat MultiCameraCalibrationNew::getCameraMat(size_t idx) { ...@@ -116,6 +118,23 @@ Mat MultiCameraCalibrationNew::getCameraMat(size_t idx) {
return K; 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) { Mat MultiCameraCalibrationNew::getDistCoeffs(size_t idx) {
DCHECK(idx < n_cameras_); DCHECK(idx < n_cameras_);
Mat D; Mat D;
...@@ -149,6 +168,7 @@ void MultiCameraCalibrationNew::addPoints(vector<vector<Point2d>> points, vector ...@@ -149,6 +168,7 @@ void MultiCameraCalibrationNew::addPoints(vector<vector<Point2d>> points, vector
void MultiCameraCalibrationNew::reset() { void MultiCameraCalibrationNew::reset() {
is_calibrated_ = false; is_calibrated_ = false;
weights_ = vector(n_cameras_, vector(points2d_[0].size(), 0.0));
inlier_ = vector(n_cameras_, vector(points2d_[0].size(), 0)); inlier_ = vector(n_cameras_, vector(points2d_[0].size(), 0));
points3d_ = vector(n_cameras_, vector(points2d_[0].size(), Point3d())); points3d_ = vector(n_cameras_, vector(points2d_[0].size(), Point3d()));
points3d_optimized_ = vector(points2d_[0].size(), Point3d()); points3d_optimized_ = vector(points2d_[0].size(), Point3d());
...@@ -163,6 +183,7 @@ void MultiCameraCalibrationNew::saveInput(const string &filename) { ...@@ -163,6 +183,7 @@ void MultiCameraCalibrationNew::saveInput(const string &filename) {
} }
void MultiCameraCalibrationNew::saveInput(cv::FileStorage &fs) { void MultiCameraCalibrationNew::saveInput(cv::FileStorage &fs) {
fs << "resolution" << resolution_;
fs << "K" << K_; fs << "K" << K_;
fs << "points2d" << points2d_; fs << "points2d" << points2d_;
fs << "visible" << visible_; fs << "visible" << visible_;
...@@ -182,6 +203,7 @@ void MultiCameraCalibrationNew::loadInput(const std::string &filename, const vec ...@@ -182,6 +203,7 @@ void MultiCameraCalibrationNew::loadInput(const std::string &filename, const vec
fs["K"] >> K; fs["K"] >> K;
fs["points2d"] >> points2d; fs["points2d"] >> points2d;
fs["visible"] >> visible; fs["visible"] >> visible;
fs["resolution"] >> resolution_;
fs.release(); fs.release();
vector<size_t> cameras; vector<size_t> cameras;
...@@ -287,13 +309,13 @@ void MultiCameraCalibrationNew::updatePoints3D(size_t camera, Point3d new_point, ...@@ -287,13 +309,13 @@ void MultiCameraCalibrationNew::updatePoints3D(size_t camera, Point3d new_point,
// instead store all triangulations and handle outliers // instead store all triangulations and handle outliers
// (perhaps inverse variance weighted mean?) // (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 << ")"; LOG(ERROR) << "bad value (skipping) " << "(" << point << " vs " << new_point << ")";
f = -1; f = -1;
} }
else { else {
point = (point * f + new_point) / (double) (f + 1); point = (point * f + new_point) / (double) (f + 1);
f = f + 1; f++;
} }
} }
else { else {
...@@ -365,8 +387,8 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer ...@@ -365,8 +387,8 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
// cameras possibly lack line of sight? // cameras possibly lack line of sight?
DCHECK(points1.size() > 8); DCHECK(points1.size() > 8);
Mat K1 = K_[camera_from]; Mat &K1 = K_[camera_from];
Mat K2 = K_[camera_to]; Mat &K2 = K_[camera_to];
vector<uchar> inliers; vector<uchar> inliers;
Mat F, E; Mat F, E;
...@@ -462,11 +484,11 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer ...@@ -462,11 +484,11 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
// Bundle Adjustment // Bundle Adjustment
// vector<Point3d> points3d_triangulated; // vector<Point3d> points3d_triangulated;
// points3d_triangulated.insert(points3d_triangulated.begin(), points3d.begin(), points3d.end()); // points3d_triangulated.insert(points3d_triangulated.begin(), points3d.begin(), points3d.end());
LOG(INFO) << K1;
double err; double err;
cvsba::Sba sba; 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; Mat rvec1, rvec2;
cv::Rodrigues(R1, rvec1); cv::Rodrigues(R1, rvec1);
...@@ -488,8 +510,12 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer ...@@ -488,8 +510,12 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
cv::Rodrigues(r[1], R2); cv::Rodrigues(r[1], R2);
t1 = t[0]; t1 = t[0];
t2 = t[1]; 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 before BA " << sba.getInitialReprjError();
LOG(INFO) << "SBA reprojection error after BA " << err; LOG(INFO) << "SBA reprojection error after BA " << err;
} }
...@@ -498,6 +524,7 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer ...@@ -498,6 +524,7 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
// Store and average 3D points for both cameras (skip garbage) // Store and average 3D points for both cameras (skip garbage)
if (err < 10.0) { if (err < 10.0) {
Mat rmat1, tvec1;
updatePoints3D(camera_from, points3d, idx, R1, t1); updatePoints3D(camera_from, points3d, idx, R1, t1);
updatePoints3D(camera_to, points3d, idx, R2, t2); updatePoints3D(camera_to, points3d, idx, R2, t2);
} }
...@@ -507,11 +534,8 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer ...@@ -507,11 +534,8 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
<< "), not updating points!"; << "), not updating points!";
} }
// LOG(INFO) << "Final error: " << reprojectionError(points3d, points2, K2, rmat, tvec); //LOG(INFO) << reprojectionError(points3d, points1, K1, R1, t1);
//if (reprojectionError(points3d, points2, K2, rmat, tvec) > 10.0) { //LOG(INFO) << reprojectionError(points3d, points2, K2, R2, t2);
// TODO: should ignore results
// LOG(ERROR) << "pairwise calibration failed! RMS: " << reprojectionError(points3d, points2, K2, rmat, tvec);
//};
return err; return err;
} }
...@@ -598,16 +622,26 @@ double MultiCameraCalibrationNew::calibrateAll(int reference_camera) { ...@@ -598,16 +622,26 @@ double MultiCameraCalibrationNew::calibrateAll(int reference_camera) {
); );
continue; continue;
} }
LOG(INFO) << "Running pairwise calibration for cameras " << c1 << " and " << c2;
size_t n_visible = getVisiblePointsCount({c1, 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()) { if (transformations.find(make_pair(c2, c1)) != transformations.end()) {
continue; continue;
} }
Mat R, t, R_i, t_i; 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 " LOG(ERROR) << "Pairwise calibration failed, skipping cameras "
<< c1 << " and " << c2; << c1 << " and " << c2;
continue; continue;
......
...@@ -34,7 +34,8 @@ private: ...@@ -34,7 +34,8 @@ private:
class MultiCameraCalibrationNew { class MultiCameraCalibrationNew {
public: public:
MultiCameraCalibrationNew( size_t n_cameras, size_t reference_camera, 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, const Mat &distCoeffs);
void setCameraParameters(size_t idx, const Mat &K); void setCameraParameters(size_t idx, const Mat &K);
...@@ -56,6 +57,8 @@ public: ...@@ -56,6 +57,8 @@ public:
void saveInput(const std::string &filename); void saveInput(const std::string &filename);
Mat getCameraMat(size_t idx); 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); Mat getDistCoeffs(size_t idx);
double calibrateAll(int reference_camera = -1); double calibrateAll(int reference_camera = -1);
...@@ -133,6 +136,7 @@ private: ...@@ -133,6 +136,7 @@ private:
size_t min_visible_points_; size_t min_visible_points_;
int fix_intrinsics_; int fix_intrinsics_;
Size resolution_;
vector<Mat> K_; vector<Mat> K_;
vector<Mat> dist_coeffs_; vector<Mat> dist_coeffs_;
vector<Mat> R_; vector<Mat> R_;
...@@ -143,6 +147,7 @@ private: ...@@ -143,6 +147,7 @@ private:
vector<vector<Point2d>> points2d_; vector<vector<Point2d>> points2d_;
vector<vector<int>> visible_; vector<vector<int>> visible_;
vector<vector<int>> inlier_; // "inlier" vector<vector<int>> inlier_; // "inlier"
vector<vector<double>> weights_;
int fm_method_; int fm_method_;
double fm_ransac_threshold_; double fm_ransac_threshold_;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment