From 38b3e8d96345372795c843209f69f209f054d20c Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Wed, 29 May 2019 10:32:56 +0300
Subject: [PATCH] Resolve "Do not use registration with singe RGB-D input"
---
applications/reconstruct/src/main.cpp | 64 ++++++++++++++++++++++-----
1 file changed, 53 insertions(+), 11 deletions(-)
diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index a74c03a1c..b63c0885a 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -189,8 +189,7 @@ std::map<string, Eigen::Matrix4f> loadRegistration() {
// Use identity transform if no registration
if (!file.is_open()) {
- Eigen::Matrix4f T;
- registration["default"] = T.setIdentity();
+ LOG(ERROR) << "Error loading registration from file!";
return registration;
}
@@ -231,7 +230,6 @@ struct Cameras {
template <template<class> class Container>
std::map<string, Eigen::Matrix4f> runRegistration(ftl::net::Universe &net, Container<Cameras> &inputs) {
-
std::map<string, Eigen::Matrix4f> registration;
// NOTE: uses config["registration"]
@@ -361,7 +359,7 @@ static void run() {
return;
}
- std::deque<Cameras> inputs;
+ std::vector<Cameras> inputs;
//std::vector<Display> displays;
// TODO Allow for non-net source types
@@ -399,16 +397,60 @@ static void run() {
LOG(INFO) << "LOAD REG";
registration = loadRegistration();
}
-
- LOG(INFO) << "Assigning poses";
- vector<Eigen::Matrix4f> T;
+
+ // verify that registration and configuration is valid
+ // (registration includes every camera)
+
+ bool valid_registration = true;
+ string ref_input = config["registration"]["reference-source"];
+
+ // check every camera is included in registration
for (auto &input : inputs) {
- LOG(INFO) << (unsigned long long)input.source;
- Eigen::Matrix4f RT = (registration.count(input.source->getConfig()["uri"].get<string>()) > 0) ? registration[(string)input.source->getConfig()["uri"]] : registration["default"];
- T.push_back(RT);
- input.source->setPose(RT);
+ string uri = input.source->getConfig()["uri"];
+ if (registration.find(uri) == registration.end()) {
+ valid_registration = false;
+ LOG(ERROR) << "Camera pose for " + uri + " not found in registration!";
+ }
+ }
+
+ if (registration.find(ref_input) == registration.end()) {
+ LOG(WARNING) << "reference input " + ref_input + " not found in registration";
}
+ // if registration not valid, use reference input or first input
+ if (!valid_registration) {
+ vector<Cameras> inputs_;
+
+ for (auto &input : inputs) {
+ if ((string) input.source->getConfig()["uri"] == ref_input) {
+ inputs_.push_back(input);
+ break;
+ }
+ }
+
+ if (inputs_.size() == 0) {
+ LOG(ERROR) << "Reference input not found in configured inputs, using first input: " + (string) inputs[0].source->getConfig()["uri"];
+ inputs_.push_back(inputs[0]);
+ }
+
+ inputs = inputs_;
+ inputs[0].source->setPose(Eigen::Matrix4f::Identity());
+ }
+ else {
+ LOG(INFO) << "Registration valid, assigning poses";
+ vector<Eigen::Matrix4f> T;
+ for (auto &input : inputs) {
+ LOG(INFO) << (unsigned long long)input.source;
+ Eigen::Matrix4f RT = (registration.count(input.source->getConfig()["uri"].get<string>()) > 0) ?
+ registration[(string)input.source->getConfig()["uri"]] : Eigen::Matrix4f::Identity();
+ T.push_back(RT);
+ input.source->setPose(RT);
+ }
+ }
+
+ LOG(INFO) << "Using sources:";
+ for (auto &input : inputs) { LOG(INFO) << " " + (string) input.source->getConfig()["uri"]; }
+
//vector<PointCloud<PointXYZRGB>::Ptr> clouds(inputs.size());
Display display_merged(config["display"], "Merged"); // todo
CUDARayCastSDF rays(config["voxelhash"]);
--
GitLab