Stream pose data over net

applications/gui/src/main.cpp

@@ -48,10 +48,90 @@ class GLTexture {
 	unsigned int glid_;
 };
 
-struct SourceViews {
+/*struct SourceViews {
 	ftl::rgbd::RGBDSource *source;
 	GLTexture texture;
 	nanogui::ImageView *view;
+};*/
+
+template<class T>
+Eigen::Matrix<T,4,4> lookAt
+(
+	Eigen::Matrix<T,3,1> const & eye,
+	Eigen::Matrix<T,3,1> const & center,
+	Eigen::Matrix<T,3,1> const & up
+)
+{
+	typedef Eigen::Matrix<T,4,4> Matrix4;
+	typedef Eigen::Matrix<T,3,1> Vector3;
+
+	Vector3 f = (center - eye).normalized();
+	Vector3 u = up.normalized();
+	Vector3 s = f.cross(u).normalized();
+	u = s.cross(f);
+
+	Matrix4 res;
+	res <<	s.x(),s.y(),s.z(),-s.dot(eye),
+			u.x(),u.y(),u.z(),-u.dot(eye),
+			-f.x(),-f.y(),-f.z(),f.dot(eye),
+			0,0,0,1;
+
+	return res;
+}
+
+class VirtualCameraView : public nanogui::ImageView {
+	public:
+	VirtualCameraView(nanogui::Widget *parent) : nanogui::ImageView(parent, 0) {
+		src_ = nullptr;
+		eye_ = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
+		centre_ = Eigen::Vector3f(0.0f, 0.0f, -4.0f);
+		up_ = Eigen::Vector3f(0,1.0f,0);
+		lookPoint_ = Eigen::Vector3f(0.0f,0.0f,-4.0f);
+		lerpSpeed_ = 0.4f;
+	}
+
+	void setSource(RGBDSource *src) { src_ = src; }
+
+	bool mouseButtonEvent(const nanogui::Vector2i &p, int button, bool down, int modifiers) {
+		//LOG(INFO) << "Mouse move: " << p[0];
+		if (src_ && down) {
+			Eigen::Vector4f camPos = src_->point(p[0],p[1]);
+			camPos *= -1.0f;
+			Eigen::Vector4f worldPos =  src_->getPose() * camPos;
+			lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
+			LOG(INFO) << "Depth at click = " << -camPos[2];
+		}
+	}
+
+	void draw(NVGcontext *ctx) {
+		//net_->broadcast("grab");
+		if (src_) {
+			cv::Mat rgb, depth;
+			centre_ += (lookPoint_ - centre_) * (lerpSpeed_ * 0.1f);
+			Eigen::Matrix4f viewPose = lookAt<float>(eye_,centre_,up_).inverse();
+
+			src_->setPose(viewPose);
+			src_->grab();
+			src_->getRGBD(rgb, depth);
+			if (rgb.rows > 0) {
+				LOG(INFO) << "Update texture";
+				texture_.update(rgb);
+				bindImage(texture_.texture());
+			}
+
+			screen()->performLayout(ctx);
+		}
+		ImageView::draw(ctx);
+	}
+
+	private:
+	RGBDSource *src_;
+	GLTexture texture_;
+	Eigen::Vector3f eye_;
+	Eigen::Vector3f centre_;
+	Eigen::Vector3f up_;
+	Eigen::Vector3f lookPoint_;
+	float lerpSpeed_;
 };
 
 class FTLApplication : public nanogui::Screen {
@@ -65,17 +145,18 @@ class FTLApplication : public nanogui::Screen {
 			if (!in) {
 				LOG(ERROR) << "Unrecognised source: " << src;
 			} else {
-				auto &cam = sources_.emplace_back();
-				cam.source = in;
+				//auto &cam = sources_.emplace_back();
+				//cam.source = in;
 
 				auto imageWindow = new Window(this, "Source");
 				imageWindow->setPosition(Eigen::Vector2i(710, 15));
 				imageWindow->setLayout(new GroupLayout());
 				imageWindow->setSize(Vector2i(400,400));
 
-				auto imageView = new ImageView(imageWindow, 0);
-				cam.view = imageView;
+				auto imageView = new VirtualCameraView(imageWindow);
+				//cam.view = imageView;
 				imageView->setGridThreshold(20);
+				imageView->setSource(in);
 				//imageView->setPixelInfoThreshold(20);
 				
 				//displays.emplace_back(config["display"], src["uri"]);
@@ -88,24 +169,12 @@ class FTLApplication : public nanogui::Screen {
 	}
 
 	virtual void draw(NVGcontext *ctx) {
-		//net_->broadcast("grab");
-		for (auto &src : sources_) {
-			cv::Mat rgb, depth;
-			src.source->grab();
-			src.source->getRGBD(rgb, depth);
-			if (rgb.rows > 0) {
-				src.texture.update(rgb);
-				src.view->bindImage(src.texture.texture());
-			}
-		}
-		performLayout();
-
 		/* Draw the user interface */
 		Screen::draw(ctx);
 	}
 
 	private:
-	std::vector<SourceViews> sources_;
+	//std::vector<SourceViews> sources_;
 	ftl::net::Universe *net_;
 };
 

components/renderers/cpp/src/rgbd_display.cpp

@@ -120,7 +120,7 @@ void Display::update() {
 
 	centre_ += (lookPoint_ - centre_) * (lerpSpeed_ * 0.1f);
 	Eigen::Matrix4f viewPose = lookAt<float>(eye_,centre_,up_).inverse();
-	source_->setPose(viewPose);
+	//source_->setPose(viewPose);
 
 	Mat rgb, depth;
 	//source_->grab();

components/rgbd-sources/include/ftl/net_source.hpp

@@ -28,6 +28,8 @@ class NetSource : public RGBDSource {
 		return new NetSource(config, net);
 	}
 
+	void setPose(const Eigen::Matrix4f &pose);
+
 	private:
 	bool has_calibration_;
 	ftl::UUID peer_;

components/rgbd-sources/src/net_source.cpp

@@ -79,6 +79,12 @@ void NetSource::_recv(const vector<unsigned char> &jpg, const vector<unsigned ch
 	}
 }
 
+void NetSource::setPose(const Eigen::Matrix4f &pose) {
+	vector<unsigned char> vec((unsigned char*)pose.data(), (unsigned char*)(pose.data()+(pose.size())));
+	net_->send(peer_, "set_pose", getURI(), vec);
+	RGBDSource::setPose(pose);
+}
+
 void NetSource::grab() {
 	// net_.broadcast("grab");
 }

components/rgbd-sources/src/rgbd_streamer.cpp

@@ -42,6 +42,16 @@ Streamer::Streamer(nlohmann::json &config, Universe *net)
 		return streams;
 	});
 
+	net->bind("set_pose", [this](const std::string &uri, const std::vector<unsigned char> &buf) {
+		shared_lock<shared_mutex> slk(mutex_);
+
+		if (sources_.find(uri) != sources_.end()) {
+			Eigen::Matrix4f pose;
+			memcpy(pose.data(), buf.data(), buf.size());
+			sources_[uri]->src->setPose(pose);
+		}
+	});
+
 	// Allow remote users to access camera calibration matrix
 	net->bind("source_calibration", [this](const std::string &uri) -> vector<unsigned char> {
 		vector<unsigned char> buf;