Verify that registration cointains all cameras

applications/reconstruct/src/main.cpp

@@ -231,7 +231,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 +360,7 @@ static void run() {
 		return;
 	}
 	
-	std::deque<Cameras> inputs;
+	std::vector<Cameras> inputs;
 	//std::vector<Display> displays;
 
 	// TODO Allow for non-net source types
@@ -400,14 +399,49 @@ static void run() {
 		registration = loadRegistration();
 	}
 	
+	// valid registration contains all inputs
+	bool valid_registration = (registration.size() == inputs.size());
+	
+	for (auto &input : inputs) {
+		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 not valid, use reference input or first input (if reference
+	// input was not configured).
+	if (valid_registration) {
+		string ref_input = config["registration"]["reference-source"];
+		vector<Cameras> inputs_;
+		LOG(ERROR) << "Different number of inputs and registered cameras, using only reference camera: " + ref_input;
+		
+		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) << "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"]] : registration["default"];
+			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);
 		}
+	}
 	
 	//vector<PointCloud<PointXYZRGB>::Ptr> clouds(inputs.size());
 	Display display_merged(config["display"], "Merged"); // todo