Skip to content
Snippets Groups Projects
Commit 56e86a47 authored by Sebastian Hahta's avatar Sebastian Hahta
Browse files

scaling missing in calibration

parent 8be40171
No related branches found
No related tags found
No related merge requests found
Pipeline #21566 passed
Stage: all
Stage: deploy
Hide whitespace changes
Inline Side-by-side
applications/calibration-ceres/src/calibration.cpp
+ 2
− 0
View file @ 56e86a47
......@@ -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();
......
applications/calibration-multi/src/main.cpp
+ 24
− 20
View file @ 56e86a47
......@@ -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;
......
applications/calibration-multi/src/multicalibrate.cpp
+ 5
− 2
View file @ 56e86a47
......@@ -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);
......
components/calibration/include/ftl/calibration/optimize.hpp
+ 11
− 14
View file @ 56e86a47
......@@ -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
......
components/calibration/src/optimize.cpp
+ 55
− 35
View file @ 56e86a47
......@@ -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;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment