From 8ef415f479dd782e932c98c7d13c5a056c77a79b Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Fri, 14 Feb 2020 16:29:07 +0200
Subject: [PATCH] use distortion parameters
---
.../calibration-ceres/src/calibration.cpp | 4 ++-
.../src/calibration_data.cpp | 6 ++---
.../src/calibration_data.hpp | 13 +++++++++-
applications/calibration-ceres/src/main.cpp | 16 ++++++------
.../calibration-ceres/src/optimization.cpp | 9 ++++++-
.../calibration-ceres/src/optimization.hpp | 3 ++-
applications/calibration-multi/src/main.cpp | 26 ++++++++++++++++++-
.../calibration-multi/src/multicalibrate.cpp | 7 ++---
.../common/cpp/include/ftl/calibration.hpp | 8 ++++++
components/common/cpp/src/calibration.cpp | 4 +--
10 files changed, 75 insertions(+), 21 deletions(-)
diff --git a/applications/calibration-ceres/src/calibration.cpp b/applications/calibration-ceres/src/calibration.cpp
index 1f468bb0f..2c09538e3 100644
--- a/applications/calibration-ceres/src/calibration.cpp
+++ b/applications/calibration-ceres/src/calibration.cpp
@@ -142,7 +142,9 @@ double ftl::calibration::computeExtrinsicParameters(const Mat &K1, const Mat &D1
double error_after = ba.reprojectionError();
// bundle adjustment didn't work correctly if these checks fail
- CHECK(error_before > error_after);
+ if (error_before < error_after) {
+ LOG(WARNING) << "error before < error_after (" << error_before << " <" << error_after << ")";
+ }
CHECK((cv::countNonZero(params1.rvec()) == 0) && (cv::countNonZero(params1.tvec()) == 0));
R = params2.rmat();
diff --git a/applications/calibration-ceres/src/calibration_data.cpp b/applications/calibration-ceres/src/calibration_data.cpp
index 02ede5ce6..28b77fa8b 100644
--- a/applications/calibration-ceres/src/calibration_data.cpp
+++ b/applications/calibration-ceres/src/calibration_data.cpp
@@ -180,7 +180,7 @@ int CalibrationData::addObservations(const vector<bool>& visible, const vector<v
return retval;
}
-pair<vector<vector<Point2d>>, vector<reference_wrapper<Point3d>>> CalibrationData::getVisible(const vector<int> &cameras) {
+pair<vector<vector<Point2d>>, vector<reference_wrapper<Point3d>>> CalibrationData::_getVisible(const vector<int> &cameras) {
int n_points = npoints();
vector<vector<Point2d>> observations(cameras.size());
@@ -204,9 +204,9 @@ pair<vector<vector<Point2d>>, vector<reference_wrapper<Point3d>>> CalibrationDat
}
vector<vector<Point2d>> CalibrationData::getObservations(const vector<int> &cameras) {
- return getVisible(cameras).first;
+ return _getVisible(cameras).first;
}
vector<reference_wrapper<Point3d>> CalibrationData::getPoints(const vector<int> &cameras) {
- return getVisible(cameras).second;
+ return _getVisible(cameras).second;
}
diff --git a/applications/calibration-ceres/src/calibration_data.hpp b/applications/calibration-ceres/src/calibration_data.hpp
index da4c00eaf..e98ed3d3c 100644
--- a/applications/calibration-ceres/src/calibration_data.hpp
+++ b/applications/calibration-ceres/src/calibration_data.hpp
@@ -77,11 +77,14 @@ public:
void reserve(int n_points) {
points_.reserve(n_points);
+ points_camera_.reserve(n_points*n_cameras_);
observations_.reserve(n_points*n_cameras_);
visible_.reserve(n_points*n_cameras_);
}
- std::pair<std::vector<std::vector<cv::Point2d>>, std::vector<std::reference_wrapper<cv::Point3d>>> getVisible(const std::vector<int> &cameras);
+ // TODO: method for save poinst_camera_, could return (for example)
+ // vector<pair<Point3d*>, vector<Point3d*>>
+
std::vector<std::vector<cv::Point2d>> getObservations(const std::vector<int> &cameras);
/* Get points corresponding to observations returned by getObservations() or getObservationsPtr() */
std::vector<std::reference_wrapper<cv::Point3d>> getPoints(const std::vector<int> &cameras);
@@ -101,11 +104,19 @@ public:
int ncameras() const { return n_cameras_; }
private:
+ std::pair<std::vector<std::vector<cv::Point2d>>, std::vector<std::reference_wrapper<cv::Point3d>>> _getVisible(const std::vector<int> &cameras);
+
int n_cameras_;
+ // cameras
std::vector<Camera> cameras_;
+ // points for each observation
std::vector<cv::Point3d> points_;
+ // points estimated from cameras' observations
+ std::vector<cv::Point3d> points_camera_;
+ // observations
std::vector<cv::Point2d> observations_;
+ // visibility
std::vector<bool> visible_;
};
diff --git a/applications/calibration-ceres/src/main.cpp b/applications/calibration-ceres/src/main.cpp
index f73f97dcb..087c3283b 100644
--- a/applications/calibration-ceres/src/main.cpp
+++ b/applications/calibration-ceres/src/main.cpp
@@ -104,7 +104,8 @@ void calibrate(const string &fname) {
// Needs better solution which allows simple access to all estimations.
// Required for calculating average coordinates and to know which points
// are missing.
- vector<tuple<int, vector<Point3d>, vector<reference_wrapper<Point3d>>>> points_all;
+
+ vector<tuple<int, vector<Point3d>>> points_all;
transformations[make_pair(refcamera, refcamera)] =
make_pair(Mat::eye(3, 3, CV_64FC1), Mat::zeros(3, 1, CV_64FC1));
@@ -129,7 +130,7 @@ void calibrate(const string &fname) {
Mat t_;
vector<Point3d> points;
- auto [observations, points_ref] = data.getVisible({current, to});
+ auto observations = data.getObservations({current, to});
double err = computeExtrinsicParameters(
data.getCamera(current).intrinsicMatrix(),
data.getCamera(current).distortionCoefficients(),
@@ -141,7 +142,7 @@ void calibrate(const string &fname) {
LOG(INFO) << current << " -> " << to << " (RMS error: " << err << ")";
transformations[edge] = make_pair(R_, t_);
- points_all.push_back(make_tuple(current, points, points_ref));
+ points_all.push_back(make_tuple(current, points));
}
const auto [R_update, t_update] = transformations[edge];
@@ -158,7 +159,8 @@ void calibrate(const string &fname) {
}
// TODO: see points_all comment
- for (auto& [i, points, points_ref] : points_all) {
+ /*
+ for (auto& [i, points] : points_all) {
Mat R = data.getCamera(i).rmat();
Mat t = data.getCamera(i).tvec();
@@ -177,7 +179,7 @@ void calibrate(const string &fname) {
}
}
}
-
+ */
vector<int> idx;
BundleAdjustment ba;
@@ -213,11 +215,9 @@ void calibrate(const string &fname) {
LOG(INFO) << "Finale reprojection error: " << ba.reprojectionError();
- // calibration done
+ // calibration done, updated values in data
}
-
int main(int argc, char* argv[]) {
- calibrate("");
return 0;
}
diff --git a/applications/calibration-ceres/src/optimization.cpp b/applications/calibration-ceres/src/optimization.cpp
index 493bdc6ab..aafe2a38a 100644
--- a/applications/calibration-ceres/src/optimization.cpp
+++ b/applications/calibration-ceres/src/optimization.cpp
@@ -393,7 +393,14 @@ void BundleAdjustment::run(const BundleAdjustment::Options &bundle_adjustment_op
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.minimizer_progress_to_stdout = bundle_adjustment_options.verbose;
- options.max_num_iterations = bundle_adjustment_options.max_iter;
+
+ if (bundle_adjustment_options.max_iter > 0) {
+ options.max_num_iterations = bundle_adjustment_options.max_iter;
+ }
+
+ if (bundle_adjustment_options.num_threads > 0) {
+ options.num_threads = bundle_adjustment_options.num_threads;
+ }
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
diff --git a/applications/calibration-ceres/src/optimization.hpp b/applications/calibration-ceres/src/optimization.hpp
index 9fbd16129..3b0716c7b 100644
--- a/applications/calibration-ceres/src/optimization.hpp
+++ b/applications/calibration-ceres/src/optimization.hpp
@@ -83,7 +83,8 @@ public:
bool optimize_motion = true;
bool optmize_structure = true;
- int max_iter = 50;
+ int num_threads = -1;
+ int max_iter = -1;
bool verbose = false;
};
diff --git a/applications/calibration-multi/src/main.cpp b/applications/calibration-multi/src/main.cpp
index b6622f70e..4cbd5f284 100644
--- a/applications/calibration-multi/src/main.cpp
+++ b/applications/calibration-multi/src/main.cpp
@@ -232,7 +232,8 @@ void calibrateRPC( ftl::net::Universe* net,
while(true) {
try {
if (params.optimize_intrinsic) {
- setIntrinsicsRPC(net, nstream, params.size, {K1, K2}, {D1, D2});
+ // never update distortion during extrinsic calibration
+ setIntrinsicsRPC(net, nstream, params.size, {K1, K2}, {Mat(0, 0, CV_64FC1), Mat(0, 0, CV_64FC1)});
}
setExtrinsicsRPC(net, nstream, R_c1c2, T_c1c2);
setPoseRPC(net, nstream, getMat4x4(R[c], t[c]));
@@ -667,6 +668,8 @@ void runCameraCalibrationPath( ftl::Configurable* root,
cv::cvtColor(left, rgb_new[2*idx], cv::COLOR_BGRA2BGR);
cv::cvtColor(right, rgb_new[2*idx+1], cv::COLOR_BGRA2BGR);
+ camera_parameters[2*idx] = createCameraMatrix(fs.frames[i].getLeftCamera());
+ camera_parameters[2*idx+1] = createCameraMatrix(fs.frames[i].getRightCamera());
if (res.empty()) res = rgb_new[2*idx].size();
}
}
@@ -721,6 +724,27 @@ void runCameraCalibrationPath( ftl::Configurable* root,
std::iota(params.idx_cameras.begin(), params.idx_cameras.end(), 0);
calib.loadInput(path + filename, params.idx_cameras);
+ for (size_t i = 0; i < nstreams.size(); i++) {
+ while(true) {
+ try {
+ if (camera_parameters[2*i].empty() || camera_parameters[2*i+1].empty()) {
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+ continue;
+ }
+ vector<Mat> D = getDistortionParametersRPC(net, nstreams[i]);
+ LOG(INFO) << "K[" << 2*i << "] = \n" << camera_parameters[2*i];
+ LOG(INFO) << "D[" << 2*i << "] = " << D[0];
+ LOG(INFO) << "K[" << 2*i+1 << "] = \n" << camera_parameters[2*i+1];
+ LOG(INFO) << "D[" << 2*i+1 << "] = " << D[1];
+ calib.setCameraParameters(2*i, camera_parameters[2*i], D[0]);
+ calib.setCameraParameters(2*i+1, camera_parameters[2*i+1], D[1]);
+ break;
+ }
+ catch (...) {}
+ }
+ }
+
+
Mat out;
vector<Mat> map1, map2;
vector<cv::Rect> roi;
diff --git a/applications/calibration-multi/src/multicalibrate.cpp b/applications/calibration-multi/src/multicalibrate.cpp
index 60f620982..b9627ce25 100644
--- a/applications/calibration-multi/src/multicalibrate.cpp
+++ b/applications/calibration-multi/src/multicalibrate.cpp
@@ -146,9 +146,9 @@ Mat MultiCameraCalibrationNew::getDistCoeffs(size_t idx) {
}
void MultiCameraCalibrationNew::setCameraParameters(size_t idx, const Mat &K, const Mat &distCoeffs) {
- DCHECK(idx < n_cameras_);
- DCHECK(K.size() == Size(3, 3));
- DCHECK(distCoeffs.size() == Size(5, 1));
+ CHECK(idx < n_cameras_);
+ CHECK(K.size() == Size(3, 3));
+ CHECK(distCoeffs.total() == 5);
K.convertTo(K_[idx], CV_64FC1);
distCoeffs.convertTo(dist_coeffs_[idx], CV_64FC1);
}
@@ -529,6 +529,7 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
t1 = Mat(Size(1, 3), CV_64FC1, Scalar(0.0));
vector<Point3d> points3d;
+
double err = ftl::calibration::computeExtrinsicParameters(K1, dist_coeffs_[camera_from], K2, dist_coeffs_[camera_to], points1, points2, object_points_, R2, t2, points3d);
calculateTransform(R2, t2, R1, t1, rmat, tvec);
diff --git a/components/common/cpp/include/ftl/calibration.hpp b/components/common/cpp/include/ftl/calibration.hpp
index 719da54d8..006626852 100644
--- a/components/common/cpp/include/ftl/calibration.hpp
+++ b/components/common/cpp/include/ftl/calibration.hpp
@@ -9,6 +9,9 @@ namespace calibration {
namespace validate {
+/**
+ * @brief Valid translation for stereo camera.
+ */
bool translationStereo(const cv::Mat &t);
bool rotationMatrix(const cv::Mat &M);
@@ -34,6 +37,11 @@ bool distortionCoefficients(const cv::Mat &D, cv::Size size);
}
+/**
+ * @brief Scale camera intrinsic matrix
+ * @param size_new New resolution
+ * @param size_old Original (camera matrix) resolution
+ */
cv::Mat scaleCameraMatrix(const cv::Mat &K, const cv::Size &size_new, const cv::Size &size_old);
}
diff --git a/components/common/cpp/src/calibration.cpp b/components/common/cpp/src/calibration.cpp
index 4ee3425a7..6a6bd8fa3 100644
--- a/components/common/cpp/src/calibration.cpp
+++ b/components/common/cpp/src/calibration.cpp
@@ -106,8 +106,8 @@ bool ftl::calibration::validate::distortionCoefficients(const Mat &D, Size size)
double dist_prev_n = 0;
double dist_prev_p = 0;
- for (int r2_ = 0; r2_ < diagonal; r2_++) {
- double r2 = r2_;
+ for (int r = 0; r < diagonal; r++) {
+ double r2 = r*r;
double r4 = r2*r2;
double r6 = r4*r2;
--
GitLab