diff --git a/applications/gui2/src/modules/calibration/calibration.hpp b/applications/gui2/src/modules/calibration/calibration.hpp
index 00b33b3ec5288c7f49469f8c51002e2a33efa372..cff1b82759b58661d98324f6b767838ad0ce9c53 100644
--- a/applications/gui2/src/modules/calibration/calibration.hpp
+++ b/applications/gui2/src/modules/calibration/calibration.hpp
@@ -162,7 +162,7 @@ public:
void run();
/** Save calibration */
- void save();
+ void saveCalibration();
ftl::calibration::CalibrationData::Intrinsic calibration();
@@ -264,7 +264,7 @@ public:
std::string cameraName(int camera);
std::vector<std::string> cameraNames();
- ftl::calibration::ExtrinsicCalibration& calib();
+ ftl::calibration::ExtrinsicCalibration& calib() { return state_.calib; } // should avoid
/** hasFrame(int) must be true before calling getFrame() **/
bool hasFrame(int camera);
@@ -412,7 +412,7 @@ public:
void run();
/** Save calibration */
- void save();
+ void saveCalibration();
/** check if calibration valid: baseline > 0 */
bool calibrated();
diff --git a/applications/gui2/src/modules/calibration/extrinsic.cpp b/applications/gui2/src/modules/calibration/extrinsic.cpp
index c1e4c07e3764bcfe62f9eadf50952b81f0cc7c8d..f5b35eed4fd85100e89adf31d1555cfb894ec7b5 100644
--- a/applications/gui2/src/modules/calibration/extrinsic.cpp
+++ b/applications/gui2/src/modules/calibration/extrinsic.cpp
@@ -83,7 +83,7 @@ void ExtrinsicCalibration::start(unsigned int fsid, std::vector<FrameID> sources
calibr.intrinsic = CalibrationData::Intrinsic(calibr.intrinsic, sz);
// Scale intrinsics
- state_.calib.addStereoCamera(calibl.intrinsic, calibr.intrinsic);
+ state_.calib.addStereoCamera(calibl, calibr);
// Update rectification
unsigned int idx = state_.cameras.size() - 2;
@@ -288,6 +288,7 @@ void ExtrinsicCalibration::updateCalibration() {
auto& frame = fs->frames[c];
update[frame_id] = frame.get<CalibrationData>(Channel::CalibrationData);
}
+ update[frame_id].origin = cv::Mat::eye(4, 4, CV_64FC1);
update[frame_id].get(c.channel) = state_.calib.calibrationOptimized(i);
}
@@ -370,12 +371,14 @@ void ExtrinsicCalibration::run() {
for (int c = 0; c < cameraCount(); c += 2) {
auto l = state_.calib.calibrationOptimized(c);
auto r = state_.calib.calibrationOptimized(c + 1);
- LOG(INFO) << c << ": rvec " << l.extrinsic.rvec << "; tvec " << l.extrinsic.tvec;
- LOG(INFO) << c + 1 << ": rvec " << r.extrinsic.rvec << "; tvec " << r.extrinsic.tvec;
stereoRectify(c, c + 1, l, r);
+ /*LOG(INFO) << c << ": rvec " << l.extrinsic.rvec
+ << "; tvec " << l.extrinsic.tvec;
+ LOG(INFO) << c + 1 << ": rvec " << r.extrinsic.rvec
+ << "; tvec " << r.extrinsic.tvec;*/
LOG(INFO) << "baseline (" << c << ", " << c + 1 << "): "
- << cv::norm(l.extrinsic.tvec, r.extrinsic.tvec);
+ << cv::norm(l.extrinsic.tvec - r.extrinsic.tvec);
}
}
catch (ftl::exception &ex) {
@@ -419,6 +422,9 @@ const std::vector<cv::Point2d>& ExtrinsicCalibration::previousPoints(int camera)
return state_.points_prev[camera];
}
+
+
+
int ExtrinsicCalibration::getFrameCount(int camera) {
return state_.calib.points().getCount(unsigned(camera));
}
diff --git a/applications/gui2/src/modules/calibration/intrinsic.cpp b/applications/gui2/src/modules/calibration/intrinsic.cpp
index 9a3dc06bdef71397f48626edf3a02e05a1a3875d..2fa2e9c54fcf996b95994d07a8b055fd9010473f 100644
--- a/applications/gui2/src/modules/calibration/intrinsic.cpp
+++ b/applications/gui2/src/modules/calibration/intrinsic.cpp
@@ -204,7 +204,7 @@ bool IntrinsicCalibration::onFrame_(const ftl::data::FrameSetPtr& fs) {
}
-void IntrinsicCalibration::save() {
+void IntrinsicCalibration::saveCalibration() {
auto& frame = fs_current_->frames[state_->id.source()];
CalibrationData calib_data = CalibrationData(frame.get<CalibrationData>(Channel::CalibrationData));
auto& calibration = calib_data.get(state_->channel);
diff --git a/applications/gui2/src/modules/calibration/stereo.cpp b/applications/gui2/src/modules/calibration/stereo.cpp
index 1ac7c9fc10cf39938abd266ab68e862050c971a3..052bf15572b0e97790cfbfed8ac7c4cf90f96230 100644
--- a/applications/gui2/src/modules/calibration/stereo.cpp
+++ b/applications/gui2/src/modules/calibration/stereo.cpp
@@ -182,7 +182,7 @@ bool StereoCalibration::onFrame_(const ftl::data::FrameSetPtr& fs) {
}
-void StereoCalibration::save() {
+void StereoCalibration::saveCalibration() {
auto fs = std::atomic_load(&(fs_current_));
setCalibration((*fs)[state_->id.source()], state_->calib);
}
diff --git a/applications/gui2/src/views/calibration/extrinsicview.cpp b/applications/gui2/src/views/calibration/extrinsicview.cpp
index b59f2ffa46b31802a5fa7b60c757439575324f29..2fe214712e4ba993dc071276130696a052a2ee8b 100644
--- a/applications/gui2/src/views/calibration/extrinsicview.cpp
+++ b/applications/gui2/src/views/calibration/extrinsicview.cpp
@@ -368,6 +368,84 @@ void ExtrinsicCalibrationView::CalibrationWindow::build() {
else { flags &= ~ExtrinsicCalibration::Flags::RATIONAL_MODEL; }
});
+ ////////////////////////////////////////////////////////////////////////////
+
+ new nanogui::Label(wfreeze, "Use available (calibrated) extrinsics for cameras: ");
+ auto* use_extrinsics = new nanogui::Widget(wfreeze);
+ use_extrinsics->setLayout(new nanogui::BoxLayout
+ (nanogui::Orientation::Horizontal, nanogui::Alignment::Minimum));
+ for (int n = 0; n < ctrl_->cameraCount(); n++) {
+ auto* b = new nanogui::Button(use_extrinsics, std::to_string(n));
+ b->setFlags(nanogui::Button::Flags::ToggleButton);
+ b->setPushed(ctrl_->calib().useExtrinsic(n));
+ b->setEnabled(ctrl_->calib().calibration(n).extrinsic.valid());
+ b->setChangeCallback([this, n](bool v) {
+ ctrl_->calib().setUseExtrinsic(n, v);
+ });
+ }
+ {
+ auto* b = new nanogui::Button(use_extrinsics, "All");
+ b->setCallback([this, use_extrinsics](){
+ for (int i = 0; i < use_extrinsics->childCount() - 2; i ++) {
+ auto* b = dynamic_cast<nanogui::Button*>(use_extrinsics->childAt(i));
+ b->setPushed(true);
+ b->changeCallback()(true);
+ }
+ });
+ }
+ {
+ auto* b = new nanogui::Button(use_extrinsics, "None");
+ b->setCallback([this, use_extrinsics](){
+ for (int i = 0; i < use_extrinsics->childCount() - 2; i ++) {
+ auto* b = dynamic_cast<nanogui::Button*>(use_extrinsics->childAt(i));
+ b->setPushed(false);
+ b->changeCallback()(false);
+ }
+ });
+ }
+
+ ////////////////////////////////////////////////////////////////////////////
+ // TODO: selecting camera should also enable use existing above for same c
+
+ new nanogui::Label(wfreeze, "Fix extrinsics for cameras: ");
+ auto* fix_extrinsics = new nanogui::Widget(wfreeze);
+ fix_extrinsics->setLayout(new nanogui::BoxLayout
+ (nanogui::Orientation::Horizontal, nanogui::Alignment::Minimum));
+ for (int n = 0; n < ctrl_->cameraCount(); n++) {
+ auto* b = new nanogui::Button(fix_extrinsics, std::to_string(n));
+ b->setFlags(nanogui::Button::Flags::ToggleButton);
+ b->setEnabled(ctrl_->calib().useExtrinsic(n));
+ b->setPushed(ctrl_->calib().options().fix_camera_extrinsic.count(n));
+ b->setChangeCallback([this, n](bool v){
+ if (v) {
+ ctrl_->calib().options().fix_camera_extrinsic.insert(n);
+ }
+ else {
+ ctrl_->calib().options().fix_camera_extrinsic.erase(n);
+ }
+ });
+ }
+ {
+ auto* b = new nanogui::Button(fix_extrinsics, "All");
+ b->setCallback([this, fix_extrinsics](){
+ for (int i = 0; i < fix_extrinsics->childCount() - 2; i ++) {
+ auto* b = dynamic_cast<nanogui::Button*>(fix_extrinsics->childAt(i));
+ b->setPushed(true);
+ b->changeCallback()(true);
+ }
+ });
+ }
+ {
+ auto* b = new nanogui::Button(fix_extrinsics, "None");
+ b->setCallback([this, fix_extrinsics](){
+ for (int i = 0; i < fix_extrinsics->childCount() - 2; i ++) {
+ auto* b = dynamic_cast<nanogui::Button*>(fix_extrinsics->childAt(i));
+ b->setPushed(false);
+ b->changeCallback()(false);
+ }
+ });
+ }
+
/* Needs thinking: visualize visibility graph? Use earlier alignment (if
* some of the cameras already calibrated), do elsewhere?
*/
@@ -509,7 +587,7 @@ private:
std::set<int> rows_;
std::map<int, nanogui::Color> colors_;
- int n_colors_ = 16;
+ int n_colors_ = 8;
float alpha_threshold_ = 2.0f;
public:
@@ -568,14 +646,14 @@ void StereoCalibrationImageView::draw(NVGcontext* ctx) {
nvgStrokeWidth(ctx, swidth);
nvgStroke(ctx);
- if (swidth*0.5f > alpha_threshold_) {
+ /*if (swidth*0.5f > alpha_threshold_) {
nvgBeginPath(ctx);
nvgMoveTo(ctx, x, p.y() - swidth*0.5f);
nvgLineTo(ctx, x + w, p.y() - swidth*0.5f);
nvgStrokeColor(ctx, nvgRGBA(0, 0, 0, 196));
nvgStrokeWidth(ctx, 1.0f);
nvgStroke(ctx);
- }
+ }*/
nvgResetScissor(ctx);
y_im += h;
}
diff --git a/applications/gui2/src/views/calibration/intrinsicview.cpp b/applications/gui2/src/views/calibration/intrinsicview.cpp
index 6129ce48db1e53f46880aae553d4a8cd0be46e2d..cfa9941adf3d88c07d0be8d4076a9259d925cc91 100644
--- a/applications/gui2/src/views/calibration/intrinsicview.cpp
+++ b/applications/gui2/src/views/calibration/intrinsicview.cpp
@@ -492,7 +492,7 @@ IntrinsicCalibrationView::ResultWindow::ResultWindow(nanogui::Widget* parent, In
bsave_ = new nanogui::Button(this, "Save");
bsave_->setCallback([button = bsave_, ctrl = ctrl_](){
- ctrl->save();
+ ctrl->saveCalibration();
button->setCaption("Saved");
button->setEnabled(false);
});
diff --git a/applications/gui2/src/views/calibration/stereoview.cpp b/applications/gui2/src/views/calibration/stereoview.cpp
index 3dccafd41b9108037fc9ac8b56ebac0cd5c5fd69..e67eb269c6d0a2bfae834fa5a18d68080c2c44ea 100644
--- a/applications/gui2/src/views/calibration/stereoview.cpp
+++ b/applications/gui2/src/views/calibration/stereoview.cpp
@@ -376,7 +376,7 @@ StereoCalibrationView::ResultWindow::ResultWindow(nanogui::Widget* parent, Stere
bsave_ = new nanogui::Button(this, "Save");
bsave_->setCallback([button = bsave_, ctrl = ctrl_](){
- ctrl->save();
+ ctrl->saveCalibration();
button->setCaption("Saved");
button->setEnabled(false);
});
diff --git a/components/calibration/include/ftl/calibration/extrinsic.hpp b/components/calibration/include/ftl/calibration/extrinsic.hpp
index a9f7208e39ff158cb830e0e8900b5d263defe2b6..645674ddba6208fcc5f35fa9e014e87452aee511 100644
--- a/components/calibration/include/ftl/calibration/extrinsic.hpp
+++ b/components/calibration/include/ftl/calibration/extrinsic.hpp
@@ -158,17 +158,10 @@ double calibratePair(const cv::Mat &K1, const cv::Mat &D1,
class ExtrinsicCalibration {
public:
- /** add a single (uncalibrated) camera. Returns index of camera. */
- unsigned int addCamera(const CalibrationData::Intrinsic &);
-
- /** add a single calibrated camera (if valid calibration). Returns index of camera. */
+ /** add a single camera (if valid calibration). Returns index of camera. */
unsigned int addCamera(const CalibrationData::Calibration &);
- /** Add a stereo camera pair. Pairs always use other cameras to estimate
- * initial pose. Returns index of first camera. */
- unsigned int addStereoCamera(const CalibrationData::Intrinsic &, const CalibrationData::Intrinsic &);
-
- /** Add calibrated stereo camera (if contains valid calibration) */
+ /** Add stereo camera */
unsigned int addStereoCamera(const CalibrationData::Calibration &, const CalibrationData::Calibration &);
const CalibrationData::Intrinsic& intrinsic(unsigned int c);
@@ -187,11 +180,16 @@ public:
/* set bundle adjustment options */
void setOptions(ftl::calibration::BundleAdjustment::Options options) { options_ = options; }
- ftl::calibration::BundleAdjustment::Options options() { return options_; }
+ ftl::calibration::BundleAdjustment::Options& options() { return options_; }
/** Number of cameras added */
unsigned int camerasCount() { return calib_.size(); }
+ /** use existing extrinsic calibration for camera */
+ void setUseExtrinsic(unsigned int c, bool v) { is_calibrated_.at(c) = v; }
+ /** is existing extrinsic parameters used for given camera */
+ bool useExtrinsic(unsigned int c) { return is_calibrated_.at(c); };
+
/** status message */
std::string status();
@@ -208,17 +206,29 @@ public:
MSGPACK_DEFINE(points_, mask_, pairs_, calib_, is_calibrated_);
protected:
- /** Initial pairwise calibration and triangulation. */
+ /** Calculate initial pose and triangulate points for two cameras **/
+ void calculatePairPose(unsigned int c1, unsigned int c2);
+
+ /** Only triangulate points using existing calibration */
+ void triangulate(unsigned int c1, unsigned int c2);
+
+ /** (1) Initial pairwise calibration and triangulation. */
void calculatePairPoses();
- /** Calculate initial poses from pairs */
+ /** (2) Calculate initial poses from pairs for non calibrated cameras */
void calculateInitialPoses();
- /** Bundle adjustment on initial poses and triangulations. */
+ /** (3) Bundle adjustment on initial poses and triangulations. */
double optimize();
+ /** Select optimal camera for chains. Optimal camera has most visibility and
+ * is already calibrated (if any initial calibrations included).
+ */
+ int selectOptimalCamera();
+
private:
void updateStatus_(std::string);
+
std::vector<CalibrationData::Calibration> calib_;
std::vector<CalibrationData::Calibration> calib_optimized_;
ftl::calibration::BundleAdjustment::Options options_;
@@ -226,13 +236,17 @@ private:
CalibrationPoints<double> points_;
std::set<std::pair<unsigned int, unsigned int>> mask_;
std::map<std::pair<unsigned int, unsigned int>, std::tuple<cv::Mat, cv::Mat, double>> pairs_;
- unsigned int c_ref_;
// true if camera already has valid calibration; initial pose estimation is
// skipped (and points are directly triangulated)
std::vector<bool> is_calibrated_;
- int min_points_ = 64; // minimum number of points required for pair calibration
+ // prune points which have higher reprojection error than given threshold
+ // and re-run bundle adjustment. (might be able to remove some problematic
+ // observations from extreme angles etc.)
+ std::vector<double> prune_observations_ = {2.5, 2.0, 2.0, 1.0};
+
+ int min_obs_ = 64; // minimum number of observations required for pair calibration
// TODO: add map {c1,c2}Â for existing calibration which is used if available.
//
std::shared_ptr<std::string> status_;
diff --git a/components/calibration/include/ftl/calibration/optimize.hpp b/components/calibration/include/ftl/calibration/optimize.hpp
index b05d9f9d3dc1cde858a706ed1404e8886162601d..a92f0354d471ae0df6192c8ecc92879af2b4b54a 100644
--- a/components/calibration/include/ftl/calibration/optimize.hpp
+++ b/components/calibration/include/ftl/calibration/optimize.hpp
@@ -67,7 +67,6 @@ struct Camera {
const static int n_parameters = 18;
const static int n_distortion_parameters = 8;
- bool quaternion = false;
double data[n_parameters] = {0.0};
enum Parameter {
@@ -136,17 +135,17 @@ public:
bool use_nonmonotonic_steps = false;
// use quaternion rotation
- bool use_quaternion = false;
+ bool use_quaternion = true;
// fix_camera_extrinsic and fix_camera_intrinsic overlap with some of
// the generic options. The more generic setting is always used, the
// specific extrinsic/intrinsic options are applied on top of those.
// fix extrinsic paramters for cameras
- std::vector<int> fix_camera_extrinsic = {};
+ std::set<int> fix_camera_extrinsic = {};
// fix intrinsic paramters for cameras
- std::vector<int> fix_camera_intrinsic = {};
+ std::set<int> fix_camera_intrinsic = {};
bool fix_focal = false;
bool fix_principal_point = false;
diff --git a/components/calibration/include/ftl/calibration/structures.hpp b/components/calibration/include/ftl/calibration/structures.hpp
index fffbe503508e29b2fbe88fe4c72566bf97c63206..37a9f3d97b181f1450e23de5f0c186db26ff2305 100644
--- a/components/calibration/include/ftl/calibration/structures.hpp
+++ b/components/calibration/include/ftl/calibration/structures.hpp
@@ -116,6 +116,9 @@ struct CalibrationData {
Intrinsic intrinsic;
Extrinsic extrinsic;
+ /** 4x4 projection matrix */
+ cv::Mat matrix();
+
MSGPACK_DEFINE(intrinsic, extrinsic);
};
diff --git a/components/calibration/src/extrinsic.cpp b/components/calibration/src/extrinsic.cpp
index d32961040338efec217048c1017b84588b12d443..25c79e4ff7a7303a1986a96f87eed5f1c6c63613 100644
--- a/components/calibration/src/extrinsic.cpp
+++ b/components/calibration/src/extrinsic.cpp
@@ -290,6 +290,24 @@ int recoverPose(const cv::Mat &E, const std::vector<cv::Point2d> &_points1,
distanceThresh, cv::noArray(), triangulatedPoints);
}
+static double scalePoints(const std::vector<cv::Point3d> &object_points, const cv::Mat& points_in, std::vector<cv::Point3d> &points_out) {
+
+ points_out.clear();
+ points_out.reserve(points_in.cols);
+ // convert from homogenous coordinates
+ for (int col = 0; col < points_in.cols; col++) {
+ CHECK_NE(points_in.at<double>(3, col), 0);
+ cv::Point3d p = cv::Point3d(points_in.at<double>(0, col),
+ points_in.at<double>(1, col),
+ points_in.at<double>(2, col))
+ / points_in.at<double>(3, col);
+ points_out.push_back(p);
+ }
+
+ double s = ftl::calibration::optimizeScale(object_points, points_out);
+ return s;
+}
+
double calibratePair(const cv::Mat &K1, const cv::Mat &D1,
const cv::Mat &K2, const cv::Mat &D2,
const std::vector<cv::Point2d> &points1,
@@ -297,27 +315,16 @@ double calibratePair(const cv::Mat &K1, const cv::Mat &D1,
const std::vector<cv::Point3d> &object_points, cv::Mat &R,
cv::Mat &t, std::vector<cv::Point3d> &points_out, bool optimize) {
+ // FM_8POINT should be good enough if there are no outliers in points1 and
+ // points2 (all correspondences are correct)
cv::Mat F = cv::findFundamentalMat(points1, points2, cv::noArray(), cv::FM_8POINT);
cv::Mat E = K2.t() * F * K1;
cv::Mat points3dh;
- // distanceThresh unit?
+ // distanceThresh?
recoverPose(E, points1, points2, K1, K2, R, t, 1000.0, points3dh);
- points_out.clear();
- points_out.reserve(points3dh.cols);
-
- // convert from homogenous coordinates
- for (int col = 0; col < points3dh.cols; col++) {
- CHECK_NE(points3dh.at<double>(3, col), 0);
- cv::Point3d p = cv::Point3d(points3dh.at<double>(0, col),
- points3dh.at<double>(1, col),
- points3dh.at<double>(2, col))
- / points3dh.at<double>(3, col);
- points_out.push_back(p);
- }
-
- double s = ftl::calibration::optimizeScale(object_points, points_out);
+ double s = scalePoints(object_points, points3dh, points_out);
t = t * s;
auto params1 = Camera(K1, D1, cv::Mat::eye(3, 3, CV_64FC1), cv::Mat::zeros(3, 1, CV_64FC1), {0, 0});
@@ -353,14 +360,6 @@ double calibratePair(const cv::Mat &K1, const cv::Mat &D1,
// ==== Extrinsic Calibration ==================================================
-unsigned int ExtrinsicCalibration::addCamera(const CalibrationData::Intrinsic &c) {
- unsigned int idx = calib_.size();
- calib_.push_back({c, {}});
- calib_optimized_.push_back(calib_.back());
- is_calibrated_.push_back(false);
- return idx;
-}
-
unsigned int ExtrinsicCalibration::addCamera(const CalibrationData::Calibration &c) {
unsigned int idx = calib_.size();
calib_.push_back(c);
@@ -369,18 +368,6 @@ unsigned int ExtrinsicCalibration::addCamera(const CalibrationData::Calibration
return idx;
}
-unsigned int ExtrinsicCalibration::addStereoCamera(const CalibrationData::Intrinsic &c1, const CalibrationData::Intrinsic &c2) {
- unsigned int idx = calib_.size();
- calib_.push_back({c1, {}});
- calib_optimized_.push_back(calib_.back());
- calib_.push_back({c2, {}});
- calib_optimized_.push_back(calib_.back());
- is_calibrated_.push_back(false);
- is_calibrated_.push_back(false);
- mask_.insert({idx, idx + 1});
- return idx;
-}
-
unsigned int ExtrinsicCalibration::addStereoCamera(const CalibrationData::Calibration &c1, const CalibrationData::Calibration &c2) {
unsigned int idx = calib_.size();
calib_.push_back({c1.intrinsic, c1.extrinsic});
@@ -403,12 +390,86 @@ void ExtrinsicCalibration::updateStatus_(std::string str) {
std::atomic_store(&status_, std::make_shared<std::string>(str));
}
+void ExtrinsicCalibration::calculatePairPose(unsigned int c1, unsigned int c2) {
+
+ // calculate paramters and update triangulation
+
+ cv::Mat K1 = calib_[c1].intrinsic.matrix();
+ cv::Mat distCoeffs1 = calib_[c1].intrinsic.distCoeffs.Mat();
+ cv::Mat K2 = calib_[c2].intrinsic.matrix();
+ cv::Mat distCoeffs2 = calib_[c2].intrinsic.distCoeffs.Mat();
+ auto pts = points().getPoints({c1, c2}, 0);
+ auto object = points().getObject(0);
+ cv::Mat R, t;
+ std::vector<cv::Point3d> points3d;
+ auto rmse = calibratePair(K1, distCoeffs1, K2, distCoeffs2,
+ pts[0], pts[1], object, R, t, points3d, true);
+
+ // debug info
+ LOG(INFO) << "RMSE (cameras " << c1 << " & " << c2 << "): " << rmse;
+
+ points().setTriangulatedPoints(c1, c2, points3d);
+
+ pairs_[{c1, c2}] = {R, t, rmse};
+
+ cv::Mat R_i, t_i;
+ R.copyTo(R_i);
+ t.copyTo(t_i);
+ transform::inverse(R_i, t_i);
+ pairs_[{c2, c1}] = {R_i, t_i, rmse};
+}
+
+void ExtrinsicCalibration::triangulate(unsigned int c1, unsigned int c2) {
+
+ cv::Mat T, R, t, R_i, t_i;
+
+ T = calib_[c1].extrinsic.matrix() *
+ transform::inverse(calib_[c2].extrinsic.matrix());
+ transform::getRotationAndTranslation(T, R_i, t_i);
+
+ T = calib_[c2].extrinsic.matrix() *
+ transform::inverse(calib_[c1].extrinsic.matrix());
+ transform::getRotationAndTranslation(T, R, t);
+
+ pairs_[{c1, c1}] = {R, t, NAN};
+ pairs_[{c2, c1}] = {R_i, t_i, NAN};
+
+ auto pts = points().getPoints({c1, c2}, 0);
+ std::vector<cv::Point3d> pointsw;
+
+ const auto& calib1 = calib_[c1];
+ const auto& calib2 = calib_[c2];
+
+ CHECK_EQ(pts[0].size(), pts[1].size());
+
+ cv::Mat pts1(1, pts[0].size(), CV_64FC2, pts[0].data());
+ cv::Mat pts2(1, pts[1].size(), CV_64FC2, pts[1].data());
+ cv::Mat out(1, pts[0].size(), CV_64FC2);
+
+ // Undistort points first. Note: undistortPoints() returns points in
+ // normalized coordinates, therefore projection matrices for
+ // cv::triangulatePoints() only include extrinsic parameters.
+ std::vector<cv::Point2d> pts1u, pts2u;
+ cv::undistortPoints(pts1, pts1u, calib1.intrinsic.matrix(), calib1.intrinsic.distCoeffs.Mat());
+ cv::undistortPoints(pts2, pts2u, calib2.intrinsic.matrix(), calib2.intrinsic.distCoeffs.Mat());
+
+ cv::Mat P1 = cv::Mat::eye(3, 4, CV_64FC1);
+ cv::Mat P2 = T(cv::Rect(0, 0, 4, 3));
+
+ // documentation claims cv::triangulatePoints() requires floats; however
+ // seems to only work with doubles (documentation outdated?).
+ // According to https://stackoverflow.com/a/16299909 cv::triangulatePoints()
+ // implements least squares method described in H&Z p312
+ cv::triangulatePoints(P1, P2, pts1u, pts2u, out);
+ // scalePoints() converts to non-homogenous coordinates and estimates scale
+ scalePoints(points().getObject(0), out, pointsw);
+ points().setTriangulatedPoints(c1, c2, pointsw);
+}
+
void ExtrinsicCalibration::calculatePairPoses() {
- const auto& visibility = points_.visibility();
// Calibrate all pairs. TODO: might be expensive if number of cameras is high
- // if not performed for all pairs, remaining non-triangulated poits have to
- // be separately triangulated later.
+ // if not performed for all pairs.
int i = 1;
int i_max = (camerasCount() * camerasCount()) / 2 + 1;
@@ -417,7 +478,9 @@ void ExtrinsicCalibration::calculatePairPoses() {
for (unsigned int c2 = c1; c2 < camerasCount(); c2++) {
updateStatus_( "Calculating pose for pair " +
- std::to_string(i++) + " of " + std::to_string(i_max));
+ std::to_string(i++) + " of " + std::to_string(i_max) +
+ " and triangulating points");
+
if (c1 == c2) {
pairs_[{c1, c2}] = { cv::Mat::eye(cv::Size(3, 3), CV_64FC1),
@@ -427,51 +490,52 @@ void ExtrinsicCalibration::calculatePairPoses() {
continue;
}
- if (mask_.count({c1, c2}) > 0 ) { continue; }
- if (pairs_.find({c1, c2}) != pairs_.end()) { continue; }
+ if (pairs_.find({c1, c2}) != pairs_.end()) {
+ LOG(WARNING) << "pair already processed (this shold not happen)";
+ continue;
+ }
// require minimum number of visible points
- if (visibility.count(c1, c2) < min_points_) {
+ if (points().visibility().count(c1, c2) < min_obs_) {
LOG(WARNING) << "skipped pair (" << c1 << ", " << c2 << "), not enough points";
continue;
}
- // calculate paramters and update triangulation
-
- cv::Mat K1 = calib_[c1].intrinsic.matrix();
- cv::Mat distCoeffs1 = calib_[c1].intrinsic.distCoeffs.Mat();
- cv::Mat K2 = calib_[c2].intrinsic.matrix();
- cv::Mat distCoeffs2 = calib_[c2].intrinsic.distCoeffs.Mat();
- auto pts = points().getPoints({c1, c2}, 0);
- auto object = points().getObject(0);
- cv::Mat R, t;
- std::vector<cv::Point3d> points3d;
- auto rmse = calibratePair(K1, distCoeffs1, K2, distCoeffs2,
- pts[0], pts[1], object, R, t, points3d, true);
-
- // debug info
- LOG(INFO) << "RMSE (cameras " << c1 << " & " << c2 << "): " << rmse;
-
- points().setTriangulatedPoints(c1, c2, points3d);
-
- pairs_[{c1, c2}] = {R, t, rmse};
-
- cv::Mat R_i, t_i;
- R.copyTo(R_i);
- t.copyTo(t_i);
- transform::inverse(R_i, t_i);
- pairs_[{c2, c1}] = {R_i, t_i, rmse};
+ if (is_calibrated_[c1] && is_calibrated_[c2]) {
+ LOG(INFO) << "using existing pose for cameras " << c1 << " and "
+ << c2 << "(only triangulating points)";
+ triangulate(c1, c2);
+ }
+ else {
+ if (mask_.count({c1, c2}) > 0 ) { continue; }
+ calculatePairPose(c1, c2);
+ }
}}
}
+int ExtrinsicCalibration::selectOptimalCamera() {
+ // Pick optimal camera: most views of calibration pattern. If existing
+ // calibration is used, reference camera must already be calibrated.
+ int c = 0;
+ int calibrated_c_ref_max = 0;
+ for (unsigned int i = 0; i < is_calibrated_[i]; i++) {
+ if (is_calibrated_[i] && points().getCount(i) > calibrated_c_ref_max) {
+ calibrated_c_ref_max = points().getCount(i);
+ c = i;
+ }
+ }
+ if (!calibrated_c_ref_max == 0) {
+ c = points().visibility().argmax();
+ }
+ return c;
+}
+
void ExtrinsicCalibration::calculateInitialPoses() {
updateStatus_("Initial poses from chained transformations");
// mask stereo cameras (do not pairwise calibrate a stereo pair; unreliable)
auto visibility = points_.visibility();
- for (const auto& m: mask_) {
- visibility.mask(m.first, m.second);
- }
+ for (const auto& [c1, c2]: mask_) { visibility.mask(c1, c2); }
// mask cameras which did not have enough points TODO: triangulation later
// would still be useful (calculate initial poses, then triangulate)
@@ -480,20 +544,21 @@ void ExtrinsicCalibration::calculateInitialPoses() {
if (pairs_.count({c1, c2}) == 0) {
visibility.mask(c1, c2);
}
-
- // mask bad pairs (high rmse)
- /*if (std::get<2>(pairs_.at({c1, c2})) > 16.0) {
- visibility.mask(c1, c2);
- }*/
}}
- // pick optimal camera: most views of calibration pattern
- c_ref_ = visibility.argmax();
+ // select optimal camera to calculate chains to. TODO: if any of the
+ // cameras is already calibrated, use most visible calibrated camera as
+ // target camera.
+ auto c_ref = selectOptimalCamera();
- auto paths = visibility.shortestPath(c_ref_);
+ auto paths = visibility.shortestPath(c_ref);
for (unsigned int c = 0; c < camerasCount(); c++) {
- if (c == c_ref_) { continue; }
+ if (is_calibrated_[c]) {
+ // already calibrated. skip chain
+ continue;
+ }
+ if (c == unsigned(c_ref)) { continue; }
cv::Mat R_chain = cv::Mat::eye(cv::Size(3, 3), CV_64FC1);
cv::Mat t_chain = cv::Mat(cv::Size(1, 3), CV_64FC1, cv::Scalar(0.0));
@@ -517,7 +582,6 @@ void ExtrinsicCalibration::calculateInitialPoses() {
while(path.size() > 1);
// note: direction of chain in the loop (ref to target transformation)
-
calib_[c].extrinsic =
CalibrationData::Extrinsic(R_chain, t_chain).inverse();
}
@@ -533,6 +597,10 @@ static std::vector<bool> visibility(unsigned int ncameras, uint64_t visible) {
/* absolute difference between min and max for each set of coordinates */
static cv::Point3d absdiff(const std::vector<double> &xs, const std::vector<double> &ys, const std::vector<double> &zs) {
+ if (xs.size() < 2) {
+ return {0.0, 0.0, 0.0};
+ }
+
double minx = INFINITY;
double maxx = -INFINITY;
for (auto x : xs) {
@@ -551,11 +619,24 @@ static cv::Point3d absdiff(const std::vector<double> &xs, const std::vector<doub
minz = std::min(minz, z);
maxz = std::max(maxz, z);
}
- return {abs(minx - maxx), abs(miny - maxy), abs(minz - maxz)};
+ cv::Point3d diff = {abs(minx - maxx), abs(miny - maxy), abs(minz - maxz)};
+ return diff;
}
double ExtrinsicCalibration::optimize() {
+ // triangulate points for stereo pairs (all points triangulated after this)
+ updateStatus_("Triangulating remaining points");
+ for (const auto& [c1, c2]: mask_) {
+ if (points().visibility().count(c1, c2) >= min_obs_) {
+ triangulate(c1, c2);
+ }
+ else {
+ LOG(INFO) << "Skipping triangulation for pair " << c1 << ", " << c2;
+ }
+ }
+
+ // Build BA
BundleAdjustment ba;
std::vector<Camera> cameras;
std::vector<cv::Mat> T; // camera to world
@@ -579,7 +660,6 @@ double ExtrinsicCalibration::optimize() {
// triangulated multiple times: use median values. Note T[] contains
// inverse transformations, as points are transformed from camera to world
// (instead the other way around by parameters in cameras[]).
-
updateStatus_("Calculating points in world coordinates");
// NOTE: above CalibrationPoints datastructure not optimal regarding how
@@ -626,11 +706,9 @@ double ExtrinsicCalibration::optimize() {
unsigned int n = px.size();
unsigned int m = n / 2;
- if (m == 0) {
+ if (n == 0) {
n_points_missing++;
break;
- // not triangulated (see earlier steps)
- // TODO: triangulate here
}
if (n % 2 == 0 && n > 1) {
// mean of two points if number of points even
@@ -657,19 +735,17 @@ double ExtrinsicCalibration::optimize() {
// was very low quality (more than % bad points)
LOG(ERROR) << "Large variation in "<< n_points_bad << " "
"triangulated points. Are initial intrinsic parameters "
- "good?";
+ "good? If initial camera poses were used, try again "
+ "without using existing values.";
}
if (float(n_points_missing)/float(n_points - n_points_bad) > threhsold_warning_) {
- // low number of points; most points only visible in pairs?
+ // this should not happen any more (all points should be triangulated).
LOG(WARNING) << "Large number of points skipped. Are there enough "
- "visible points between stereo camera pairs? (TODO: "
- "implement necessary triangulation after pair "
- "calibration)";
+ "visible points between stereo camera pairs?";
}
updateStatus_("Bundle adjustment");
- options_.fix_camera_extrinsic = {int(c_ref_)};
options_.verbose = true;
options_.max_iter = 250; // should converge much earlier
@@ -679,10 +755,19 @@ double ExtrinsicCalibration::optimize() {
LOG(INFO) << "fix distortion: " << (options_.fix_distortion ? "yes" : "no");
ba.run(options_);
- LOG(INFO) << "removed points: " << ba.removeObservations(2.0);
- ba.run(options_);
- LOG(INFO) << "removed points: " << ba.removeObservations(1.0);
- ba.run(options_);
+
+ int n_removed = 0;
+ for (const auto& t : prune_observations_) {
+ n_removed += ba.removeObservations(t);
+ if (float(n_removed)/float(n_points) > threhsold_warning_) {
+ LOG(WARNING) << "significant number of observations removed";
+ break;
+ }
+ else {
+ LOG(INFO) << "removed observations: " << n_removed;
+ ba.run(options_);
+ }
+ }
calib_optimized_.resize(calib_.size());
rmse_.resize(calib_.size());
diff --git a/components/calibration/src/optimize.cpp b/components/calibration/src/optimize.cpp
index 26a28d5c99c6067c386fc33154f43c11311ca616..73788380cb14d09f0eaa50d4791a5e52b6b25a6e 100644
--- a/components/calibration/src/optimize.cpp
+++ b/components/calibration/src/optimize.cpp
@@ -157,8 +157,7 @@ Mat Camera::distortionCoefficients() const {
Mat Camera::rvec() const {
cv::Mat rvec(cv::Size(3, 1), CV_64FC1);
CHECK_EQ(rvec.step1(), 3);
- ceres::QuaternionToAngleAxis(data + Parameter::ROTATION,
- (double*)(rvec.data));
+ ceres::QuaternionToAngleAxis(data + Parameter::ROTATION, (double*)(rvec.data));
return rvec;
}
@@ -361,7 +360,7 @@ double ftl::calibration::optimizeScale(const vector<Point3d> &object_points, vec
vector<double> d;
ceres::Problem problem;
- auto loss_function = new ceres::HuberLoss(1.0);
+ ceres::LossFunction* loss_function = nullptr;
// use all pairwise distances
for (size_t i = 0; i < object_points.size(); i++) {
@@ -612,7 +611,7 @@ void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, co
}
void BundleAdjustment::_buildProblem(ceres::Problem &problem, const BundleAdjustment::Options &options) {
-
+ CHECK(options.use_quaternion) << "Only quaternion rotations are supported";
_buildBundleAdjustmentProblem(problem, options);
_buildLengthProblem(problem, options);
}
diff --git a/components/calibration/src/structures.cpp b/components/calibration/src/structures.cpp
index 8c2c9ec5f5457d7c20fb92d8dc784fe5b276e81c..de8b34bee70e91fb67fc2098c5e51ce66773c783 100644
--- a/components/calibration/src/structures.cpp
+++ b/components/calibration/src/structures.cpp
@@ -87,7 +87,7 @@ cv::Mat CalibrationData::Intrinsic::matrix(cv::Size size) const {
}
bool CalibrationData::Intrinsic::valid() const {
- return (resolution == cv::Size{0, 0});
+ return (resolution != cv::Size{0, 0});
}
////////////////////////////////////////////////////////////////////////////////
@@ -247,3 +247,16 @@ CalibrationData::Calibration& CalibrationData::get(ftl::codecs::Channel channel)
bool CalibrationData::hasCalibration(ftl::codecs::Channel channel) const {
return data.count(channel) != 0;
}
+
+// ==== CalibrationData::Calibration ===========================================
+#include <loguru.hpp>
+cv::Mat CalibrationData::Calibration::matrix() {
+ if (!intrinsic.valid() || !extrinsic.valid()) {
+ throw ftl::exception("Invalid calibration");
+ }
+
+ cv::Mat P = extrinsic.matrix();
+ cv::Mat R = P(cv::Rect(0, 0, 3, 3));
+ R = intrinsic.matrix() * R;
+ return P;
+}