diff --git a/applications/reconstruct/include/ftl/registration.hpp b/applications/reconstruct/include/ftl/registration.hpp index 547751b7f692abdb8cebb60e3c2827705feb353b..d75d96efae5b168dd5a4065acc54d19d8d87f1ed 100644 --- a/applications/reconstruct/include/ftl/registration.hpp +++ b/applications/reconstruct/include/ftl/registration.hpp @@ -162,6 +162,11 @@ private: class ChessboardRegistration : public Registration { public: ChessboardRegistration(nlohmann::json &config); + /** + * @brief Creates new ChessboardRegistration or ChessboardRegistrationChain + * object depending on chain option in config. User of the method + * needs to free the memory. + */ static ChessboardRegistration* create(nlohmann::json &config); void run() override; @@ -184,7 +189,8 @@ protected: */ class ChessboardRegistrationChain : public ChessboardRegistration { public: - ChessboardRegistrationChain(nlohmann::json &config) : ChessboardRegistration(config) {} + ChessboardRegistrationChain(nlohmann::json &config); + bool findTransformations(std::vector<Eigen::Matrix4f> &data) override; protected: diff --git a/applications/reconstruct/src/registration.cpp b/applications/reconstruct/src/registration.cpp index 9ca5c8921517eb8f8ab07b910e0f4ebd28b6b961..370fcf59dbc3838b5c0de66624763ab26d994df9 100644 --- a/applications/reconstruct/src/registration.cpp +++ b/applications/reconstruct/src/registration.cpp @@ -331,7 +331,7 @@ vector<bool> visited(matrix.size(), false); new_edges.push_back(pair(current, i)); } }} - edges.push_back(new_edges); + if (new_edges.size() > 0) edges.push_back(new_edges); } return visited_count == matrix.size(); @@ -377,8 +377,8 @@ void Registration::run() { for (size_t i = 0; i < visible.size(); ++i) { for (size_t j = 0; j < visible.size(); ++j) { bool val = visible[i] && visible[j]; - visibility_[i][j] = val; - visibility_[j][i] = val; + visibility_[i][j] = visibility_[i][j] || val; + visibility_[j][i] = visibility_[j][i] || val; }} } while(processData()); @@ -516,6 +516,11 @@ bool ChessboardRegistration::findTransformations(vector<Matrix4f> &data) { return true; } +ChessboardRegistrationChain::ChessboardRegistrationChain(nlohmann::json &config) : + ChessboardRegistration(config) { + error_threshold_ = std::numeric_limits<float>::infinity(); +} + bool ChessboardRegistrationChain::processData() { for (auto &sample : data_ ) { sample.clear(); } bool retval = isConnected(visibility_, getTargetSourceIdx(), edges_); @@ -525,7 +530,7 @@ bool ChessboardRegistrationChain::processData() { return false; } else{ - LOG(INFO) << "Chain not complete "; + LOG(5) << "Chain not complete "; } return true; @@ -534,11 +539,16 @@ bool ChessboardRegistrationChain::processData() { bool ChessboardRegistrationChain::findTransformations(vector<Matrix4f> &data) { // TODO Change to group registration: register all sources which have visibility // to the target source in chain. + 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) { + LOG(INFO) << "Registering source " + << getSource(edge.second)->getURI() << " to source" + << getSource(edge.first)->getURI(); + nlohmann::json conf(config_); conf["targetsource"] = getSource(edge.first)->getURI(); conf["chain"] = false; diff --git a/components/rgbd-sources/include/ftl/rgbd_source.hpp b/components/rgbd-sources/include/ftl/rgbd_source.hpp index 04acf11af077e6e6ef492470c1210fb188e284d3..aa5748324dc9aeddd1134556fb3153baf08c1915 100644 --- a/components/rgbd-sources/include/ftl/rgbd_source.hpp +++ b/components/rgbd-sources/include/ftl/rgbd_source.hpp @@ -29,7 +29,7 @@ class RGBDSource : public ftl::Configurable { void getRGBD(cv::Mat &rgb, cv::Mat &depth); const CameraParameters &getParameters() { return params_; }; - std::string getURI() const { return config_["uri"].get<std::string>(); } + std::string getURI() const { return config_["$id"].get<std::string>(); } virtual void setPose(const Eigen::Matrix4f &pose) { pose_ = pose; }; const Eigen::Matrix4f &getPose() { return pose_; };