From 1e46843df8484d7af08e35e5b32877e60f565049 Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Fri, 7 Jun 2019 09:54:39 +0300
Subject: [PATCH] chain works now (pairwise)
---
.../reconstruct/include/ftl/registration.hpp | 8 +++++++-
applications/reconstruct/src/registration.cpp | 18 ++++++++++++++----
.../rgbd-sources/include/ftl/rgbd_source.hpp | 2 +-
3 files changed, 22 insertions(+), 6 deletions(-)
diff --git a/applications/reconstruct/include/ftl/registration.hpp b/applications/reconstruct/include/ftl/registration.hpp
index 547751b7f..d75d96efa 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 9ca5c8921..370fcf59d 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 04acf11af..aa5748324 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_; };
--
GitLab