From 33844b24a38dd0b518910d85d0f76a16ca92ed9b Mon Sep 17 00:00:00 2001 From: Sebastian Hahta <joseha@utu.fi> Date: Thu, 6 Jun 2019 16:50:28 +0300 Subject: [PATCH] camera chaining --- .../reconstruct/include/ftl/registration.hpp | 45 ++++- applications/reconstruct/src/main.cpp | 2 +- applications/reconstruct/src/registration.cpp | 180 +++++++++++++----- config/config.jsonc | 3 +- 4 files changed, 178 insertions(+), 52 deletions(-) diff --git a/applications/reconstruct/include/ftl/registration.hpp b/applications/reconstruct/include/ftl/registration.hpp index c73b71012..500616b53 100644 --- a/applications/reconstruct/include/ftl/registration.hpp +++ b/applications/reconstruct/include/ftl/registration.hpp @@ -126,19 +126,43 @@ protected: */ virtual bool findFeatures(ftl::rgbd::RGBDSource* source, size_t idx)=0; + std::vector<std::vector<bool>> visibility_; /*< Adjacency matrix for sources (feature visibility). */ + private: std::optional<std::string> target_source_; /*< Reference coordinate system for transformations. */ std::vector<ftl::rgbd::RGBDSource*> sources_; - std::vector<std::vector<bool>> visibility_; /*< Adjacency matrix for sources (feature visibility). */ }; /** * @brief Registration using chessboard calibration pattern - * @todo Document chessboard parameters + * + * Parameters from configuration: + * + * patternsize: required + * Chessboard pattern size, inner corners. + * + * maxerror: default +inf + * Maximum allowed error value for pattern detection. MSE error between + * estimated plane and points captured from input. + * + * delay: default 500 + * Milliseconds between captured images. + * + * chain: default false + * Enabling allows camera chaining. In chain mode, pattern is not + * required to be visible in every source. In default (chain: false) + * mode, pattern visibility is required for every source. + * + * iter: default 10 + * Number of iterations for capturing calibration samples. In + * non-chaining mode, each iteration consists of images where patterns + * were detected on every input. In chaining mode each iteration only + * requires camera visibility to be connected. */ class ChessboardRegistration : public Registration { public: ChessboardRegistration(nlohmann::json &config); + static ChessboardRegistration* create(nlohmann::json &config); void run() override; bool findTransformations(std::vector<Eigen::Matrix4f> &data) override; @@ -146,16 +170,29 @@ public: protected: bool findFeatures(ftl::rgbd::RGBDSource* source, size_t idx) override; bool processData() override; - -private: cv::Size pattern_size_; std::vector<std::vector<std::optional<pcl::PointCloud<pcl::PointXYZ>::Ptr>>> data_; + std::vector<Eigen::Matrix4f> T_; float error_threshold_; uint delay_; uint iter_; uint iter_remaining_; }; +/** + * @brief Chain registration. Finds visibility and then runs pairwise registration. + */ +class ChessboardRegistrationChain : public ChessboardRegistration { +public: + ChessboardRegistrationChain(nlohmann::json &config) : ChessboardRegistration(config) {} + bool findTransformations(std::vector<Eigen::Matrix4f> &data) override; + +protected: + bool processData() override; + std::vector<Eigen::Matrix4f> T_; + std::vector<std::vector<std::pair<size_t, size_t>>> edges_; +}; + } } diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp index ca4630ecf..4ebc6fbd3 100644 --- a/applications/reconstruct/src/main.cpp +++ b/applications/reconstruct/src/main.cpp @@ -124,7 +124,7 @@ static void run(ftl::Configurable *root) { if ((*merge)["register"]) { LOG(INFO) << "Registration requested"; - ftl::registration::Registration *reg = new ftl::registration::ChessboardRegistration(*merge); + ftl::registration::Registration *reg = ftl::registration::ChessboardRegistration::create(*merge); for (auto &input : inputs) { while(!input.source->isReady()) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); } reg->addSource(input.source); diff --git a/applications/reconstruct/src/registration.cpp b/applications/reconstruct/src/registration.cpp index 55891d57f..abe5f95b2 100644 --- a/applications/reconstruct/src/registration.cpp +++ b/applications/reconstruct/src/registration.cpp @@ -30,6 +30,7 @@ using ftl::rgbd::RGBDSource; using std::string; using std::vector; +using std::pair; using std::map; using std::optional; @@ -295,25 +296,67 @@ void Registration::addSource(RGBDSource *source) { sources_.push_back(source); } -bool Registration::connectedVisibility() { - vector<bool> visited(visibility_.size(), false); - std::queue<size_t> next; - - visited[0] = true; +/** + * @param adjacency matrix + * @param index of starting vertex + * @param (out) edges connecting each level + * @returns true if graph connected (all vertices visited), otherwise false + * + * Breadth First Search + */ +bool isConnected(vector<vector<bool>> matrix, size_t start_idx, vector<vector<pair<size_t, size_t>>> &edges) { +vector<bool> visited(matrix.size(), false); + DCHECK(start_idx < matrix.size()); + + edges.clear(); + vector<size_t> level { start_idx }; + + visited[start_idx] = true; size_t visited_count = 1; - next.push(0); - while(next.size() > 0) { - size_t current = next.front(); next.pop(); - for (size_t i = 0; i < visibility_.size(); ++i) { - if (visibility_[current][i] && !visited[i]) { + while(level.size() != 0) { + vector<size_t> level_prev = level; + level = {}; + + vector<pair<size_t, size_t>> new_edges; + + for (size_t current : level_prev) { + for (size_t i = 0; i < matrix.size(); ++i) { + if (matrix[current][i] && !visited[i]) { visited[i] = true; visited_count += 1; - next.push(i); + level.push_back(i); + // could also save each level's vertices + + new_edges.push_back(pair(current, i)); } - } + }} + edges.push_back(new_edges); } - return visited_count == visibility_.size(); + + return visited_count == matrix.size(); +} + +bool isConnected(vector<vector<bool>> matrix, size_t start_idx = 0) { + vector<vector<pair<size_t, size_t>>> edges; + return isConnected(matrix, start_idx, edges); +} + +/** + * @param Adjacency matrix + * @returns Vector containing degree of each vertex +*/ +vector<uint> verticleDegrees(vector<vector<bool>> matrix) { + vector<uint> res(matrix.size(), 0); + for (size_t i = 0; i < matrix.size(); ++i) { + for (size_t j = 0; j < matrix.size(); ++j) { + if (matrix[i][j]) res[i] = res[i] + 1; + }} + return res; +} + +bool Registration::connectedVisibility() { + return isConnected(visibility_, getTargetSourceIdx()); } void Registration::resetVisibility() { @@ -352,6 +395,15 @@ bool Registration::findTransformations(map<string, Matrix4f> &data) { return true; } +ChessboardRegistration* ChessboardRegistration::create(nlohmann::json &config) { + if (config.value<bool>("chain", false)) { + return new ChessboardRegistrationChain(config); + } + else { + return new ChessboardRegistration(config); + } +} + ChessboardRegistration::ChessboardRegistration(nlohmann::json &config) : Registration(config) { @@ -365,10 +417,36 @@ ChessboardRegistration::ChessboardRegistration(nlohmann::json &config) : if (!delay) { LOG(INFO) << "delay not set in configuration"; } auto iter = get<int>("iterations"); if (!iter) { LOG(INFO) << "iterations not set in configuration"; } - + auto chain = get<bool>("chain"); + if (!chain) { LOG(INFO) << "input chaining disabled"; } + else { LOG(INFO) << "Input chaining enabled"; } + error_threshold_ = maxerror ? *maxerror : std::numeric_limits<float>::infinity(); iter_ = iter ? *iter : 10; - delay_ = delay ? *delay : 50; + delay_ = delay ? *delay : 50; +} + +void ChessboardRegistration::run() { + if (!isTargetSourceFound()) { + LOG(WARNING) << "targetsource not found in sources"; + } + + if (data_.size() != getSourcesCount()) { + data_ = vector<vector<optional<PointCloud<PointXYZ>::Ptr>>>(getSourcesCount()); + } + iter_remaining_ = iter_; + + // TODO: Move GUI elsewhere. Also applies to processData() and findFeatures() + for (size_t i = 0; i < getSourcesCount(); ++i) { + cv::namedWindow("Registration: " + getSource(i)->getURI(), + cv::WINDOW_KEEPRATIO|cv::WINDOW_NORMAL); + } + + Registration::run(); + + for (size_t i = 0; i < getSourcesCount(); ++i) { + cv::destroyWindow("Registration: " + getSource(i)->getURI()); + } } bool ChessboardRegistration::findFeatures(RGBDSource *source, size_t idx) { @@ -390,10 +468,6 @@ bool ChessboardRegistration::findFeatures(RGBDSource *source, size_t idx) { } bool ChessboardRegistration::processData() { - // TODO: Check there is enough samples for each input source. - // TODO: With multiple samples which are not totally connected, find the - // smallest subset which is connected. - bool retval = connectedVisibility(); resetVisibility(); @@ -401,43 +475,19 @@ bool ChessboardRegistration::processData() { iter_remaining_--; } else{ - // remove samples and try again - // TODO: decide what to do in findTransformations() instead LOG(INFO) << "Pattern not visible in all inputs"; for (auto &sample : data_) { sample.pop_back(); } } - + //std::this_thread::sleep_for(std::chrono::milliseconds(delay_)); cv::waitKey(delay_); // OpenCV GUI doesn't show otherwise return iter_remaining_ > 0; } -void ChessboardRegistration::run() { - if (!isTargetSourceFound()) { - LOG(WARNING) << "targetsource not found in sources"; - } - - if (data_.size() != getSourcesCount()) { - data_ = vector<vector<optional<PointCloud<PointXYZ>::Ptr>>>(getSourcesCount()); - } - iter_remaining_ = iter_; - - // TODO: Move GUI elsewhere. Also applies to processData() and findFeatures() - for (size_t i = 0; i < getSourcesCount(); ++i) { - cv::namedWindow("Registration: " + getSource(i)->getURI(), - cv::WINDOW_KEEPRATIO|cv::WINDOW_NORMAL); - } - - Registration::run(); - - for (size_t i = 0; i < getSourcesCount(); ++i) { - cv::destroyWindow("Registration: " + getSource(i)->getURI()); - } -} - bool ChessboardRegistration::findTransformations(vector<Matrix4f> &data) { data.clear(); + vector<bool> status(getSourcesCount(), false); size_t idx_target = getTargetSourceIdx(); for (size_t idx = 0; idx < getSourcesCount(); ++idx) { @@ -461,12 +511,50 @@ bool ChessboardRegistration::findTransformations(vector<Matrix4f> &data) { } T = findTransformation(d, d_target); } - data.push_back(T); } return true; } +bool ChessboardRegistrationChain::processData() { + for (auto &sample : data_ ) { sample.clear(); } + bool retval = isConnected(visibility_, getTargetSourceIdx(), edges_); + + if (retval) { + LOG(INFO) << "Chain complete, depth: " << edges_.size(); + return false; + } + else{ + LOG(INFO) << "Chain not complete "; + } + + return true; +} + +bool ChessboardRegistrationChain::findTransformations(vector<Matrix4f> &data) { + LOG(INFO) << "Running pairwise registration"; + data = vector<Matrix4f>(getSourcesCount(), Matrix4f::Identity()); + + for (vector<pair<size_t, size_t>> level : edges_) { + for (pair<size_t, size_t> edge : level) { + nlohmann::json conf(config_); + conf["targetsource"] = getSource(edge.first)->getURI(); + conf["chain"] = false; + + + vector<Matrix4f> result; + ChessboardRegistration reg(conf); + reg.addSource(getSource(edge.first)); + reg.addSource(getSource(edge.second)); + reg.run(); + if (!reg.findTransformations(result)) { return false; } + data[edge.second] = data[edge.first] * result[1]; + } + } + + return true; +} + } // namespace registration } // namespace ftl diff --git a/config/config.jsonc b/config/config.jsonc index 493333317..95fc7fde6 100644 --- a/config/config.jsonc +++ b/config/config.jsonc @@ -171,8 +171,9 @@ "virtual": { "$ref": "#virtual_cams/default" }, "voxelhash": { "$ref": "#hash_conf/default" }, "merge": { - "register": false, "targetsource" : "ftl://utu.fi/node4#vision_default/source", + "register": false, + "chain": false, "maxerror": 25, "iterations" : 10, "delay" : 500, -- GitLab