diff --git a/applications/reconstruct/include/ftl/registration.hpp b/applications/reconstruct/include/ftl/registration.hpp
index 4c37d63821f3e81df1e841e2e5ad16862d5489b1..d75d96efae5b168dd5a4065acc54d19d8d87f1ed 100644
--- a/applications/reconstruct/include/ftl/registration.hpp
+++ b/applications/reconstruct/include/ftl/registration.hpp
@@ -2,6 +2,7 @@
#define _FTL_RECONSTRUCT_REGISTRATION_HPP_
#include <ftl/config.h>
+#include <ftl/configurable.hpp>
#include <ftl/rgbd.hpp>
#include <opencv2/opencv.hpp>
@@ -13,23 +14,193 @@
namespace ftl {
namespace registration {
-/* Find transformation matrix for transforming clouds_source to clouds_target
- * Assumes that corresponding points in clouds_source[i] and clouds_target[i] have same indices
+void to_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4f> &transformations);
+void from_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4f> &transformations);
+
+bool loadTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4f> &data);
+bool saveTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4f> &data);
+
+/** @brief Find transformation matrix for transforming clouds_source to clouds_target.
+ * Assumes that corresponding points in clouds_source[i] and clouds_target[i] have same indices.
*/
Eigen::Matrix4f findTransformation( std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds_source,
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds_target);
-/* Convert chessboard corners found with OpenCV's findChessboardCorners to PCL point cloud.
- */
+/** @brief Convert chessboard corners found with OpenCV's findChessboardCorners to PCL point cloud. */
pcl::PointCloud<pcl::PointXYZ>::Ptr cornersToPointCloud(const std::vector<cv::Point2f> &corners, const cv::Mat &disp, const ftl::rgbd::CameraParameters &p);
-/* Find chessboard corners from image.
+/** @brief Find chessboard corners from image and store them in PCL PointCloud.
+ * @note Corners will be drawn in rgb.
+ */
+bool findChessboardCorners(cv::Mat &rgb, const cv::Mat &depth, const ftl::rgbd::CameraParameters &p, const cv::Size pattern_size, pcl::PointCloud<pcl::PointXYZ>::Ptr &out, float error_threshold);
+
+/**
+ * @brief Abstract class for registration
+ *
+ * Registration procedure
+ *
+ * @todo Support for multiple features (possibly necessary for other algorithms).
+ */
+class Registration : public ftl::Configurable {
+public:
+ Registration(nlohmann::json &config);
+ void addSource(ftl::rgbd::RGBDSource* source);
+ size_t getSourcesCount() { return sources_.size(); }
+
+ /**
+ * @brief Run registration loop.
+ *
+ * Loop terminates when processData() returns false.
+ */
+ virtual void run();
+
+ /**
+ * @brief Find registration transformations. run() must be called before
+ * findTransformations(). Transformations are in same order as
+ * sources were added with addSource().
+ *
+ * Transformations are calculated to targetsource if configured. If
+ * targetsource is not configured or is not found in inputs, which target
+ * coordinate system is used depends on sub-classes' implementation.
+ *
+ * @param Output parameter for transformations.
+ * @return True if transformations found, otherwise false.
+ */
+ virtual bool findTransformations(std::vector<Eigen::Matrix4f> &data)=0;
+
+ /**
+ * @brief Overload of findTransformations(). Map keys are source URIs.
+ */
+ virtual bool findTransformations(std::map<std::string, Eigen::Matrix4f> &data);
+
+protected:
+ ftl::rgbd::RGBDSource* getSource(size_t idx);
+
+ bool isTargetSourceSet();
+ bool isTargetSourceFound();
+ bool isTargetSource(ftl::rgbd::RGBDSource* source);
+ bool isTargetSource(size_t idx);
+
+ /**
+ * @brief Get index for target source. If target source not defined
+ * returns 0. If target source is not found, returns 0.
+ */
+ size_t getTargetSourceIdx();
+
+ /**
+ * @brief Resets visibility matrix.
+ */
+ void resetVisibility();
+
+ /**
+ * @brief Check if there is enough data to cover all the cameras;
+ * that is visibility is a connected graph. Implemented with BFS.
+ * Implementations of processData() in sub-classes may use this
+ * method (but it is not required).
+ *
+ * @todo Add support for checking visibility for each different features
+ * found in all images. Also see findFeatures().
+ */
+ bool connectedVisibility();
+
+ /**
+ * @brief Check if there are enough data for all the sources to calculate
+ * transformations. Method may also implement additional processing
+ * for the data. Called once after iteration round. run() stops
+ * when processData() returns false.
+ */
+ virtual bool processData()=0;
+
+ /**
+ * @brief Find features in source, return
+ *
+ * Called iteratively n times for every input (n * inputs.size()). Exact
+ * details depend on implementation of processData(). Implementations may
+ * choose not to set/use visibility information.
+ *
+ * @param Input source
+ * @param Unique index for source provided by run(). Same source will
+ * always have same idx. Implementations may choose to ignore it.
+ * @return True/false if feature was found. Used to build adjacency matrix.
+ */
+ virtual bool findFeatures(ftl::rgbd::RGBDSource* source, size_t idx)=0;
+
+ std::vector<std::vector<bool>> visibility_; /*< Adjacency matrix for sources (feature visibility). */
+
+private:
+ std::optional<std::string> target_source_; /*< Reference coordinate system for transformations. */
+ std::vector<ftl::rgbd::RGBDSource*> sources_;
+};
+
+/**
+ * @brief Registration using chessboard calibration pattern
+ *
+ * Parameters from configuration:
+ *
+ * patternsize: required
+ * Chessboard pattern size, inner corners.
+ *
+ * maxerror: default +inf
+ * Maximum allowed error value for pattern detection. MSE error between
+ * estimated plane and points captured from input.
+ *
+ * delay: default 500
+ * Milliseconds between captured images.
+ *
+ * chain: default false
+ * Enabling allows camera chaining. In chain mode, pattern is not
+ * required to be visible in every source. In default (chain: false)
+ * mode, pattern visibility is required for every source.
+ *
+ * iter: default 10
+ * Number of iterations for capturing calibration samples. In
+ * non-chaining mode, each iteration consists of images where patterns
+ * were detected on every input. In chaining mode each iteration only
+ * requires camera visibility to be connected.
*/
-bool findChessboardCorners(cv::Mat &rgb, const cv::Mat &disp, const ftl::rgbd::CameraParameters &p, const cv::Size pattern_size, pcl::PointCloud<pcl::PointXYZ>::Ptr &out, float error_threshold);
+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;
+ bool findTransformations(std::vector<Eigen::Matrix4f> &data) override;
+
+protected:
+ bool findFeatures(ftl::rgbd::RGBDSource* source, size_t idx) override;
+ bool processData() override;
+ cv::Size pattern_size_;
+ std::vector<std::vector<std::optional<pcl::PointCloud<pcl::PointXYZ>::Ptr>>> data_;
+ std::vector<Eigen::Matrix4f> T_;
+ float error_threshold_;
+ uint delay_;
+ uint iter_;
+ uint iter_remaining_;
};
+
+/**
+ * @brief Chain registration. Finds visibility and then runs registration.
+ */
+class ChessboardRegistrationChain : public ChessboardRegistration {
+public:
+ ChessboardRegistrationChain(nlohmann::json &config);
+
+ bool findTransformations(std::vector<Eigen::Matrix4f> &data) override;
+
+protected:
+ bool processData() override;
+ std::vector<Eigen::Matrix4f> T_;
+ std::vector<std::vector<std::pair<size_t, size_t>>> edges_;
};
+}
+}
+
#endif // HAVE_PCL
-#endif // _FTL_RECONSTRUCT_REGISTRATION_HPP_
+#endif // _FTL_RECONSTRUCT_REGISTRATION_HPP_
\ No newline at end of file
diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index 3c561795f0598f002f9d3743e02e54c2d26932ed..4ebc6fbd37fc8fe9e68a632f03bac386b940eb8d 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -60,168 +60,10 @@ using std::unique_lock;
using std::vector;
-#ifdef HAVE_PCL
-using pcl::PointCloud;
-using pcl::PointXYZ;
-using pcl::PointXYZRGB;
-#endif
-
using cv::Mat;
-namespace ftl {
-namespace rgbd {
-PointCloud<PointXYZRGB>::Ptr createPointCloud(RGBDSource *src);
-PointCloud<PointXYZRGB>::Ptr createPointCloud(RGBDSource *src, const cv::Point3_<uchar> &col);
-}
-}
-
-PointCloud<PointXYZRGB>::Ptr ftl::rgbd::createPointCloud(RGBDSource *src) {
- const double CX = src->getParameters().cx;
- const double CY = src->getParameters().cy;
- const double FX = src->getParameters().fx;
- const double FY = src->getParameters().fy;
-
- cv::Mat rgb;
- cv::Mat depth;
- src->getRGBD(rgb, depth);
-
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
- point_cloud_ptr->width = rgb.cols * rgb.rows;
- point_cloud_ptr->height = 1;
-
- for(int i=0;i<rgb.rows;i++) {
- const float *sptr = depth.ptr<float>(i);
- for(int j=0;j<rgb.cols;j++) {
- float d = sptr[j]; // * 1000.0f;
-
- pcl::PointXYZRGB point;
- point.x = (((double)j + CX) / FX) * d;
- point.y = (((double)i + CY) / FY) * d;
- point.z = d;
-
- if (point.x == INFINITY || point.y == INFINITY || point.z > 20.0f || point.z < 0.04f) {
- point.x = 0.0f; point.y = 0.0f; point.z = 0.0f;
- }
-
- cv::Point3_<uchar> prgb = rgb.at<cv::Point3_<uchar>>(i, j);
- uint32_t rgb = (static_cast<uint32_t>(prgb.z) << 16 | static_cast<uint32_t>(prgb.y) << 8 | static_cast<uint32_t>(prgb.x));
- point.rgb = *reinterpret_cast<float*>(&rgb);
-
- point_cloud_ptr -> points.push_back(point);
- }
- }
-
- return point_cloud_ptr;
-}
-
-PointCloud<PointXYZRGB>::Ptr ftl::rgbd::createPointCloud(RGBDSource *src, const cv::Point3_<uchar> &prgb) {
- const double CX = src->getParameters().cx;
- const double CY = src->getParameters().cy;
- const double FX = src->getParameters().fx;
- const double FY = src->getParameters().fy;
-
- cv::Mat rgb;
- cv::Mat depth;
- src->getRGBD(rgb, depth);
-
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
- point_cloud_ptr->width = rgb.cols * rgb.rows;
- point_cloud_ptr->height = 1;
-
- for(int i=0;i<rgb.rows;i++) {
- const float *sptr = depth.ptr<float>(i);
- for(int j=0;j<rgb.cols;j++) {
- float d = sptr[j]; // * 1000.0f;
-
- pcl::PointXYZRGB point;
- point.x = (((double)j + CX) / FX) * d;
- point.y = (((double)i + CY) / FY) * d;
- point.z = d;
-
- if (point.x == INFINITY || point.y == INFINITY || point.z > 20.0f || point.z < 0.04f) {
- point.x = 0.0f; point.y = 0.0f; point.z = 0.0f;
- }
-
- uint32_t rgb = (static_cast<uint32_t>(prgb.z) << 16 | static_cast<uint32_t>(prgb.y) << 8 | static_cast<uint32_t>(prgb.x));
- point.rgb = *reinterpret_cast<float*>(&rgb);
-
- point_cloud_ptr -> points.push_back(point);
- }
- }
-
- return point_cloud_ptr;
-}
-
-
-#ifdef HAVE_PCL
-#include <pcl/filters/uniform_sampling.h>
-
-/*
-class InputMerged {
-private:
- // todo: Abstract input systems, can also use other 3D inputs (like InputMerged)
- vector<InputStereo> inputs_;
- vector<Eigen::Matrix4f> T_;
-
-public:
- PointCloud<PointXYZRGB>::Ptr getPointCloud() {
- PointCloud<PointXYZRGB>::Ptr result(new PointCloud<PointXYZRGB>);
- for (size_t i = 0; i < T_.size(); i++) {
- inputs_[i].lock();
- PointCloud<PointXYZRGB>::Ptr cloud = inputs_[i].getPointCloud();
- // Documentation: Can be used with cloud_in equal to cloud_out
- pcl::transformPointCloud(*cloud, *cloud, T_[i]);
- *result += *cloud;
- }
-
- PointCloud<PointXYZRGB>::Ptr result_sampled(new PointCloud<PointXYZRGB>);
- pcl::UniformSampling<PointXYZRGB> uniform_sampling;
- uniform_sampling.setInputCloud(result);
- uniform_sampling.setRadiusSearch(0.1f); // todo parametrize
- uniform_sampling.filter(*result_sampled);
-
- return result_sampled;
- }
-};
-*/
-std::map<string, Eigen::Matrix4f> loadRegistration() {
- std::map<string, Eigen::Matrix4f> registration;
- std::ifstream file(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json");
-
- // Use identity transform if no registration
- if (!file.is_open()) {
- LOG(ERROR) << "Error loading registration from file!";
- return registration;
- }
-
- nlohmann::json load;
- file >> load;
-
- for (auto it = load.begin(); it != load.end(); ++it) {
- Eigen::Matrix4f m;
- auto data = m.data();
-
- for(size_t i = 0; i < 16; i++) {;
- data[i] = it.value()[i];
- }
-
- registration[it.key()] = m;
-
- }
- return registration;
-}
-
-void saveRegistration(std::map<string, Eigen::Matrix4f> registration) {
- nlohmann::json save;
- for (auto &item : registration) {
- auto val = nlohmann::json::array();
- for(size_t j = 0; j < 16; j++) { val.push_back((float) item.second.data()[j]); }
- save[item.first] = val;
- }
-
- std::ofstream file(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json");
- file << save;
-}
+using ftl::registration::loadTransformations;
+using ftl::registration::saveTransformations;
struct Cameras {
RGBDSource *source;
@@ -229,96 +71,12 @@ struct Cameras {
DepthCameraParams params;
};
-template <template<class> class Container>
-std::map<string, Eigen::Matrix4f> runRegistration(ftl::Configurable *root, ftl::net::Universe &net, Container<Cameras> &inputs) {
- std::map<string, Eigen::Matrix4f> registration;
-
- // NOTE: uses config["registration"]
-
- if (!root->getConfig()["registration"].is_object()) {
- LOG(FATAL) << "Configuration missing \"registration\" entry!";
- return registration;
- }
-
- int iter = (int) root->getConfig()["registration"]["calibration"]["iterations"];
- int delay = (int) root->getConfig()["registration"]["calibration"]["delay"];
- float max_error = (int) root->getConfig()["registration"]["calibration"]["max_error"];
- string ref_uri = (string) root->getConfig()["registration"]["reference-source"];
- cv::Size pattern_size( (int) root->getConfig()["registration"]["calibration"]["patternsize"][0],
- (int) root->getConfig()["registration"]["calibration"]["patternsize"][1]);
-
- // config["registration"] done
-
- size_t ref_i;
- bool found = false;
- for (size_t i = 0; i < inputs.size(); i++) {
- if (inputs[i].source->getConfig()["uri"] == ref_uri) {
- ref_i = i;
- found = true;
- break;
- }
- }
-
- if (!found) { LOG(ERROR) << "Reference input not found!"; return registration; }
-
- for (auto &input : inputs) {
- cv::namedWindow("Registration: " + (string)input.source->getConfig()["uri"], cv::WINDOW_KEEPRATIO|cv::WINDOW_NORMAL);
- }
-
- // vector for every input: vector of point clouds of patterns
- vector<vector<PointCloud<PointXYZ>::Ptr>> patterns(inputs.size());
-
- while (iter > 0) {
- net.broadcast("grab");
- bool retval = true; // set to false if pattern not found in one of the sources
- vector<PointCloud<PointXYZ>::Ptr> patterns_iter(inputs.size());
-
- for (size_t i = 0; i < inputs.size(); i++) {
- RGBDSource *input = inputs[i].source;
- Mat rgb, depth;
-
- input->getRGBD(rgb, depth);
-
- if ((rgb.cols == 0) || (depth.cols == 0)) { retval = false; break; }
-
- retval &= ftl::registration::findChessboardCorners(rgb, depth, input->getParameters(), pattern_size, patterns_iter[i], max_error);
-
- cv::imshow("Registration: " + (string)input->getConfig()["uri"], rgb);
- }
- cv::waitKey(delay);
-
- // every camera found the pattern
- if (retval) {
- for (size_t i = 0; i < patterns_iter.size(); i++) {
- patterns[i].push_back(patterns_iter[i]);
- }
- iter--;
- }
- else { LOG(WARNING) << "Pattern not detected on all inputs";}
- }
-
- for (auto &input : inputs) { cv::destroyWindow("Registration: " + (string)input.source->getConfig()["uri"]); }
-
- for (size_t i = 0; i < inputs.size(); i++) {
- Eigen::Matrix4f T;
- if (i == ref_i) {
- T.setIdentity();
- }
- else {
- T = ftl::registration::findTransformation(patterns[i], patterns[ref_i]);
- }
- registration[(string)inputs[i].source->getConfig()["uri"]] = T;
- }
- saveRegistration(registration);
- return registration;
-}
-#endif
-
static void run(ftl::Configurable *root) {
Universe *net = ftl::create<Universe>(root, "net");
// Make sure connections are complete
- //sleep_for(milliseconds(500));
+ // sleep_for(milliseconds(500));
+ // TODO: possible to do more reliably?
net->waitConnections();
std::vector<Cameras> inputs;
@@ -328,13 +86,12 @@ static void run(ftl::Configurable *root) {
LOG(ERROR) << "No sources configured!";
return;
}
- //std::vector<Display> displays;
// TODO Allow for non-net source types
- for (auto &src : *sources) {
- RGBDSource *in = ftl::rgbd::RGBDSource::create(src, net); //new ftl::rgbd::NetSource(src, &net);
+ for (auto &src : *sources) {
+ RGBDSource *in = ftl::rgbd::RGBDSource::create(src, net);
if (!in) {
- LOG(ERROR) << "Unrecognised source: " << src;
+ LOG(ERROR) << "Unrecognized source: " << src;
} else {
auto &cam = inputs.emplace_back();
cam.source = in;
@@ -348,85 +105,78 @@ static void run(ftl::Configurable *root) {
cam.params.m_sensorDepthWorldMin = in->getParameters().minDepth;
cam.gpu.alloc(cam.params);
- //displays.emplace_back(config["display"], src["uri"]);
- LOG(INFO) << (string)src["uri"] << " loaded " << cam.params.fx;
- }
- }
-
- // Displays and Inputs configured
-
- // load point cloud transformations
-
- std::map<string, Eigen::Matrix4f> registration;
- if (root->getConfig()["registration"]["calibration"]["run"]) {
- registration = runRegistration(root, *net, inputs);
- }
- else {
- LOG(INFO) << "LOAD REG";
- registration = loadRegistration();
- }
-
- // verify that registration and configuration is valid
- // (registration includes every camera)
-
- bool valid_registration = true;
- string ref_input = root->getConfig()["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()) {
- valid_registration = false;
- LOG(ERROR) << "Camera pose for " + uri + " not found in registration!";
+ LOG(INFO) << (string) src["uri"] << " loaded " << cam.params.fx;
}
}
+
+ // TODO move this block in its own method and add automated tests
+ // for error cases
- if (registration.find(ref_input) == registration.end()) {
- LOG(WARNING) << "reference input " + ref_input + " not found in registration";
+ std::optional<json_t> merge = root->get<json_t>("merge");
+ if (!merge) {
+ LOG(WARNING) << "Input merging not configured, using only first input in configuration";
+ inputs = { inputs[0] };
}
-
- // 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() > 1) {
+ std::map<std::string, Eigen::Matrix4f> transformations;
+
+ if ((*merge)["register"]) {
+ LOG(INFO) << "Registration requested";
+
+ ftl::registration::Registration *reg = ftl::registration::ChessboardRegistration::create(*merge);
+ for (auto &input : inputs) {
+ while(!input.source->isReady()) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); }
+ reg->addSource(input.source);
+ }
+
+ reg->run();
+ if (reg->findTransformations(transformations)) {
+ if (!saveTransformations(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json", transformations)) {
+ LOG(ERROR) << "Error saving new registration";
+ };
}
+ else {
+ LOG(ERROR) << "Registration failed";
+ }
+
+ free(reg);
}
-
- 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]);
+ else {
+ if (loadTransformations(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json", transformations)) {
+ LOG(INFO) << "Loaded camera trasformations from file";
+ }
+ else {
+ LOG(ERROR) << "Error loading camera transformations from file";
+ }
}
-
- 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);
+ string uri = input.source->getURI();
+ auto T = transformations.find(uri);
+ if (T == transformations.end()) {
+ LOG(ERROR) << "Camera pose for " + uri + " not found in transformations";
+ LOG(WARNING) << "Using only first configured source";
+ // TODO: use target source if configured and found
+ inputs = { inputs[0] };
+ inputs[0].source->setPose(Eigen::Matrix4f::Identity());
+ break;
+ }
+ input.source->setPose(T->second);
}
}
+
+ // Displays and Inputs configured
LOG(INFO) << "Using sources:";
- for (auto &input : inputs) { LOG(INFO) << " " + (string) input.source->getConfig()["uri"]; }
+ for (auto &input : inputs) { LOG(INFO) << " " + (string) input.source->getURI(); }
- //vector<PointCloud<PointXYZRGB>::Ptr> clouds(inputs.size());
ftl::rgbd::Display *display = ftl::create<ftl::rgbd::Display>(root, "display");
ftl::rgbd::VirtualSource *virt = ftl::create<ftl::rgbd::VirtualSource>(root, "virtual", net);
ftl::voxhash::SceneRep *scene = ftl::create<ftl::voxhash::SceneRep>(root, "voxelhash");
virt->setScene(scene);
display->setSource(virt);
-
unsigned char frameCount = 0;
bool paused = false;
@@ -470,7 +220,6 @@ static void run(ftl::Configurable *root) {
}
frameCount++;
-
display->update();
}
}
diff --git a/applications/reconstruct/src/registration.cpp b/applications/reconstruct/src/registration.cpp
index 37be030b9cdca8aa22bb008f75e3e54cc428660b..370fcf59dbc3838b5c0de66624763ab26d994df9 100644
--- a/applications/reconstruct/src/registration.cpp
+++ b/applications/reconstruct/src/registration.cpp
@@ -26,12 +26,64 @@ namespace ftl {
namespace registration {
using ftl::rgbd::CameraParameters;
+using ftl::rgbd::RGBDSource;
+
+using std::string;
using std::vector;
+using std::pair;
+using std::map;
+using std::optional;
+
using pcl::PointCloud;
using pcl::PointXYZ;
using pcl::PointXYZRGB;
using cv::Mat;
+using Eigen::Matrix4f;
+
+void from_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
+ for (auto it = json.begin(); it != json.end(); ++it) {
+ Eigen::Matrix4f m;
+ auto data = m.data();
+ for(size_t i = 0; i < 16; i++) { data[i] = it.value()[i]; }
+ transformations[it.key()] = m;
+ }
+}
+
+void to_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
+ for (auto &item : transformations) {
+ auto val = nlohmann::json::array();
+ for(size_t i = 0; i < 16; i++) { val.push_back((float) item.second.data()[i]); }
+ json[item.first] = val;
+ }
+}
+
+bool loadTransformations(const string &path, map<string, Matrix4f> &data) {
+ std::ifstream file(path);
+ if (!file.is_open()) {
+ LOG(ERROR) << "Error loading transformations from file " << path;
+ return false;
+ }
+
+ nlohmann::json json_registration;
+ file >> json_registration;
+ from_json(json_registration, data);
+ return true;
+}
+
+bool saveTransformations(const string &path, map<string, Matrix4f> &data) {
+ nlohmann::json data_json;
+ to_json(data_json, data);
+ std::ofstream file(path);
+
+ if (!file.is_open()) {
+ LOG(ERROR) << "Error writing transformations to file " << path;
+ return false;
+ }
+
+ file << std::setw(4) << data_json;
+ return true;
+}
// todo template: fitPlane<typename T>(PointCloud<T> cloud_in, PointCloud<T> cloud_out)
//
@@ -76,11 +128,11 @@ float fitPlaneError(PointCloud<PointXYZ>::Ptr cloud_in, float distance_threshold
score += d * d;
}
- return score / cloud_proj.size();
+ return (score / cloud_proj.size()) * 10000000.0f;
}
//template<typename T = PointXYZ> typename
-PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners, const Mat &disp, const CameraParameters &p) {
+PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners, const Mat &depth, const CameraParameters &p) {
int corners_len = corners.size();
vector<cv::Vec3f> points(corners_len);
@@ -102,7 +154,7 @@ PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners
for (int i = 0; i < corners_len; i++) {
double x = corners[i].x;
double y = corners[i].y;
- double d = disp.at<float>((int) y, (int) x); // * 1000.0f; // todo: better estimation
+ double d = depth.at<float>((int) y, (int) x); // * 1000.0f; // todo: better estimation
//cv::Vec4d homg_pt = Q_ * cv::Vec4d(x, y, d, 1.0);
//cv::Vec3d p = cv::Vec3d(homg_pt.val) / homg_pt[3];
@@ -112,16 +164,13 @@ PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners
point.y = (((double)y + CY) / FY) * d; // / 1000.0f;
point.z = d;
- // no check since disparities assumed to be good in the calibration pattern
- // if (fabs(d-minDisparity) <= FLT_EPSILON)
-
cloud->push_back(point);
}
return cloud;
}
-bool findChessboardCorners(Mat &rgb, const Mat &disp, const CameraParameters &p, const cv::Size pattern_size, PointCloud<PointXYZ>::Ptr &out, float error_threshold) {
+bool findChessboardCorners(Mat &rgb, const Mat &depth, const CameraParameters &p, const cv::Size pattern_size, PointCloud<PointXYZ>::Ptr &out, float error_threshold) {
vector<cv::Point2f> corners(pattern_size.width * pattern_size.height);
#if CV_VERSION_MAJOR >= 4
@@ -133,7 +182,7 @@ bool findChessboardCorners(Mat &rgb, const Mat &disp, const CameraParameters &p,
cv::drawChessboardCorners(rgb, pattern_size, Mat(corners), retval);
if (!retval) { return false; }
- auto corners_cloud = cornersToPointCloud(corners, disp, p);
+ auto corners_cloud = cornersToPointCloud(corners, depth, p);
// simple check that the values make some sense
float error = fitPlaneError(corners_cloud, error_threshold); // should use different parameter?
LOG(INFO) << "MSE against estimated plane: " << error;
@@ -200,6 +249,323 @@ Eigen::Matrix4f findTransformation(vector<PointCloud<PointXYZ>::Ptr> clouds_sour
return T;
}
+Registration::Registration(nlohmann::json &config) :
+ ftl::Configurable(config) {
+ target_source_ = get<string>("targetsource");
+ if (!target_source_) {
+ LOG(WARNING) << "targetsource not set";
+ }
+}
+
+RGBDSource* Registration::getSource(size_t idx) {
+ return sources_[idx];
+}
+
+bool Registration::isTargetSourceSet() {
+ return (bool) target_source_;
+}
+
+bool Registration::isTargetSourceFound() {
+ for (RGBDSource* source : sources_ ) {
+ if (isTargetSource(source)) return true;
+ }
+ return false;
+}
+
+bool Registration::isTargetSource(RGBDSource *source) {
+ if (target_source_) { return source->getURI() == *target_source_; }
+ return false;
+}
+
+bool Registration::isTargetSource(size_t idx) {
+ if (idx >= sources_.size()) return false; // assert
+ return isTargetSource(sources_[idx]);
+}
+
+size_t Registration::getTargetSourceIdx() {
+ if (!target_source_) return 0;
+ for (size_t idx = 0; idx < sources_.size(); ++idx) {
+ if (isTargetSource(sources_[idx])) return idx;
+ }
+
+ return 0;
+}
+
+void Registration::addSource(RGBDSource *source) {
+ // TODO: check that source is not already included
+ sources_.push_back(source);
+}
+
+/**
+ * @param adjacency matrix
+ * @param index of starting vertex
+ * @param (out) edges connecting each level
+ * @returns true if graph connected (all vertices visited), otherwise false
+ *
+ * Breadth First Search
+ */
+bool isConnected(vector<vector<bool>> matrix, size_t start_idx, vector<vector<pair<size_t, size_t>>> &edges) {
+vector<bool> visited(matrix.size(), false);
+ DCHECK(start_idx < matrix.size());
+
+ edges.clear();
+ vector<size_t> level { start_idx };
+
+ visited[start_idx] = true;
+ size_t visited_count = 1;
+
+ while(level.size() != 0) {
+ vector<size_t> level_prev = level;
+ level = {};
+
+ vector<pair<size_t, size_t>> new_edges;
+
+ for (size_t current : level_prev) {
+ for (size_t i = 0; i < matrix.size(); ++i) {
+ if (matrix[current][i] && !visited[i]) {
+ visited[i] = true;
+ visited_count += 1;
+ level.push_back(i);
+ // could also save each level's vertices
+
+ new_edges.push_back(pair(current, i));
+ }
+ }}
+ if (new_edges.size() > 0) edges.push_back(new_edges);
+ }
+
+ return visited_count == matrix.size();
+}
+
+bool isConnected(vector<vector<bool>> matrix, size_t start_idx = 0) {
+ vector<vector<pair<size_t, size_t>>> edges;
+ return isConnected(matrix, start_idx, edges);
+}
+
+/**
+ * @param Adjacency matrix
+ * @returns Vector containing degree of each vertex
+*/
+vector<uint> verticleDegrees(vector<vector<bool>> matrix) {
+ vector<uint> res(matrix.size(), 0);
+ for (size_t i = 0; i < matrix.size(); ++i) {
+ for (size_t j = 0; j < matrix.size(); ++j) {
+ if (matrix[i][j]) res[i] = res[i] + 1;
+ }}
+ return res;
+}
+
+bool Registration::connectedVisibility() {
+ return isConnected(visibility_, getTargetSourceIdx());
+}
+
+void Registration::resetVisibility() {
+ visibility_ = vector(sources_.size(), vector<bool>(sources_.size(), false));
+}
+
+void Registration::run() {
+ resetVisibility();
+
+ do {
+ vector<bool> visible(sources_.size(), false);
+
+ for (size_t i = 0; i < sources_.size(); ++i) {
+ bool retval = findFeatures(sources_[i], i);
+ visible[i] = retval;
+ }
+
+ 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] = visibility_[i][j] || val;
+ visibility_[j][i] = visibility_[j][i] || val;
+ }}
+ }
+ while(processData());
+}
+
+bool Registration::findTransformations(map<string, Matrix4f> &data) {
+ vector<Matrix4f> T;
+ data.clear();
+
+ if (!findTransformations(T)) return false;
+ for (size_t i = 0; i < sources_.size(); ++i) {
+ data[sources_[i]->getURI()] = T[i];
+ }
+ return true;
+}
+
+ChessboardRegistration* ChessboardRegistration::create(nlohmann::json &config) {
+ if (config.value<bool>("chain", false)) {
+ return new ChessboardRegistrationChain(config);
+ }
+ else {
+ return new ChessboardRegistration(config);
+ }
+}
+
+ChessboardRegistration::ChessboardRegistration(nlohmann::json &config) :
+ Registration(config) {
+
+ auto patternsize = get<vector<int>>("patternsize");
+ if (!patternsize) { LOG(FATAL) << "Registration run enabled but pattern size not set"; }
+ pattern_size_ = cv::Size((*patternsize)[0], (*patternsize)[1]);
+
+ auto maxerror = get<float>("maxerror");
+ if (!maxerror) { LOG(WARNING) << "maxerror not set"; }
+ auto delay = get<int>("delay");
+ if (!delay) { LOG(INFO) << "delay not set in configuration"; }
+ auto iter = get<int>("iterations");
+ if (!iter) { LOG(INFO) << "iterations not set in configuration"; }
+ auto chain = get<bool>("chain");
+ if (!chain) { LOG(INFO) << "input chaining disabled"; }
+ else { LOG(INFO) << "Input chaining enabled"; }
+
+ error_threshold_ = maxerror ? *maxerror : std::numeric_limits<float>::infinity();
+ iter_ = iter ? *iter : 10;
+ delay_ = delay ? *delay : 50;
+}
+
+void ChessboardRegistration::run() {
+ if (!isTargetSourceFound()) {
+ LOG(WARNING) << "targetsource not found in sources";
+ }
+
+ if (data_.size() != getSourcesCount()) {
+ data_ = vector<vector<optional<PointCloud<PointXYZ>::Ptr>>>(getSourcesCount());
+ }
+ iter_remaining_ = iter_;
+
+ // TODO: Move GUI elsewhere. Also applies to processData() and findFeatures()
+ for (size_t i = 0; i < getSourcesCount(); ++i) {
+ cv::namedWindow("Registration: " + getSource(i)->getURI(),
+ cv::WINDOW_KEEPRATIO|cv::WINDOW_NORMAL);
+ }
+
+ Registration::run();
+
+ for (size_t i = 0; i < getSourcesCount(); ++i) {
+ cv::destroyWindow("Registration: " + getSource(i)->getURI());
+ }
+}
+
+bool ChessboardRegistration::findFeatures(RGBDSource *source, size_t idx) {
+ optional<PointCloud<PointXYZ>::Ptr> result;
+ PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
+
+ Mat rgb, depth;
+ source->getRGBD(rgb, depth);
+
+ bool retval = findChessboardCorners(rgb, depth, source->getParameters(), pattern_size_, cloud, error_threshold_);
+ if (retval) {
+ result.emplace(cloud);
+ }
+ data_[idx].push_back(result);
+
+ cv::imshow("Registration: " + source->getURI(), rgb);
+
+ return retval;
+}
+
+bool ChessboardRegistration::processData() {
+ bool retval = connectedVisibility();
+ resetVisibility();
+
+ if (retval) {
+ iter_remaining_--;
+ }
+ else{
+ LOG(INFO) << "Pattern not visible in all inputs";
+ for (auto &sample : data_) { sample.pop_back(); }
+ }
+
+ //std::this_thread::sleep_for(std::chrono::milliseconds(delay_));
+ cv::waitKey(delay_); // OpenCV GUI doesn't show otherwise
+
+ return iter_remaining_ > 0;
+}
+
+bool ChessboardRegistration::findTransformations(vector<Matrix4f> &data) {
+ data.clear();
+ vector<bool> status(getSourcesCount(), false);
+ size_t idx_target = getTargetSourceIdx();
+
+ for (size_t idx = 0; idx < getSourcesCount(); ++idx) {
+ Matrix4f T;
+ if (idx == idx_target) {
+ T.setIdentity();
+ }
+ else {
+ vector<PointCloud<PointXYZ>::Ptr> d;
+ vector<PointCloud<PointXYZ>::Ptr> d_target;
+ d.reserve(iter_);
+ d_target.reserve(iter_);
+
+ for (size_t i = 0; i < iter_; ++i) {
+ auto val = data_[idx][i];
+ auto val_target = data_[idx_target][i];
+ if (val && val_target) {
+ d.push_back(*val);
+ d_target.push_back(*val_target);
+ }
+ }
+ T = findTransformation(d, d_target);
+ }
+ data.push_back(T);
+ }
+ 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_);
+
+ if (retval) {
+ LOG(INFO) << "Chain complete, depth: " << edges_.size();
+ return false;
+ }
+ else{
+ LOG(5) << "Chain not complete ";
+ }
+
+ return true;
+}
+
+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;
+
+ vector<Matrix4f> result;
+ ChessboardRegistration reg(conf);
+ reg.addSource(getSource(edge.first));
+ reg.addSource(getSource(edge.second));
+ reg.run();
+ if (!reg.findTransformations(result)) { return false; }
+ data[edge.second] = data[edge.first] * result[1];
+ }
+ }
+
+ return true;
+}
+
} // namespace registration
} // namespace ftl
diff --git a/config/config.jsonc b/config/config.jsonc
index 8f1f8f8e4ae7acceffa9705a9c0273434b6d7086..95fc7fde61d4bfe83c386b1f442ee5f310824b84 100644
--- a/config/config.jsonc
+++ b/config/config.jsonc
@@ -149,15 +149,13 @@
"display": { "$ref": "#displays/left" },
"virtual": { "$ref": "#virtual_cams/default" },
"voxelhash": { "$ref": "#hash_conf/default" },
- "registration": {
- "reference-source" : "ftl://utu.fi#vision_default/source",
- "calibration" : {
- "max_error": 25,
- "run": false,
- "iterations" : 10,
- "delay" : 500,
- "patternsize" : [9, 6]
- }
+ "merge": {
+ "register": false,
+ "targetsource" : "ftl://utu.fi#vision_default/source",
+ "maxerror": 25,
+ "iterations" : 10,
+ "delay" : 500,
+ "patternsize" : [9, 6]
}
},
@@ -172,15 +170,14 @@
"display": { "$ref": "#displays/left" },
"virtual": { "$ref": "#virtual_cams/default" },
"voxelhash": { "$ref": "#hash_conf/default" },
- "registration": {
- "reference-source" : "ftl://utu.fi/node4#vision_default/source",
- "calibration" : {
- "max_error": 25,
- "run": false,
- "iterations" : 10,
- "delay" : 500,
- "patternsize" : [9, 6]
- }
+ "merge": {
+ "targetsource" : "ftl://utu.fi/node4#vision_default/source",
+ "register": false,
+ "chain": false,
+ "maxerror": 25,
+ "iterations" : 10,
+ "delay" : 500,
+ "patternsize" : [9, 6]
},
"stream": {}
},