diff --git a/applications/calibration-ceres/src/calibration.cpp b/applications/calibration-ceres/src/calibration.cpp
index ab0f9412f366fad9675192a9082b25ce3e053ae6..a78905a2cfb762ffc1081994f0d09c8ee6463a9a 100644
--- a/applications/calibration-ceres/src/calibration.cpp
+++ b/applications/calibration-ceres/src/calibration.cpp
@@ -110,6 +110,8 @@ double ftl::calibration::computeExtrinsicParameters(const Mat &K1, const Mat &D1
for (size_t i = 0; i < points_out.size(); i++) {
ba.addPoint({points1[i], points2[i]}, points_out[i]);
}
+
+ ba.addObject(object_points);
double error_before = ba.reprojectionError();
diff --git a/applications/calibration-multi/src/main.cpp b/applications/calibration-multi/src/main.cpp
index 4cbd5f28466d52e6cbdf55f774e85f9cbad94925..f492b8e7b6f68f7082b62c523efd44781ec7f8fc 100644
--- a/applications/calibration-multi/src/main.cpp
+++ b/applications/calibration-multi/src/main.cpp
@@ -5,6 +5,8 @@
#include <ftl/rgbd/source.hpp>
#include <ftl/rgbd/group.hpp>
+#include <ftl/calibration.hpp>
+
#include <ftl/master.hpp>
#include <ftl/streams/receiver.hpp>
#include <ftl/streams/netstream.hpp>
@@ -231,14 +233,18 @@ void calibrateRPC( ftl::net::Universe* net,
auto *nstream = nstreams[c/2];
while(true) {
try {
- if (params.optimize_intrinsic) {
+ if (params.save_intrinsic && params.optimize_intrinsic) {
// 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]));
- saveCalibrationRPC(net, nstream);
- LOG(INFO) << "CALIBRATION SENT";
+ if (params.save_extrinsic) {
+ setExtrinsicsRPC(net, nstream, R_c1c2, T_c1c2);
+ setPoseRPC(net, nstream, getMat4x4(R[c], t[c]));
+ }
+ if (params.save_intrinsic || params.save_extrinsic) {
+ saveCalibrationRPC(net, nstream);
+ LOG(INFO) << "CALIBRATION SENT";
+ }
break;
} catch (std::exception &ex) {
@@ -382,8 +388,11 @@ void runCameraCalibration( 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());
+ camera_parameters[2*idx] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getLeftCamera()),
+ rgb_new[2*idx].size(), Size(fs.frames[i].getLeftCamera().width, fs.frames[i].getLeftCamera().height));
+ camera_parameters[2*idx+1] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getRightCamera()),
+ rgb_new[2*idx].size(), Size(fs.frames[i].getRightCamera().width, fs.frames[i].getRightCamera().height));
+
if (res.empty()) res = rgb_new[2*idx].size();
}
}
@@ -667,10 +676,13 @@ 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] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getLeftCamera()),
+ rgb_new[2*idx].size(), Size(fs.frames[i].getLeftCamera().width, fs.frames[i].getLeftCamera().height));
+ camera_parameters[2*idx+1] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getRightCamera()),
+ rgb_new[2*idx].size(), Size(fs.frames[i].getRightCamera().width, fs.frames[i].getRightCamera().height));
- 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();
+ if (res.empty()) res = rgb_new[2*idx].size();*/
}
}
catch (std::exception ex) {
@@ -687,14 +699,6 @@ void runCameraCalibrationPath( ftl::Configurable* root,
stream->begin();
ftl::timer::start(false);
-
- while(true) {
- if (!res.empty()) {
- LOG(INFO) << "Camera resolution: " << params.size;
- break;
- }
- std::this_thread::sleep_for(std::chrono::seconds(1));
- }
for (auto *nstream: nstreams) {
bool res = true;
@@ -724,7 +728,7 @@ 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++) {
+ /*for (size_t i = 0; i < nstreams.size(); i++) {
while(true) {
try {
if (camera_parameters[2*i].empty() || camera_parameters[2*i+1].empty()) {
@@ -742,7 +746,7 @@ void runCameraCalibrationPath( ftl::Configurable* root,
}
catch (...) {}
}
- }
+ }*/
Mat out;
diff --git a/applications/calibration-multi/src/multicalibrate.cpp b/applications/calibration-multi/src/multicalibrate.cpp
index 836a90fde0d0b091ad8207315463ffab6a20e295..630dd576acfc85337352a0fd7743ace0db53a1e3 100644
--- a/applications/calibration-multi/src/multicalibrate.cpp
+++ b/applications/calibration-multi/src/multicalibrate.cpp
@@ -700,7 +700,6 @@ double MultiCameraCalibrationNew::calibrateAll(int reference_camera) {
calculateInverse(R_[c_from], t_[c_from], R, t);
LOG(INFO) << "Error before BA, cameras " << reference_camera_ << " and " << c_from << ": "
<< getReprojectionErrorOptimized(c_from, K_[c_from], R, t);
-
}
double err;
@@ -739,13 +738,17 @@ double MultiCameraCalibrationNew::calibrateAll(int reference_camera) {
ba.addPoint(visible, points2d, p);
}
+ ba.addObject(object_points_);
+
ftl::calibration::BundleAdjustment::Options options;
+ options.loss = ftl::calibration::BundleAdjustment::Options::Loss::CAUCHY;
options.optimize_intrinsic = !fix_intrinsics_;
options.fix_distortion = true;
options.max_iter = 50;
options.fix_camera_extrinsic = {reference_camera};
options.verbose = true;
-
+ options.max_iter = 500;
+
err = ba.reprojectionError();
ba.run(options);
diff --git a/components/calibration/include/ftl/calibration/optimize.hpp b/components/calibration/include/ftl/calibration/optimize.hpp
index 1009a97b21aa9d763801b7c512a4e1d13456b4a3..ac122f517e07bece99c84e5ec0a48912681ed861 100644
--- a/components/calibration/include/ftl/calibration/optimize.hpp
+++ b/components/calibration/include/ftl/calibration/optimize.hpp
@@ -108,8 +108,7 @@ public:
void addPoint(const std::vector<cv::Point2d>& observations, cv::Point3d& point);
void addPoints(const std::vector<std::vector<cv::Point2d>>& observations, std::vector<cv::Point3d>& points);
- void addConstraintPlane(int group_size);
- void addConstraintObject(const std::vector<cv::Point3d>& object_points);
+ void addObject(const std::vector<cv::Point3d>& object_points);
/** @brief Perform bundle adjustment with custom options.
*/
@@ -119,11 +118,11 @@ public:
*/
void run();
- /** @brief Calculate RMS error (for one camera)
+ /** @brief Calculate MSE error (for one camera)
*/
double reprojectionError(const int camera) const;
- /** @brief Calculate RMS error for all cameras
+ /** @brief Calculate MSE error for all cameras
*/
double reprojectionError() const;
@@ -134,6 +133,11 @@ protected:
*/
void _reprojectionErrorMSE(const int camera, double &error, double &npoints) const;
+ /** @brief Set camera parametrization (fixed parameters/cameras)
+ */
+ void _setCameraParametrization(ceres::Problem& problem, const BundleAdjustment::Options& options);
+ void _setStructureParametrization(ceres::Problem& problem, const BundleAdjustment::Options& options);
+
void _buildProblem(ceres::Problem& problem, const BundleAdjustment::Options& options);
void _buildBundleAdjustmentProblem(ceres::Problem& problem, const BundleAdjustment::Options& options);
void _buildLengthProblem(ceres::Problem& problem, const BundleAdjustment::Options& options);
@@ -150,24 +154,17 @@ private:
double* point;
};
- // object shape based constraint for group of points from idx_start to idx_end
- struct ConstraintObject {
+ // group of points with known structure; from idx_start to idx_end
+ struct Object {
int idx_start;
int idx_end;
std::vector<cv::Point3d> object_points;
};
- // planar constraint for group of points from idx_start to idx_end
- struct ConstraintPlane {
- int idx_start;
- int idx_end;
- int group_size;
- };
-
// camera paramters (as pointers)
std::vector<Camera*> cameras_;
std::vector<BundleAdjustment::Point> points_;
- std::vector<BundleAdjustment::ConstraintObject> constraints_object_;
+ std::vector<BundleAdjustment::Object> objects_;
};
#endif
diff --git a/components/calibration/src/optimize.cpp b/components/calibration/src/optimize.cpp
index e5319e3fc190284d82f250cc8fc58a2df0fa1de5..6c8f3106498b6b949253ba728f81869efc26bd47 100644
--- a/components/calibration/src/optimize.cpp
+++ b/components/calibration/src/optimize.cpp
@@ -237,37 +237,13 @@ void BundleAdjustment::addPoints(const vector<vector<Point2d>>& observations, st
}
}
-void BundleAdjustment::addConstraintObject(const vector<Point3d> &object_points) {
+void BundleAdjustment::addObject(const vector<Point3d> &object_points) {
if (points_.size() % object_points.size() != 0) { throw std::exception(); }
- constraints_object_.push_back(BundleAdjustment::ConstraintObject {0, (int) points_.size(), object_points});
+ objects_.push_back(BundleAdjustment::Object {0, (int) points_.size(), object_points});
}
-void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, const BundleAdjustment::Options &options) {
-
- ceres::LossFunction *loss_function = nullptr;
-
- if (options.loss == Options::Loss::HUBER) {
- loss_function = new ceres::HuberLoss(1.0);
- }
- else if (options.loss == Options::Loss::CAUCHY) {
- loss_function = new ceres::CauchyLoss(1.0);
- }
-
- for (auto point : points_) {
- for (size_t i = 0; i < point.observations.size(); i++) {
- if (!point.visibility[i]) { continue; }
- ceres::CostFunction* cost_function =
- ReprojectionError::Create(point.observations[i]);
-
- problem.AddResidualBlock(cost_function,
- loss_function,
- getCameraPtr(i),
- point.point);
- }
- }
-
- // apply options
-
+void BundleAdjustment::_setCameraParametrization(ceres::Problem &problem, const BundleAdjustment::Options &options) {
+
vector<int> constant_camera_parameters;
// extrinsic paramters
@@ -336,6 +312,7 @@ void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, co
if (params.size() == Camera::n_parameters) {
// Ceres crashes if all parameters are set constant using
// SubsetParameterization()
+ // https://github.com/ceres-solver/ceres-solver/issues/347
problem.SetParameterBlockConstant(getCameraPtr(i));
}
else if (params.size() > 0) {
@@ -344,6 +321,35 @@ void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, co
}
}
}
+}
+
+void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, const BundleAdjustment::Options &options) {
+
+ ceres::LossFunction *loss_function = nullptr;
+
+ if (options.loss == Options::Loss::HUBER) {
+ loss_function = new ceres::HuberLoss(1.0);
+ }
+ else if (options.loss == Options::Loss::CAUCHY) {
+ loss_function = new ceres::CauchyLoss(1.0);
+ }
+
+ for (auto point : points_) {
+ for (size_t i = 0; i < point.observations.size(); i++) {
+ if (!point.visibility[i]) { continue; }
+ ceres::CostFunction* cost_function =
+ ReprojectionError::Create(point.observations[i]);
+
+ problem.AddResidualBlock(cost_function,
+ loss_function,
+ getCameraPtr(i),
+ point.point);
+ }
+ }
+
+ // apply options
+
+ _setCameraParametrization(problem, options);
if (!options.optmize_structure) {
// do not optimize points
@@ -353,9 +359,23 @@ void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, co
void BundleAdjustment::_buildLengthProblem(ceres::Problem &problem, const BundleAdjustment::Options &options) {
- for (auto &constraint : constraints_object_) {
- int npoints = constraint.object_points.size();
- auto &object_points = constraint.object_points;
+ // same idea as in scale optimization
+
+ ceres::LossFunction *loss_function = nullptr;
+
+ // should use separate configuration option
+ /*
+ if (options.loss == Options::Loss::HUBER) {
+ loss_function = new ceres::HuberLoss(1.0);
+ }
+ else if (options.loss == Options::Loss::CAUCHY) {
+ loss_function = new ceres::CauchyLoss(1.0);
+ }
+ */
+
+ for (auto &object : objects_) {
+ int npoints = object.object_points.size();
+ auto &object_points = object.object_points;
vector<double> d;
for (int i = 0; i < npoints; i++) {
@@ -364,7 +384,7 @@ void BundleAdjustment::_buildLengthProblem(ceres::Problem &problem, const Bundle
}
}
- for (int p = constraint.idx_start; p < constraint.idx_end; p += npoints) {
+ for (int p = object.idx_start; p < object.idx_end; p += npoints) {
size_t i_d = 0;
for (size_t i = 0; i < object_points.size(); i++) {
for (size_t j = i + 1; j < object_points.size(); j++) {
@@ -373,7 +393,7 @@ void BundleAdjustment::_buildLengthProblem(ceres::Problem &problem, const Bundle
auto cost_function = LengthError::Create(d[i_d++]);
- problem.AddResidualBlock(cost_function, nullptr, p1, p2);
+ problem.AddResidualBlock(cost_function, loss_function, p1, p2);
}
}
}
@@ -442,7 +462,7 @@ void BundleAdjustment::_reprojectionErrorMSE(const int camera, double &error, do
double BundleAdjustment::reprojectionError(const int camera) const {
double error, ncameras;
_reprojectionErrorMSE(camera, ncameras, error);
- return sqrt(error);
+ return error / ncameras;
}
double BundleAdjustment::reprojectionError() const {
@@ -454,5 +474,5 @@ double BundleAdjustment::reprojectionError() const {
error += e * n;
npoints += n;
}
- return sqrt(error / npoints);
+ return error / npoints;
}