Merge branch 'master' into 'feature/calibapp'

# Conflicts:
#   components/rgbd-sources/src/algorithms/fixstars_sgm.cpp

applications/gui/src/main.cpp

@@ -161,7 +161,24 @@ class FTLApplication : public nanogui::Screen {
 		} else {
 			auto src_ = swindow_->getSource();
 			if (src_ && src_->isReady() && down) {
-				Eigen::Vector4f camPos = src_->point(p[0],p[1]);
+				Eigen::Vector2f screenSize = size().cast<float>();
+				auto mScale = (screenSize.cwiseQuotient(imageSize).minCoeff());
+				Eigen::Vector2f scaleFactor = mScale * imageSize.cwiseQuotient(screenSize);
+				Eigen::Vector2f positionInScreen(0.0f, 0.0f);
+				auto mOffset = (screenSize - (screenSize.cwiseProduct(scaleFactor))) / 2;
+				Eigen::Vector2f positionAfterOffset = positionInScreen + mOffset;
+
+				float sx = ((float)p[0] - positionAfterOffset[0]) / mScale;
+				float sy = ((float)p[1] - positionAfterOffset[1]) / mScale;
+
+				Eigen::Vector4f camPos;
+
+				try {
+					camPos = src_->point(sx,sy);
+				} catch(...) {
+					return true;
+				}
+				
 				camPos *= -1.0f;
 				Eigen::Vector4f worldPos =  src_->getPose() * camPos;
 				lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
@@ -195,7 +212,7 @@ class FTLApplication : public nanogui::Screen {
 		using namespace Eigen;
 
 		auto src_ = swindow_->getSource();
-		Vector2f imageSize(0, 0);
+		imageSize = {0, 0};
 
 		float now = (float)glfwGetTime();
 		float delta = now - ftime_;
@@ -273,6 +290,7 @@ class FTLApplication : public nanogui::Screen {
 	float lerpSpeed_;
 	bool depth_;
 	float ftime_;
+	Eigen::Vector2f imageSize;
 };
 
 int main(int argc, char **argv) {

applications/reconstruct/include/ftl/ray_cast_sdf.hpp

@@ -15,9 +15,11 @@ public:
 	CUDARayCastSDF(nlohmann::json& config) : ftl::Configurable(config) {
 		auto &cfg = ftl::config::resolve(config);
 		create(parametersFromConfig(cfg));
-		hash_render_ = cfg.value("hash_renderer", false);
+		hash_render_ = value("hash_renderer", false);
 	}
 
+	bool isIntegerDepth() const { return hash_render_; }
+
 	~CUDARayCastSDF(void) {
 		destroy();
 	}

applications/reconstruct/src/virtual_source.cpp

@@ -26,6 +26,7 @@ VirtualSource::VirtualSource(ftl::rgbd::Source *host)
 
 	rgb_ = cv::Mat(cv::Size(params_.width,params_.height), CV_8UC3);
 	idepth_ = cv::Mat(cv::Size(params_.width,params_.height), CV_32SC1);
+	depth_ = cv::Mat(cv::Size(params_.width,params_.height), CV_32FC1);
 }
 
 VirtualSource::~VirtualSource() {
@@ -52,8 +53,12 @@ bool VirtualSource::grab() {
 		rays_->render(scene_->getHashData(), scene_->getHashParams(), params, host_->getPose());
 
 		//unique_lock<mutex> lk(mutex_);
+		if (rays_->isIntegerDepth()) {
 			rays_->getRayCastData().download((int*)idepth_.data, (uchar3*)rgb_.data, rays_->getRayCastParams());
 			idepth_.convertTo(depth_, CV_32FC1, 1.0f / 100.0f);
+		} else {
+			rays_->getRayCastData().download((int*)depth_.data, (uchar3*)rgb_.data, rays_->getRayCastParams());
+		}
 	}
 	return true;
 }

applications/vision/src/main.cpp

@@ -95,17 +95,21 @@ static void run(ftl::Configurable *root) {
 	
 	Streamer *stream = ftl::create<Streamer>(root, "stream", net);
 	stream->add(source);
-	stream->run();
 	
 	net->start();
-	LOG(INFO) << "Running...";
 
+	LOG(INFO) << "Running...";
+	if (display->hasDisplays()) {
+		stream->run();
 		while (ftl::running && display->active()) {
 			cv::Mat rgb, depth;
 			source->getFrames(rgb, depth);
 			if (!rgb.empty()) display->render(rgb, depth, source->parameters());
 			display->wait(10);
 		}
+	} else {
+		stream->run(true);
+	}
 
 	LOG(INFO) << "Stopping...";
 	slave.stop();

components/renderers/cpp/include/ftl/display.hpp

@@ -44,6 +44,8 @@ class Display : public ftl::Configurable {
 
 	bool active() const;
 
+	bool hasDisplays();
+	
 	void wait(int ms);
 
 	void onKey(const std::function<void(int)> &h) { key_handlers_.push_back(h); }

components/renderers/cpp/src/display.cpp

@@ -21,7 +21,7 @@ Display::Display(nlohmann::json &config, std::string name) : ftl::Configurable(c
 	//cv::namedWindow("Image", cv::WINDOW_KEEPRATIO);
 
 #if defined HAVE_PCL
-	if (config.value("points", false)) {
+	if (value("points", false)) {
 		pclviz_ = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer ("FTL Cloud: " + name));
 		pclviz_->setBackgroundColor (255, 255, 255);
 		pclviz_->addCoordinateSystem (1.0);
@@ -243,6 +243,10 @@ bool Display::render(const cv::Mat &img, style_t s) {
 	return true;
 }
 
+bool Display::hasDisplays() {
+	return value("depth", false) || value("left", false) || value("right", false) || value("points", false);
+}
+
 void Display::wait(int ms) {
 	if (value("points", false)) {
 		#if defined HAVE_PCL

components/renderers/cpp/src/rgbd_display.cpp

@@ -42,7 +42,7 @@ static void setMouseAction(const std::string& winName, const MouseAction &action
 }
 
 Display::Display(nlohmann::json &config) : ftl::Configurable(config) {
-	name_ = config.value("name", string("View [")+std::to_string(viewcount__)+string("]"));
+	name_ = value("name", string("View [")+std::to_string(viewcount__)+string("]"));
 	viewcount__++;
 
 	init();
@@ -50,7 +50,7 @@ Display::Display(nlohmann::json &config) : ftl::Configurable(config) {
 
 Display::Display(nlohmann::json &config, Source *source)
 		: ftl::Configurable(config) {
-	name_ = config.value("name", string("View [")+std::to_string(viewcount__)+string("]"));
+	name_ = value("name", string("View [")+std::to_string(viewcount__)+string("]"));
 	viewcount__++;
 	init();
 }

components/rgbd-sources/src/algorithms/fixstars_sgm.cpp

@@ -12,10 +12,6 @@ using cv::Mat;
 FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
 	ssgm_ = nullptr;
 	use_filter_ = value("use_filter", false);
-
-	LOG(INFO) << "Filter status: " << use_filter_;
-	LOG(INFO) << "Disp config: " << config;
-
 	// note: (max_disp_ << 4) libsgm subpixel accuracy.
 	//       What is the impact in the filter? (possible artifacts)
 	filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, value("filter_radius", 25), value("filter_iter", 1));

components/rgbd-sources/src/source.cpp

@@ -128,8 +128,8 @@ void Source::getFrames(cv::Mat &rgb, cv::Mat &depth) {
 
 Eigen::Vector4f Source::point(uint ux, uint uy) {
 	const auto &params = parameters();
-	const float x = ((float)ux-params.width/2) / (float)params.fx;
-	const float y = ((float)uy-params.height/2) / (float)params.fy;
+	const float x = ((float)ux-(float)params.cx) / (float)params.fx;
+	const float y = ((float)uy-(float)params.cy) / (float)params.fy;
 
 	shared_lock<shared_mutex> lk(mutex_);
 	const float depth = depth_.at<float>(uy,ux);

web-service/src/index.js

@@ -11,6 +11,76 @@ let peer_uris = {};
 
 let uri_data = {};
 
+function RGBDClient(peer, N, rate, dest) {
+	this.peer = peer;
+	this.txmax = N;
+	this.rate = rate;
+	this.dest = dest;
+	this.txcount = 0;
+}
+
+RGBDClient.prototype.push = function(uri, rgb, depth) {
+	this.peer.send(uri, rgb, depth);
+	this.txcount++;
+}
+
+function RGBDStream(uri, peer) {
+	this.uri = uri;
+	this.peer = peer;
+	this.title = "";
+	this.rgb = null;
+	this.depth = null;
+	this.pose = null;
+	this.clients = [];
+	this.rxcount = 10;
+	this.rxmax = 10;
+
+	peer.bind(uri, (rgb, depth) => {
+		this.pushFrames(rgb, depth);
+		this.rxcount++;
+		if (this.rxcount >= this.rxmax && this.clients.length > 0) {
+			this.subscribe();
+		}
+	});
+}
+
+RGBDStream.prototype.addClient = function(peer, N, rate, dest) {
+	// TODO(Nick) Verify that it isn't already in list...
+	for (let i=0; i<this.clients.length; i++) {
+		if (this.clients[i].peer.string_id == peer.string_id) return;
+	}
+
+	this.clients.push(new RGBDClient(peer, N, rate, dest));
+
+	if (this.rxcount >= this.rxmax) {
+		this.subscribe();
+	}
+}
+
+RGBDStream.prototype.subscribe = function() {
+	this.rxcount = 0;
+	this.rxmax = 10;
+	console.log("Subscribe to ", this.uri);
+	this.peer.send("get_stream", this.uri, 10, 0, [Peer.uuid], this.uri);
+}
+
+RGBDStream.prototype.pushFrames = function(rgb, depth) {
+	this.rgb = rgb;
+	this.depth = depth;
+
+	for (let i=0; i < this.clients.length; i++) {
+		this.clients[i].push(this.uri, rgb, depth);
+	}
+
+	let i=0;
+	while (i < this.clients.length) {
+		if (this.clients[i].txcount >= this.clients[i].txmax) {
+			console.log("remove client");
+			this.clients.splice(i, 1);
+		} else i++;
+	}
+}
+
 // ---- PROTOCOL ---------------------------------------------------------------
 
 app.get('/', (req, res) => {
@@ -49,13 +119,7 @@ function checkStreams(peer) {
 				//uri_to_peer[streams[i]] = peer;
 				peer_uris[peer.string_id].push(streams[i]);
 
-				uri_data[streams[i]] = {
-					peer: peer,
-					title: "",
-					rgb: null,
-					depth: null,
-					pose: null
-				};
+				uri_data[streams[i]] = new RGBDStream(streams[i], peer);
 			}
 		});
 	}
@@ -144,13 +208,8 @@ app.ws('/', (ws, req) => {
 	p.bind("get_stream", (uri, N, rate, pid, dest) => {
 		let peer = uri_data[uri].peer;
 		if (peer) {
-			// FIXME (NICK) BUG HERE, can't have multiple peers listening to same stream...
-			peer.bind(uri, (rgb, depth) => {
-				uri_data[uri].rgb = rgb;
-				uri_data[uri].depth = depth;
-				p.send(uri, rgb, depth);
-			});
-			peer.send("get_stream", uri, N, rate, [Peer.uuid], dest);
+			uri_data[uri].addClient(p, N, rate, dest);
+			//peer.send("get_stream", uri, N, rate, [Peer.uuid], dest);
 		}
 	});
 
@@ -159,13 +218,7 @@ app.ws('/', (ws, req) => {
 		//uri_to_peer[streams[i]] = peer;
 		peer_uris[p.string_id].push(uri);
 
-		uri_data[uri] = {
-			peer: p,
-			title: "",
-			rgb: null,
-			depth: null,
-			pose: null
-		};
+		uri_data[uri] = new RGBDStream(uri, p);
 
 		broadcastExcept(p, "add_stream", uri);
 	});