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_;