diff --git a/applications/registration/CMakeLists.txt b/applications/registration/CMakeLists.txt
index 6597192ca991425405641b534cd68607fbb605e1..8b27dcb38f13a92f2988d963e250c1028b6712f8 100644
--- a/applications/registration/CMakeLists.txt
+++ b/applications/registration/CMakeLists.txt
@@ -3,6 +3,8 @@ set(REGSRC
src/manual.cpp
src/correspondances.cpp
src/sfm.cpp
+ src/aruco.cpp
+ src/common.cpp
)
add_executable(ftl-register ${REGSRC})
diff --git a/applications/registration/src/aruco.cpp b/applications/registration/src/aruco.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..28cddabea859cdef754c80d9303099eea03dbc45
--- /dev/null
+++ b/applications/registration/src/aruco.cpp
@@ -0,0 +1,210 @@
+#include "aruco.hpp"
+#include "common.hpp"
+#include "correspondances.hpp"
+
+#include <ftl/net/universe.hpp>
+#include <ftl/rgbd/source.hpp>
+
+#include <opencv2/aruco.hpp>
+
+using ftl::registration::aruco;
+using ftl::registration::Correspondances;
+using std::map;
+using std::string;
+using std::vector;
+using ftl::Configurable;
+using ftl::net::Universe;
+using ftl::rgbd::Source;
+using std::pair;
+using std::make_pair;
+
+void ftl::registration::aruco(ftl::Configurable *root) {
+ using namespace cv;
+
+ Universe *net = ftl::create<Universe>(root, "net");
+
+ net->start();
+ net->waitConnections();
+
+ auto sources = ftl::createArray<Source>(root, "sources", net);
+
+ int curtarget = 0;
+ bool freeze = false;
+ bool depthtoggle = false;
+ double lastScore = 1000.0;
+
+ if (sources.size() == 0) return;
+
+ cv::namedWindow("Target", cv::WINDOW_KEEPRATIO);
+ cv::namedWindow("Source", cv::WINDOW_KEEPRATIO);
+
+ map<string, Eigen::Matrix4d> oldTransforms;
+ ftl::registration::loadTransformations(root->value("output", string("./test.json")), oldTransforms);
+
+ //Correspondances c(sources[targsrc], sources[cursrc]);
+ map<string, Correspondances*> corrs;
+ ftl::registration::build_correspondances(sources, corrs, root->value("origin", 0), oldTransforms);
+
+ Correspondances *current = corrs[sources[curtarget]->getURI()];
+
+ map<int, vector<Point2f>> targ_corners;
+ map<int, vector<Point2f>> src_corners;
+ map<int, pair<Vec3d,Vec3d>> targ_trans;
+ map<int, pair<Vec3d,Vec3d>> src_trans;
+
+ while (ftl::running) {
+ if (!freeze) {
+ // Grab the latest frames from sources
+ for (int i=0; i<sources.size(); i++) {
+ if (sources[i]->isReady()) {
+ sources[i]->grab();
+ }
+ }
+ }
+
+ // Get the raw rgb and depth frames from sources
+ Mat ttrgb, trgb, tdepth, ttdepth;
+ Mat tsrgb, srgb, sdepth, tsdepth;
+
+ if (current == nullptr) {
+ srgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
+ trgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
+
+ putText(srgb, string("No correspondance for ") + sources[curtarget]->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+
+ if (!trgb.empty()) imshow("Target", trgb);
+ if (!srgb.empty()) imshow("Source", srgb);
+ } else if (current->source()->isReady() && current->target()->isReady()) {
+ current->source()->getFrames(tsrgb, tsdepth);
+ current->target()->getFrames(ttrgb, ttdepth);
+
+ tsrgb.copyTo(srgb);
+ ttrgb.copyTo(trgb);
+ ttdepth.convertTo(tdepth, CV_8U, 255.0f / 10.0f);
+ applyColorMap(tdepth, tdepth, cv::COLORMAP_JET);
+ tsdepth.convertTo(sdepth, CV_8U, 255.0f / 10.0f);
+ applyColorMap(sdepth, sdepth, cv::COLORMAP_JET);
+
+ vector<int> mids_targ, mids_src;
+ vector<vector<Point2f>> mpoints_targ, mpoints_src;
+ Ptr<aruco::DetectorParameters> parameters(new aruco::DetectorParameters);
+ auto dictionary = aruco::getPredefinedDictionary(aruco::DICT_4X4_50);
+
+ aruco::detectMarkers(trgb, dictionary, mpoints_targ, mids_targ, parameters);
+ aruco::drawDetectedMarkers(trgb, mpoints_targ, mids_targ);
+
+ // Cache the marker positions
+ for (size_t i=0; i<mids_targ.size(); i++) {
+ targ_corners[mids_targ[i]] = mpoints_targ[i];
+ }
+
+ vector<Vec3d> rvecs, tvecs;
+ cv::Mat distCoef(cv::Size(14,1), CV_64F, cv::Scalar(0.0));
+ cv::Mat targ_cam = current->target()->cameraMatrix();
+ aruco::estimatePoseSingleMarkers(mpoints_targ, 0.1, targ_cam, distCoef, rvecs, tvecs);
+ for (size_t i=0; i<rvecs.size(); i++) {
+ targ_trans[mids_targ[i]] = make_pair(tvecs[i], rvecs[i]);
+ aruco::drawAxis(trgb, current->target()->cameraMatrix(), distCoef, rvecs[i], tvecs[i], 0.1);
+ }
+
+ aruco::detectMarkers(srgb, dictionary, mpoints_src, mids_src, parameters);
+ aruco::drawDetectedMarkers(srgb, mpoints_src, mids_src);
+
+ rvecs.clear();
+ tvecs.clear();
+ cv::Mat src_cam = current->source()->cameraMatrix();
+ aruco::estimatePoseSingleMarkers(mpoints_src, 0.1, src_cam, distCoef, rvecs, tvecs);
+ for (size_t i=0; i<rvecs.size(); i++) {
+ src_trans[mids_src[i]] = make_pair(tvecs[i], rvecs[i]);
+ aruco::drawAxis(srgb, current->source()->cameraMatrix(), distCoef, rvecs[i], tvecs[i], 0.1);
+ }
+
+ // Cache the marker positions
+ for (size_t i=0; i<mids_src.size(); i++) {
+ src_corners[mids_src[i]] = mpoints_src[i];
+ }
+
+ current->drawTarget(trgb);
+ current->drawTarget(tdepth);
+ current->drawSource(srgb);
+ current->drawSource(sdepth);
+
+ putText(srgb, string("Source: ") + current->source()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+ putText(trgb, string("Target: ") + current->target()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+ putText(srgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+ putText(trgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+ if (freeze) putText(srgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+ if (freeze) putText(trgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+
+ if (!trgb.empty()) imshow("Target", (depthtoggle) ? tdepth : trgb);
+ if (!srgb.empty()) imshow("Source", (depthtoggle) ? sdepth : srgb);
+ }
+
+ int key = cv::waitKey(20);
+ if (key == 27) break;
+ else if (key >= 48 && key <= 57) {
+ curtarget = key-48;
+ if (curtarget >= sources.size()) curtarget = 0;
+ current = corrs[sources[curtarget]->getURI()];
+ //lastScore = 1000.0;
+ } else if (key == 'd') {
+ depthtoggle = !depthtoggle;
+ } else if (key == 'i') {
+ current->icp();
+ } else if (key == 'e') {
+ vector<Vec3d> targfeat;
+ vector<Vec3d> srcfeat;
+
+ for (auto i : targ_trans) {
+ auto si = src_trans.find(i.first);
+ if (si != src_trans.end()) {
+ //Vec3d dt = std::get<0>(si->second) - std::get<0>(i.second);
+ targfeat.push_back(std::get<0>(i.second));
+ srcfeat.push_back(std::get<0>(si->second));
+ }
+ }
+
+ Eigen::Matrix4d t;
+ lastScore = current->estimateTransform(t, srcfeat, targfeat);
+ current->setTransform(t);
+ } else if (key == 'f') {
+ Mat f1,f2;
+ current->capture(f1,f2);
+
+ LOG(INFO) << "Captured frames";
+
+ // Convert matching marker corners into correspondence points
+ for (auto i : targ_corners) {
+ auto si = src_corners.find(i.first);
+ if (si != src_corners.end()) {
+ for (size_t j=0; j<4; ++j) {
+ current->add(i.second[j].x, i.second[j].y, si->second[j].x, si->second[j].y);
+ }
+ }
+ }
+
+ LOG(INFO) << "Estimating transform...";
+
+ Eigen::Matrix4d t;
+ lastScore = current->estimateTransform(t);
+ current->setTransform(t);
+ lastScore = current->icp();
+ } else if (key == 's') {
+ // Save
+ map<string, Eigen::Matrix4d> transforms;
+ //transforms[sources[targsrc]->getURI()] = Eigen::Matrix4f().setIdentity();
+ //transforms[sources[cursrc]->getURI()] = c.transform();
+
+ for (auto x : corrs) {
+ if (x.second == nullptr) {
+ transforms[x.first] = Eigen::Matrix4d().setIdentity();
+ } else {
+ transforms[x.first] = x.second->transform();
+ }
+ }
+
+ saveTransformations(root->value("output", string("./test.json")), transforms);
+ LOG(INFO) << "Saved!";
+ } else if (key == 32) freeze = !freeze;
+ }
+}
diff --git a/applications/registration/src/aruco.hpp b/applications/registration/src/aruco.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..88438eac1bbae457c763bddb21c5259622bd1faa
--- /dev/null
+++ b/applications/registration/src/aruco.hpp
@@ -0,0 +1,14 @@
+#ifndef _FTL_REGISTRATION_ARUCO_HPP_
+#define _FTL_REGISTRATION_ARUCO_HPP_
+
+#include <ftl/configuration.hpp>
+
+namespace ftl {
+namespace registration {
+
+void aruco(ftl::Configurable *root);
+
+}
+}
+
+#endif // _FTL_REGISTRATION_ARUCO_HPP_
diff --git a/applications/registration/src/common.cpp b/applications/registration/src/common.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d225c636734f0f8899e719b53ff1ea79f91c96e3
--- /dev/null
+++ b/applications/registration/src/common.cpp
@@ -0,0 +1,49 @@
+#include "common.hpp"
+
+using std::string;
+using std::map;
+
+void ftl::registration::from_json(nlohmann::json &json, map<string, Eigen::Matrix4d> &transformations) {
+ for (auto it = json.begin(); it != json.end(); ++it) {
+ Eigen::Matrix4d m;
+ auto data = m.data();
+ for(size_t i = 0; i < 16; i++) { data[i] = it.value()[i]; }
+ transformations[it.key()] = m;
+ }
+}
+
+void ftl::registration::to_json(nlohmann::json &json, map<string, Eigen::Matrix4d> &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 ftl::registration::saveTransformations(const string &path, map<string, Eigen::Matrix4d> &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;
+}
+
+bool ftl::registration::loadTransformations(const string &path, map<string, Eigen::Matrix4d> &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;
+}
+
diff --git a/applications/registration/src/common.hpp b/applications/registration/src/common.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..bbb4e3749fd5c4ec3651291b01663667e406677e
--- /dev/null
+++ b/applications/registration/src/common.hpp
@@ -0,0 +1,21 @@
+#ifndef _FTL_REGISTRATION_COMMON_HPP_
+#define _FTL_REGISTRATION_COMMON_HPP_
+
+#include <ftl/rgbd/source.hpp>
+#include <nlohmann/json.hpp>
+
+#include <string>
+#include <map>
+
+namespace ftl {
+namespace registration {
+
+bool loadTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4d> &data);
+bool saveTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4d> &data);
+void from_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
+void to_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
+
+}
+}
+
+#endif // _FTL_REGISTRATION_COMMON_HPP_
diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index 4dbda754ab201ad807b17c9b542bcacabbe1f1cd..f40eaca97242d51ada14428b2293e562a4df10c4 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -19,6 +19,44 @@ using std::tuple;
using std::make_tuple;
using cv::Mat;
+void ftl::registration::build_correspondances(const vector<Source*> &sources, map<string, Correspondances*> &cs, int origin, map<string, Eigen::Matrix4d> &old) {
+ Correspondances *last = nullptr;
+
+ cs[sources[origin]->getURI()] = nullptr;
+
+ for (int i=origin-1; i>=0; i--) {
+ if (last == nullptr) {
+ auto *c = new Correspondances(sources[i], sources[origin]);
+ last = c;
+ cs[sources[i]->getURI()] = c;
+ if (old.find(sources[i]->getURI()) != old.end()) {
+ c->setTransform(old[sources[i]->getURI()]);
+ }
+ } else {
+ auto *c = new Correspondances(last, sources[i]);
+ last = c;
+ cs[sources[i]->getURI()] = c;
+ if (old.find(sources[i]->getURI()) != old.end()) {
+ c->setTransform(old[sources[i]->getURI()]);
+ }
+ }
+ }
+
+ last = nullptr;
+
+ for (int i=origin+1; i<sources.size(); i++) {
+ if (last == nullptr) {
+ auto *c = new Correspondances(sources[i], sources[origin]);
+ last = c;
+ cs[sources[i]->getURI()] = c;
+ } else {
+ auto *c = new Correspondances(last, sources[i]);
+ last = c;
+ cs[sources[i]->getURI()] = c;
+ }
+ }
+}
+
Correspondances::Correspondances(Source *src, Source *targ)
: parent_(nullptr), targ_(targ), src_(src),
@@ -92,6 +130,40 @@ static PointXYZ makePCL(Source *s, int x, int y) {
return pcl_p1;
}
+void Correspondances::drawTarget(cv::Mat &img) {
+ using namespace cv;
+
+ for (size_t i=0; i<log_.size(); i++) {
+ //for (auto &p : points) {
+ auto [tx,ty,sx,sy] = log_[i];
+ drawMarker(img, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
+ }
+
+ vector<Eigen::Vector2i> tpoints;
+ getTransformedFeatures2D(tpoints);
+ for (size_t i=0; i<tpoints.size(); i++) {
+ Eigen::Vector2i p = tpoints[i];
+ drawMarker(img, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
+ }
+}
+
+void Correspondances::drawSource(cv::Mat &img) {
+ using namespace cv;
+
+ for (size_t i=0; i<log_.size(); i++) {
+ //for (auto &p : points) {
+ auto [tx,ty,sx,sy] = log_[i];
+ drawMarker(img, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
+ }
+
+ /*vector<Eigen::Vector2i> tpoints;
+ getTransformedFeatures2D(tpoints);
+ for (size_t i=0; i<tpoints.size(); i++) {
+ Eigen::Vector2i p = tpoints[i];
+ drawMarker(img, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
+ }*/
+}
+
void Correspondances::clear() {
targ_cloud_->clear();
src_cloud_->clear();
@@ -126,8 +198,8 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
- averageDepth(buffer[0], d1, 0.01f);
- averageDepth(buffer[1], d2, 0.01f);
+ averageDepth(buffer[0], d1, 0.02f);
+ averageDepth(buffer[1], d2, 0.02f);
Mat d1_v, d2_v;
d1.convertTo(d1_v, CV_8U, 255.0f / 10.0f);
d2.convertTo(d2_v, CV_8U, 255.0f / 10.0f);
@@ -158,7 +230,7 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
float d1_value = d1ptr[x];
float d2_value = d2ptr[x];
- if (d1_value < 39.0f) {
+ if (d1_value > 0.1f && d1_value < 39.0f) {
// Make PCL points with specific depth value
pcl::PointXYZ p1;
Eigen::Vector4d p1e = src_->point(x,y,d1_value);
@@ -170,7 +242,7 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
six++;
}
- if (d2_value < 39.0f) {
+ if (d2_value > 0.1f && d2_value < 39.0f) {
// Make PCL points with specific depth value
pcl::PointXYZ p2;
Eigen::Vector4d p2e = targ_->point(x,y,d2_value);
@@ -218,10 +290,55 @@ double Correspondances::estimateTransform(Eigen::Matrix4d &T) {
pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ, double> validate;
pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ, double> svd;
- validate.setMaxRange(0.1);
+ //validate.setMaxRange(0.1);
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+ vector<int> idx;
+
+ for (int i=0; i<src_feat_.size(); i++) {
+ idx.push_back(i);
+ src_cloud->push_back(src_cloud_->at(src_feat_[i]));
+ targ_cloud->push_back(targ_cloud_->at(targ_feat_[i]));
+ }
+
+ pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
+
+ svd.estimateRigidTransformation(*src_cloud, idx, *targ_cloud, idx, T);
+ return validate.validateTransformation(src_cloud, targ_cloud, T);
+}
+
+double Correspondances::estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat) {
+ pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ, double> validate;
+ pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ, double> svd;
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+ vector<int> idx;
+
+ for (int i=0; i<src_feat.size(); i++) {
+ pcl::PointXYZ ps,pt;
+
+ ps.x = src_feat[i][0];
+ ps.y = src_feat[i][1];
+ ps.z = src_feat[i][2];
+ pt.x = targ_feat[i][0];
+ pt.y = targ_feat[i][1];
+ pt.z = targ_feat[i][2];
+
+ idx.push_back(i);
+ src_cloud->push_back(ps);
+ targ_cloud->push_back(pt);
+ }
+
+ pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
- svd.estimateRigidTransformation(*src_cloud_, src_feat_, *targ_cloud_, targ_feat_, T);
- return validate.validateTransformation(src_cloud_, targ_cloud_, T);
+ svd.estimateRigidTransformation(*src_cloud, idx, *targ_cloud, idx, T);
+ return validate.validateTransformation(src_cloud, targ_cloud, T);
}
double Correspondances::estimateTransform(Eigen::Matrix4d &T, const vector<int> &src_feat, const vector<int> &targ_feat) {
diff --git a/applications/registration/src/correspondances.hpp b/applications/registration/src/correspondances.hpp
index 57f4442f6c893863fb2df146ec2ae13894691f9f..53f50a155ba4921c4b14f65571ee775473d738fb 100644
--- a/applications/registration/src/correspondances.hpp
+++ b/applications/registration/src/correspondances.hpp
@@ -65,6 +65,7 @@ class Correspondances {
*/
double estimateTransform(Eigen::Matrix4d &);
double estimateTransform(Eigen::Matrix4d &T, const std::vector<int> &src_feat, const std::vector<int> &targ_feat);
+ double estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat);
double findBestSubset(Eigen::Matrix4d &tr, int K, int N);
@@ -81,6 +82,9 @@ class Correspondances {
void setTransform(Eigen::Matrix4d &t) { uptodate_ = true; transform_ = t; }
+ void drawTarget(cv::Mat &);
+ void drawSource(cv::Mat &);
+
private:
Correspondances *parent_;
ftl::rgbd::Source *targ_;
@@ -96,6 +100,10 @@ class Correspondances {
std::vector<int> src_feat_;
};
+void build_correspondances(const std::vector<ftl::rgbd::Source*> &sources,
+ std::map<std::string, Correspondances*> &cs, int origin,
+ std::map<std::string, Eigen::Matrix4d> &old);
+
}
}
diff --git a/applications/registration/src/main.cpp b/applications/registration/src/main.cpp
index 904320ae8c62eef6b91ae3d82c7e652bf3e1d5da..bb566ed074c6af8b1a011102aebbc6ec06831e9b 100644
--- a/applications/registration/src/main.cpp
+++ b/applications/registration/src/main.cpp
@@ -3,16 +3,18 @@
#include <string>
#include "manual.hpp"
+#include "aruco.hpp"
using std::string;
int main(int argc, char **argv) {
auto root = ftl::configure(argc, argv, "registration_default");
- // TODO
- ftl::registration::manual(root);
- if (root->value("mode", string("manual") == "manual")) {
- //ftl::registration::manual(root);
+ string mode = root->value("mode", string("manual"));
+ if (mode == "manual") {
+ ftl::registration::manual(root);
+ } else if (mode == "aruco") {
+ ftl::registration::aruco(root);
}
return 0;
diff --git a/applications/registration/src/manual.cpp b/applications/registration/src/manual.cpp
index 3138a0ae8219de9fe02128339808d969fac8049f..90f128855ea1cd9b3c87f750f0d72e11cf912890 100644
--- a/applications/registration/src/manual.cpp
+++ b/applications/registration/src/manual.cpp
@@ -1,6 +1,7 @@
#include "manual.hpp"
#include "correspondances.hpp"
#include "sfm.hpp"
+#include "common.hpp"
#include <loguru.hpp>
@@ -25,6 +26,7 @@ using std::tuple;
using ftl::net::Universe;
using ftl::rgbd::Source;
+using ftl::registration::Correspondances;
using cv::Mat;
using cv::Point;
@@ -45,88 +47,6 @@ static void setMouseAction(const std::string& winName, const MouseAction &action
static MouseAction tmouse;
static MouseAction smouse;
-void from_json(nlohmann::json &json, map<string, Eigen::Matrix4d> &transformations) {
- for (auto it = json.begin(); it != json.end(); ++it) {
- Eigen::Matrix4d m;
- auto data = m.data();
- for(size_t i = 0; i < 16; i++) { data[i] = it.value()[i]; }
- transformations[it.key()] = m;
- }
-}
-
-static void to_json(nlohmann::json &json, map<string, Eigen::Matrix4d> &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;
- }
-}
-
-static bool saveTransformations(const string &path, map<string, Eigen::Matrix4d> &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;
-}
-
-bool loadTransformations(const string &path, map<string, Eigen::Matrix4d> &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;
-}
-
-static void build_correspondances(const vector<Source*> &sources, map<string, Correspondances*> &cs, int origin, map<string, Eigen::Matrix4d> &old) {
- Correspondances *last = nullptr;
-
- cs[sources[origin]->getURI()] = nullptr;
-
- for (int i=origin-1; i>=0; i--) {
- if (last == nullptr) {
- auto *c = new Correspondances(sources[i], sources[origin]);
- last = c;
- cs[sources[i]->getURI()] = c;
- if (old.find(sources[i]->getURI()) != old.end()) {
- c->setTransform(old[sources[i]->getURI()]);
- }
- } else {
- auto *c = new Correspondances(last, sources[i]);
- last = c;
- cs[sources[i]->getURI()] = c;
- if (old.find(sources[i]->getURI()) != old.end()) {
- c->setTransform(old[sources[i]->getURI()]);
- }
- }
- }
-
- last = nullptr;
-
- for (int i=origin+1; i<sources.size(); i++) {
- if (last == nullptr) {
- auto *c = new Correspondances(sources[i], sources[origin]);
- last = c;
- cs[sources[i]->getURI()] = c;
- } else {
- auto *c = new Correspondances(last, sources[i]);
- last = c;
- cs[sources[i]->getURI()] = c;
- }
- }
-}
-
void ftl::registration::manual(ftl::Configurable *root) {
using namespace cv;
@@ -152,11 +72,11 @@ void ftl::registration::manual(ftl::Configurable *root) {
cv::namedWindow("Source", cv::WINDOW_KEEPRATIO);
map<string, Eigen::Matrix4d> oldTransforms;
- loadTransformations(root->value("output", string("./test.json")), oldTransforms);
+ ftl::registration::loadTransformations(root->value("output", string("./test.json")), oldTransforms);
//Correspondances c(sources[targsrc], sources[cursrc]);
map<string, Correspondances*> corrs;
- build_correspondances(sources, corrs, root->value("origin", 0), oldTransforms);
+ ftl::registration::build_correspondances(sources, corrs, root->value("origin", 0), oldTransforms);
int lastTX = 0;
int lastTY = 0;
@@ -219,37 +139,10 @@ void ftl::registration::manual(ftl::Configurable *root) {
tsdepth.convertTo(sdepth, CV_8U, 255.0f / 10.0f);
applyColorMap(sdepth, sdepth, cv::COLORMAP_JET);
- /*Ptr<xphoto::WhiteBalancer> wb;
- wb = xphoto::createSimpleWB();
- wb->balanceWhite(tsrgb, srgb);
- wb->balanceWhite(ttrgb, trgb);*/
-
- // Apply points and labels...
- auto &points = current->screenPoints();
- if (point_n == -1) {
- for (int i=0; i<points.size(); i++) {
- //for (auto &p : points) {
- auto [tx,ty,sx,sy] = points[i];
- drawMarker(srgb, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
- drawMarker(sdepth, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
- drawMarker(trgb, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
- drawMarker(tdepth, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
- }
- } else if(point_n < points.size()) {
- auto [tx,ty,sx,sy] = points[point_n];
- drawMarker(srgb, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
- drawMarker(sdepth, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
- drawMarker(trgb, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
- drawMarker(tdepth, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
- }
-
- vector<Eigen::Vector2i> tpoints;
- current->getTransformedFeatures2D(tpoints);
- for (int i=0; i<tpoints.size(); i++) {
- Eigen::Vector2i p = tpoints[i];
- drawMarker(trgb, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
- drawMarker(tdepth, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
- }
+ current->drawTarget(trgb);
+ current->drawTarget(tdepth);
+ current->drawSource(srgb);
+ current->drawSource(sdepth);
// Add most recent click position
drawMarker(srgb, Point(lastSX, lastSY), Scalar(0,0,255), MARKER_TILTED_CROSS);
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index d5b57a72c34f145c83dfa6acb09d00aef6b4e826..d90385f50aa0a61c9c79c80a307cbf224ad75ca1 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp
@@ -113,6 +113,8 @@ class Source : public ftl::Configurable {
else return params_;
}
+ cv::Mat cameraMatrix() const;
+
/**
* Change the camera extrinsics by providing a new pose matrix. For virtual
* cameras this will move the camera, for physical cameras it is set by the
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 06caebb232f64fd2b244c914cb8e01a518a2f6bc..7beea75193d4ee0110cfb6a03a5373505d357a3a 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -53,6 +53,11 @@ Source::~Source() {
}
+cv::Mat Source::cameraMatrix() const {
+ cv::Mat m = (cv::Mat_<float>(3,3) << parameters().fx, 0.0, -parameters().cx, 0.0, parameters().fy, -parameters().cy, 0.0, 0.0, 1.0);
+ return m;
+}
+
void Source::customImplementation(ftl::rgbd::detail::Source *impl) {
if (impl_) delete impl_;
impl_ = impl;