diff --git a/applications/calibration-multi/src/main.cpp b/applications/calibration-multi/src/main.cpp
index ebf72ab7893b8a0af9a02e79d187d1b5530347c8..bf301728e32efc26e22667f934327b5a2affd921 100644
--- a/applications/calibration-multi/src/main.cpp
+++ b/applications/calibration-multi/src/main.cpp
@@ -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);
diff --git a/applications/calibration-multi/src/multicalibrate.cpp b/applications/calibration-multi/src/multicalibrate.cpp
index 1840e68275c24d9b6ac4538ba71d4713148da63c..074e778ed8323fed3f7a8625288b37f62e43f612 100644
--- a/applications/calibration-multi/src/multicalibrate.cpp
+++ b/applications/calibration-multi/src/multicalibrate.cpp
@@ -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;
diff --git a/applications/calibration-multi/src/multicalibrate.hpp b/applications/calibration-multi/src/multicalibrate.hpp
index e739bc0d9b60d42b6ded37f51dc0a0fa9742e8b5..0168e107d69730e72bcd60790e550c19ca16439a 100644
--- a/applications/calibration-multi/src/multicalibrate.hpp
+++ b/applications/calibration-multi/src/multicalibrate.hpp
@@ -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_;
diff --git a/applications/calibration/src/common.cpp b/applications/calibration/src/common.cpp
index 49967754dc0dd4f6665db7dae9bd9c5e965e35d3..0098bba58c5e6cbcbc0b75185f4286894447f16c 100644
--- a/applications/calibration/src/common.cpp
+++ b/applications/calibration/src/common.cpp
@@ -89,7 +89,7 @@ bool loadIntrinsics(const string &ifile, vector<Mat> &K1, vector<Mat> &D1) {
LOG(INFO) << "Intrinsics from: " << ifile;
- fs["M"] >> K1;
+ fs["K"] >> K1;
fs["D"] >> D1;
return true;
diff --git a/applications/calibration/src/lens.cpp b/applications/calibration/src/lens.cpp
index 5d47043fd47fea2821ead3d0905886510a5a9a02..f793dbfc6907b79ac65dfcbcf4a921884e6ca5cc 100644
--- a/applications/calibration/src/lens.cpp
+++ b/applications/calibration/src/lens.cpp
@@ -33,12 +33,13 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
const Size image_size = Size( getOptionInt(opt, "width", 1280),
getOptionInt(opt, "height", 720));
const int n_cameras = getOptionInt(opt, "n_cameras", 2);
- const int iter = getOptionInt(opt, "iter", 60);
- const int delay = getOptionInt(opt, "delay", 750);
+ const int iter = getOptionInt(opt, "iter", 40);
+ const int delay = getOptionInt(opt, "delay", 1000);
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);
+ const bool use_guess = getOptionInt(opt, "use_guess", 1);
LOG(INFO) << "Intrinsic calibration parameters";
LOG(INFO) << " profile: " << filename_intrinsics;
@@ -49,12 +50,25 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
LOG(INFO) << " delay: " << delay;
LOG(INFO) << " aperture_width: " << aperture_width;
LOG(INFO) << " aperture_height: " << aperture_height;
+ LOG(INFO) << " use_guess: " << use_guess;
LOG(INFO) << "-----------------------------------";
- int calibrate_flags = cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_ASPECT_RATIO |
- cv::CALIB_FIX_PRINCIPAL_POINT;
+ int calibrate_flags = cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_ASPECT_RATIO;
+ if (use_guess) { calibrate_flags |= cv::CALIB_USE_INTRINSIC_GUESS; }
+ // cv::CALIB_FIX_PRINCIPAL_POINT;
// PARAMETERS
+
+ vector<Mat> camera_matrix(n_cameras), dist_coeffs(n_cameras);
+
+ if (use_guess) {
+ camera_matrix.clear();
+ vector<Mat> tmp;
+ //dist_coeffs.clear();
+ loadIntrinsics(filename_intrinsics, camera_matrix, tmp);
+ CHECK(camera_matrix.size() == n_cameras); // (camera_matrix.size() == dist_coeffs.size())
+ }
+
vector<cv::VideoCapture> cameras;
cameras.reserve(n_cameras);
for (int c = 0; c < n_cameras; c++) { cameras.emplace_back(c); }
@@ -102,8 +116,6 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
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);
diff --git a/components/rgbd-sources/src/calibrate.cpp b/components/rgbd-sources/src/calibrate.cpp
index c31fea5c5c2b7885b450a0ebcd51f6e0540dd67f..df7fac455b68d8ee326957053ae99da946aa941f 100644
--- a/components/rgbd-sources/src/calibrate.cpp
+++ b/components/rgbd-sources/src/calibrate.cpp
@@ -89,6 +89,8 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
D[1].copyTo(D2_);
}
+ fs.release();
+
CHECK(M1_.size() == Size(3, 3));
CHECK(M2_.size() == Size(3, 3));
CHECK(D1_.size() == Size(5, 1));
@@ -116,15 +118,18 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
fs["P1"] >> P1_;
fs["P2"] >> P2_;
fs["Q"] >> Q_;
+
+ fs.release();
+
img_size_ = img_size;
// TODO: normalize calibration
- double scale_x = 1.0 / 1280.0;
- double scale_y = 1.0 / 720.0;
-
+ double scale_x = ((double) img_size.width) / 1280.0;
+ double scale_y = ((double) img_size.height) / 720.0;
+
Mat scale(cv::Size(3, 3), CV_64F, 0.0);
- scale.at<double>(0, 0) = (double) img_size.width * scale_x;
- scale.at<double>(1, 1) = (double) img_size.height * scale_y;
+ scale.at<double>(0, 0) = scale_x;
+ scale.at<double>(1, 1) = scale_y;
scale.at<double>(2, 2) = 1.0;
M1_ = scale * M1_;
@@ -132,9 +137,10 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
P1_ = scale * P1_;
P2_ = scale * P2_;
- Q_.at<double>(0, 3) = Q_.at<double>(3, 2) * (double) img_size.width * scale_x;
- Q_.at<double>(1, 3) = Q_.at<double>(3, 2) * (double) img_size.height * scale_y;
- Q_.at<double>(3, 3) = Q_.at<double>(3, 3) * (double) img_size.width * scale_x;
+ Q_.at<double>(0, 3) = Q_.at<double>(0, 3) * scale_x;
+ Q_.at<double>(1, 3) = Q_.at<double>(1, 3) * scale_y;
+ Q_.at<double>(2, 3) = Q_.at<double>(2, 3) * scale_x; // TODO: scaling?
+ Q_.at<double>(3, 3) = Q_.at<double>(3, 3) * scale_x;
// cv::cuda::remap() works only with CV_32FC1
initUndistortRectifyMap(M1_, D1_, R1_, P1_, img_size_, CV_32FC1, map1.first, map2.first);