Merge branch 'multiple-sources' into 'master'

Multiple sources

See merge request nicolas.pope/ftl!3

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,

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

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; }

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;

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"]);