diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp index 0794163cdd8bd7bcc4f4a66d787f3562a9096eff..b63c0885a4e8fa631f759a41138ec490c379a2be 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; } @@ -399,9 +398,13 @@ static void run() { registration = loadRegistration(); } - // valid registration contains all inputs - bool valid_registration = (registration.size() == inputs.size()); + // 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) { string uri = input.source->getConfig()["uri"]; if (registration.find(uri) == registration.end()) { @@ -410,12 +413,13 @@ static void run() { } } - // 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"]; + 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_; - 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) { @@ -425,24 +429,28 @@ static void run() { } if (inputs_.size() == 0) { - LOG(ERROR) << "reference input not found in configured inputs, using first input: " + (string) inputs[0].source->getConfig()["uri"]; + 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"; + 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(); + 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"]);