From 9b97e8337cc59379cc7b4bf23b680bb4c41cf67d Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Mon, 13 May 2019 13:46:21 +0300
Subject: [PATCH] Multiple sources
---
common/config/config.json | 2 +-
reconstruct/src/main.cpp | 192 ++++++++++++++++++---------
renderer/cpp/include/ftl/display.hpp | 4 +-
renderer/cpp/src/display.cpp | 11 +-
vision/src/main.cpp | 2 +-
5 files changed, 143 insertions(+), 68 deletions(-)
diff --git a/common/config/config.json b/common/config/config.json
index 4c0ec3225..fabeed003 100644
--- a/common/config/config.json
+++ b/common/config/config.json
@@ -69,7 +69,7 @@
"net": {
"peers": ["tcp://localhost:9001"]
},
- "source": "ftl://utu.fi/dummy/rgb-d",
+ "sources": ["ftl://utu.fi/node1/rgb-d"],
"display": {
"flip_vert": false,
"disparity": false,
diff --git a/reconstruct/src/main.cpp b/reconstruct/src/main.cpp
index 6145a65a3..61049ec33 100644
--- a/reconstruct/src/main.cpp
+++ b/reconstruct/src/main.cpp
@@ -7,6 +7,7 @@
#include <glog/logging.h>
#include <ftl/config.h>
#include <ftl/configuration.hpp>
+
// #include <zlib.h>
// #include <lz4.h>
@@ -30,93 +31,164 @@
#pragma comment(lib, "Rpcrt4.lib")
#endif
+//#include <pcl/point_cloud.h>
+//#include <pcl/common/transforms.h>
+//#include <pcl/filters/uniform_sampling.h>
+
using ftl::net::Universe;
using ftl::Display;
using ftl::config;
using std::string;
using std::vector;
-using cv::Mat;
+
using json = nlohmann::json;
using std::this_thread::sleep_for;
using std::chrono::milliseconds;
using std::mutex;
using std::unique_lock;
-static void run() {
- Universe net(config["net"]);
- Mat rgb, depth, Q;
+using cv::Mat;
+
+class SourceStereo {
+private:
+ Mat rgb, disp;
+ Mat q;
mutex m;
-
- Display disp(config["display"]);
-
- // Make sure connections are complete
- sleep_for(milliseconds(500));
- // TODO(nick) Allow for many sources
- net.subscribe(config["source"], [&rgb,&m,&depth](const vector<unsigned char> &jpg, const vector<unsigned char> &d) {
+ /*
+ static pcl::PointCloud<pcl::PointXYZRGB>::Ptr _getPC(Mat rgb, Mat depth, Mat q) {
+ auto pc = matToPointXYZ(depth, rgb);
+ std::vector<int> indices;
+ pcl::removeNaNFromPointCloud(*pc, *pc, indices);
+ return pc;
+ }*/
+
+public:
+ string uri;
+
+ void recv(const vector<unsigned char> &jpg, const vector<unsigned char> &d) {
unique_lock<mutex> lk(m);
cv::imdecode(jpg, cv::IMREAD_COLOR, &rgb);
- //LOG(INFO) << "Received JPG : " << rgb.cols;
-
- depth = Mat(rgb.size(), CV_16UC1);
-
- /*z_stream infstream;
- infstream.zalloc = Z_NULL;
- infstream.zfree = Z_NULL;
- infstream.opaque = Z_NULL;
- // setup "b" as the input and "c" as the compressed output
- infstream.avail_in = (uInt)d.size(); // size of input
- infstream.next_in = (Bytef *)d.data(); // input char array
- infstream.avail_out = (uInt)depth.step*depth.rows; // size of output
- infstream.next_out = (Bytef *)depth.data; // output char array
-
- // the actual DE-compression work.
- inflateInit(&infstream);
- inflate(&infstream, Z_NO_FLUSH);
- inflateEnd(&infstream);*/
-
- //LZ4_decompress_safe((char*)d.data(), (char*)depth.data, d.size(), depth.step*depth.rows);
+ Mat(rgb.size(), CV_16UC1);
+ cv::imdecode(d, cv::IMREAD_UNCHANGED, &disp);
+ disp.convertTo(disp, CV_32FC1, 1.0f/16.0f);
+ }
+
+ /* thread unsafe, use lock() */
+
+ void setQ(Mat &Q) { q = Q; }
+ Mat getQ() { return q; }
+
+ /*
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr getPC() {
+ return _getPC(rgb, getDepth(), q);
+ }
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr getPC(cv::Size size) {
+ Mat rgb_, depth_;
- cv::imdecode(d, cv::IMREAD_UNCHANGED, &depth);
- depth.convertTo(depth, CV_32FC1, 1.0f/16.0f);
- });
+ cv::resize(rgb, rgb_, size);
+ cv::resize(getDepth(), depth_, size);
+ return _getPC(rgb_, depth_, q);
+ }
+ */
- while (disp.active()) {
- Mat idepth;
+ Mat getDepth() {
+ cv::Mat_<cv::Vec3f> depth(disp.rows, disp.cols);
+ reprojectImageTo3D(disp, depth, q, false);
+ return depth;
+ }
+
+ Mat getRGB() {
+ return rgb;
+ }
+
+ Mat getDisparity() {
+ return disp;
+ }
+
+ unique_lock<mutex> lock() {return unique_lock<mutex>(m);} // use recursive mutex instead (and move locking to methods)?
+};
- net.broadcast("grab");
+static void run() {
+ Universe net(config["net"]);
+
+ // Make sure connections are complete
+ sleep_for(milliseconds(500));
+
+ if (!config["sources"].is_array()) {
+ LOG(ERROR) << "No sources configured!";
+ return;
+ }
+
+ // todo: create display objects at the same time, store in pair/tuple?
+ //
+ std::deque<SourceStereo> sources; // mutex in SourceStereo
+ std::deque<Display> displays;
+
+ for (auto &src : config["sources"]) {
+ SourceStereo &in = sources.emplace_back();
+ Display &display = displays.emplace_back(Display(config["display"], src));
- unique_lock<mutex> lk(m);
- if (depth.cols > 0) {
- // If no calibration data then get it from the remote machine
- if (Q.rows == 0) {
- // Must unlock before calling findOne to prevent net block!!
- lk.unlock();
- auto buf = net.findOne<vector<unsigned char>>((string)config["source"]+"/calibration");
- lk.lock();
- if (buf) {
- Q = Mat(cv::Size(4,4), CV_32F);
- memcpy(Q.data, (*buf).data(), (*buf).size());
- if (Q.step*Q.rows != (*buf).size()) LOG(ERROR) << "Corrupted calibration";
- disp.setCalibration(Q);
- }
+ // get calibration parameters from nodes
+ while(true) {
+ auto buf = net.findOne<vector<unsigned char>>((string) src +"/calibration");
+ if (buf) {
+ Mat Q = Mat(cv::Size(4,4), CV_32F);
+ memcpy(Q.data, (*buf).data(), (*buf).size());
+ if (Q.step*Q.rows != (*buf).size()) LOG(ERROR) << "Corrupted calibration";
+ in.setQ(Q);
+ LOG(INFO) << "Calibration loaded for " << (string) src;
+ break;
+ }
+ else {
+ LOG(INFO) << "Could not get calibration, retrying";
+ sleep_for(milliseconds(500));
}
}
+ net.subscribe(src, [&in](const vector<unsigned char> &jpg, const vector<unsigned char> &d) {
+ in.recv(jpg, d);
+ });
- if (rgb.cols > 0) {
- //cv::imshow("RGB", rgb);
- disp.render(rgb,depth);
+ // todo: Display should only need processed input, depth calculation etc.
+ // should happen somewhere else.
+ display.setCalibration(in.getQ());
+ }
+
+ int active = displays.size();
+ while (active > 0) {
+ active = 0;
+
+ //std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
+ //pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+ for (size_t i = 0; i < sources.size(); i++) {
+ Display &display = displays[i];
+ SourceStereo &source = sources[i];
+
+ if (!display.active()) continue;
+ active += 1;
+
+ auto lk = source.lock();
+ Mat rgb = source.getRGB();
+ Mat disparity = source.getDisparity();
+ lk.unlock();
+
+ display.render(rgb, disparity);
+ display.wait(50);
}
- // TODO(nick) Use a swap buffer so this can be unlocked earlier
- lk.unlock();
- //if (cv::waitKey(40) == 27) break;
- disp.wait(40);
+ /*
+ pcl::transformPointCloud(*clouds[1], *clouds[1], T);
+ pcl::UniformSampling<pcl::PointXYZRGB> uniform_sampling;
+ uniform_sampling.setInputCloud(pc);
+ uniform_sampling.setRadiusSearch(0.1f);
+ uniform_sampling.filter(*pc);
+ */
}
}
int main(int argc, char **argv) {
ftl::configure(argc, argv, "representation"); // TODO(nick) change to "reconstruction"
run();
-}
-
+}
\ No newline at end of file
diff --git a/renderer/cpp/include/ftl/display.hpp b/renderer/cpp/include/ftl/display.hpp
index 4301fa922..bb7d5444c 100644
--- a/renderer/cpp/include/ftl/display.hpp
+++ b/renderer/cpp/include/ftl/display.hpp
@@ -22,13 +22,15 @@ namespace ftl {
* Multiple local display options for disparity or point clouds.
*/
class Display {
+ private:
+ std::string name_;
public:
enum style_t {
STYLE_NORMAL, STYLE_DISPARITY, STYLE_DEPTH
};
public:
- explicit Display(nlohmann::json &config);
+ explicit Display(nlohmann::json &config, std::string name);
~Display();
void setCalibration(const cv::Mat &q) { q_ = q; }
diff --git a/renderer/cpp/src/display.cpp b/renderer/cpp/src/display.cpp
index c24fe35b1..6f70787ed 100644
--- a/renderer/cpp/src/display.cpp
+++ b/renderer/cpp/src/display.cpp
@@ -11,14 +11,15 @@ using ftl::Display;
using cv::Mat;
using cv::Vec3f;
-Display::Display(nlohmann::json &config) : config_(config) {
+Display::Display(nlohmann::json &config, std::string name) : config_(config) {
+ name_ = name;
#if defined HAVE_VIZ
- window_ = new cv::viz::Viz3d("FTL");
+ window_ = new cv::viz::Viz3d("FTL: " + name);
window_->setBackgroundColor(cv::viz::Color::white());
#endif // HAVE_VIZ
#if defined HAVE_PCL
- pclviz_ = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer ("FTL Cloud"));
+ pclviz_ = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer ("FTL Cloud: " + name));
pclviz_->setBackgroundColor (255, 255, 255);
pclviz_->addCoordinateSystem (1.0);
pclviz_->setShowFPS(true);
@@ -116,7 +117,7 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) {
}
if (config_["left"]) {
- cv::imshow("RGB", rgb);
+ cv::imshow("RGB: " + name_, rgb);
}
if (config_["depth"]) {
@@ -137,7 +138,7 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) {
idepth.convertTo(idepth, CV_8U, 255.0f / 256.0f); // TODO(nick)
applyColorMap(idepth, idepth, cv::COLORMAP_JET);
- cv::imshow("Disparity", idepth);
+ cv::imshow("Disparity:" + name_, idepth);
//if(cv::waitKey(40) == 27) {
// exit if ESC is pressed
// active_ = false;
diff --git a/vision/src/main.cpp b/vision/src/main.cpp
index c71940a6c..99d89aeba 100644
--- a/vision/src/main.cpp
+++ b/vision/src/main.cpp
@@ -131,7 +131,7 @@ static void run(const string &file) {
vector<unsigned char> d_buf;
string uri = string("ftl://utu.fi/")+(string)config["stream"]["name"]+string("/rgb-d");
- Display display(config["display"]);
+ Display display(config["display"], "local");
display.setCalibration(Q_32F);
Streamer stream(net, config["stream"]);
--
GitLab