Improve Poser and fix Net Bugs

applications/gui2/src/main.cpp

@@ -153,6 +153,8 @@ void FTLGui::mainloop() {
 	// Process events once more
 	glfwPollEvents();
 
+	ftl::config::save();
+
 	// Stop everything before deleting feed etc
 	LOG(INFO) << "Stopping...";
 	ftl::timer::stop(true);

applications/gui2/src/modules/camera.cpp

@@ -8,6 +8,8 @@
 #include <ftl/utility/matrix_conversion.hpp>
 #include <ftl/calibration/structures.hpp>
 #include <ftl/calibration/parameters.hpp>
+#include <ftl/codecs/shapes.hpp>
+#include <ftl/operators/poser.hpp>
 
 #include <opencv2/imgproc.hpp>
 #include <opencv2/imgcodecs.hpp>
@@ -427,7 +429,11 @@ bool Camera::hasFrame() {
 	return false;
 }
 
-Eigen::Matrix4d Camera::cursor() const {
+const Eigen::Matrix4d &Camera::cursor() const {
+	return cursor_;
+}
+
+Eigen::Matrix4d Camera::_cursor() const {
 	if (cursor_normal_.norm() > 0.0f) return nanogui::lookAt(cursor_pos_, cursor_target_, cursor_normal_).cast<double>();
 
 	Eigen::Matrix4d ident;
@@ -573,17 +579,19 @@ void Camera::setCursor(int x, int y) {
 			cursor_target_[2] = CP.z;
 		}
 	}
+
+	cursor_ = _cursor();
 }
 
 void Camera::setOriginToCursor() {
 	using ftl::calibration::transform::inverse;
 	
 	// Check for valid cursor
-	if (cursor_normal_.norm() == 0.0f) return;
+	/*if (cursor_normal_.norm() == 0.0f) return;
 	float cursor_length = (cursor_target_ - cursor_pos_).norm();
 	float cursor_dist = cursor_pos_.norm();
 	if (cursor_length < 0.01f || cursor_length > 5.0f) return;
-	if (cursor_dist > 10.0f) return;
+	if (cursor_dist > 10.0f) return;*/
 
 	if (movable_) {
 		auto *rend = io->feed()->getRenderer(frame_id_);
@@ -612,12 +620,14 @@ void Camera::setOriginToCursor() {
 	cursor_target_ = Eigen::Vector3f(0.0f,0.0f,0.0f); 
 	cursor_pos_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
 	cursor_normal_ = Eigen::Vector3f(0.0f,0.0f,0.0f); 
+	cursor_ = _cursor();
 }
 
 void Camera::resetOrigin() {
 	cursor_target_ = Eigen::Vector3f(0.0f,0.0f,0.0f); 
 	cursor_pos_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
-	cursor_normal_ = Eigen::Vector3f(0.0f,0.0f,0.0f); 
+	cursor_normal_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
+	cursor_ = _cursor(); 
 
 	if (movable_) {
 		auto *rend = io->feed()->getRenderer(frame_id_);
@@ -642,3 +652,45 @@ void Camera::resetOrigin() {
 		}
 	}
 }
+
+void Camera::saveCursorToPoser() {
+	ftl::codecs::Shape3D shape;
+	shape.type = ftl::codecs::Shape3DType::CURSOR;
+	shape.id = cursor_save_id_++;
+	shape.label = std::string("Cursor") + std::to_string(shape.id);
+	shape.pose = cursor().inverse().cast<float>();
+	shape.size = Eigen::Vector3f(0.1f,0.1f,0.1f);
+
+	ftl::operators::Poser::add(shape, frame_id_);
+}
+
+Eigen::Matrix4d Camera::getActivePose() {
+	return cursor(); //.inverse();
+}
+
+nanogui::Vector2i Camera::getActivePoseScreenCoord() {
+	Eigen::Matrix4d pose = getActivePose().inverse();
+
+	auto ptr = std::atomic_load(&latest_);
+	if (ptr) {
+		const auto &frame = ptr->frames[frame_idx].cast<ftl::rgbd::Frame>();
+		auto campose = frame.getPose().inverse() * pose;
+		float3 campos;
+		campos.x = campose(0,3);
+		campos.y = campose(1,3);
+		campos.z = campose(2,3);
+
+		int2 spos = frame.getLeft().camToScreen<int2>(campos);
+		return nanogui::Vector2i(spos.x, spos.y);
+	}
+
+	return nanogui::Vector2i(-1,-1);
+}
+
+void Camera::transformActivePose(const Eigen::Matrix4d &pose) {
+	cursor_ = pose * cursor_;
+}
+
+void Camera::setActivePose(const Eigen::Matrix4d &pose) {
+	cursor_ = pose; //.inverse();
+}

applications/gui2/src/modules/camera.hpp

@@ -71,17 +71,23 @@ public:
 
 	void snapshot(const std::string &filename);
 
-	Eigen::Matrix4d cursor() const;
+	const Eigen::Matrix4d &cursor() const;
 
-	void setCursorPosition(const Eigen::Vector3f &pos) { cursor_pos_ = pos; }
-	void setCursorNormal(const Eigen::Vector3f &norm) { cursor_normal_ = norm; }
-	void setCursorTarget(const Eigen::Vector3f &targ) { cursor_target_ = targ; }
+	void setCursorPosition(const Eigen::Vector3f &pos) { cursor_pos_ = pos; cursor_ = _cursor(); }
+	void setCursorNormal(const Eigen::Vector3f &norm) { cursor_normal_ = norm; cursor_ = _cursor(); }
+	void setCursorTarget(const Eigen::Vector3f &targ) { cursor_target_ = targ; cursor_ = _cursor(); }
 	void setCursor(int x, int y);
 
 	const Eigen::Vector3f getCursorPosition() const { return cursor_pos_; }
 
 	void setOriginToCursor();
 	void resetOrigin();
+	void saveCursorToPoser();
+
+	Eigen::Matrix4d getActivePose();
+	nanogui::Vector2i getActivePoseScreenCoord();
+	void transformActivePose(const Eigen::Matrix4d &pose);
+	void setActivePose(const Eigen::Matrix4d &pose);
 
 private:
 	int frame_idx = -1;
@@ -102,6 +108,8 @@ private:
 	Eigen::Vector3f cursor_pos_;
 	Eigen::Vector3f cursor_target_;
 	Eigen::Vector3f cursor_normal_;
+	int cursor_save_id_=0;
+	Eigen::Matrix4d cursor_;
 
 	ftl::data::FrameSetPtr current_fs_;
 	ftl::data::FrameSetPtr latest_;
@@ -120,6 +128,7 @@ private:
 
 	void initiate_(ftl::data::Frame &frame);
 	void _updateCapabilities(ftl::data::Frame &frame);
+	Eigen::Matrix4d _cursor() const;
 };
 
 }

applications/gui2/src/modules/camera_tools.hpp

@@ -24,6 +24,9 @@ enum class Tools {
 	ROTATE_X,
 	ROTATE_Y,
 	ROTATE_Z,
+	TRANSLATE_X,
+	TRANSLATE_Y,
+	TRANSLATE_Z,
 	INSPECT_POINT
 };
 

applications/gui2/src/screen.cpp

@@ -25,7 +25,7 @@ using Eigen::Vector2i;
 using ftl::gui2::Screen;
 
 static const int toolbar_w = 50;
-static const Vector2i wsize(1280+toolbar_w,720);
+static const Vector2i wsize(1280,720);
 
 Screen::Screen() :
 		nanogui::Screen(wsize, "FT-Lab Remote Presence"),
@@ -40,6 +40,7 @@ Screen::Screen() :
 	toolbar_->setPosition({0, 0});
 	toolbar_->setWidth(toolbar_w);
 	toolbar_->setHeight(height());
+	toolbar_->setTheme(getTheme("media"));
 
 	setResizeCallback([this](const Vector2i &s) {
 		toolbar_->setFixedSize({toolbar_->width(), s[1]});
@@ -98,7 +99,7 @@ void Screen::redraw() {
 }
 
 nanogui::Vector2i Screen::viewSize(const nanogui::Vector2i &ws) {
-	return {ws.x() - toolbar_->width(), ws.y()};
+	return {ws.x(), ws.y()};
 }
 
 nanogui::Vector2i Screen::viewSize() {
@@ -119,7 +120,7 @@ void Screen::showError(const std::string&title, const std::string& msg) {
 
 void Screen::setView(ftl::gui2::View *view) {
 
-	view->setPosition({toolbar_->width(), 0});
+	view->setPosition({0, 0});
 
 	view->setTheme(getTheme("view"));
 	view->setVisible(true);

applications/gui2/src/views/camera.cpp

@@ -397,7 +397,13 @@ ToolPanel::ToolPanel(nanogui::Widget *parent, ftl::gui2::Camera* ctrl, CameraVie
 		Tools::PAN,
 		Tools::INSPECT_POINT,
 		Tools::ZOOM_IN,
-		Tools::ZOOM_OUT
+		Tools::ZOOM_OUT,
+		Tools::ROTATE_X,
+		Tools::ROTATE_Y,
+		Tools::ROTATE_Z,
+		Tools::TRANSLATE_X,
+		Tools::TRANSLATE_Y,
+		Tools::TRANSLATE_Z
 	});
 	_addButton(mouse_group, Tools::SELECT_POINT, ENTYPO_ICON_MOUSE_POINTER, "Select Point");
 	_addButton(mouse_group, Tools::MOVEMENT, ENTYPO_ICON_MAN, "First Person Camera");
@@ -406,6 +412,17 @@ ToolPanel::ToolPanel(nanogui::Widget *parent, ftl::gui2::Camera* ctrl, CameraVie
 	_addButton(mouse_group, Tools::INSPECT_POINT, ENTYPO_ICON_MAGNIFYING_GLASS, "Inspect Point");
 	_addButton(mouse_group, Tools::ZOOM_IN, ENTYPO_ICON_CIRCLE_WITH_PLUS, "Zoom In (+)");
 	_addButton(mouse_group, Tools::ZOOM_OUT, ENTYPO_ICON_CIRCLE_WITH_MINUS, "Zoom Out (-)");
+	auto *trans_but = _addButton(mouse_group, {
+		Tools::ROTATE_X,
+		Tools::ROTATE_Y,
+		Tools::ROTATE_Z
+	}, ENTYPO_ICON_CYCLE, "Transform Pose");
+	_addButton(trans_but, Tools::ROTATE_X, "Rotate X");
+	_addButton(trans_but, Tools::ROTATE_Y, "Rotate Y");
+	_addButton(trans_but, Tools::ROTATE_Z, "Rotate Z");
+	_addButton(trans_but, Tools::TRANSLATE_X, "Translate X");
+	_addButton(trans_but, Tools::TRANSLATE_Y, "Translate Y");
+	_addButton(trans_but, Tools::TRANSLATE_Z, "Translate Z");
 
 	auto *view2d_group = _addGroup(ToolGroup::VIEW_2D_ACTIONS, Button::Flags::NormalButton, {
 		Tools::CENTRE_VIEW,

applications/gui2/src/views/camera3d.cpp

@@ -44,7 +44,13 @@ CameraView3D::CameraView3D(ftl::gui2::Screen *parent, ftl::gui2::Camera *ctrl) :
 		Tools::MOVE_CURSOR,
 		Tools::ORIGIN_TO_CURSOR,
 		Tools::RESET_ORIGIN,
-		Tools::SAVE_CURSOR
+		Tools::SAVE_CURSOR,
+		Tools::ROTATE_X,
+		Tools::ROTATE_Y,
+		Tools::ROTATE_Z,
+		Tools::TRANSLATE_X,
+		Tools::TRANSLATE_Y,
+		Tools::TRANSLATE_Z
 	});
 
 	setZoom(false);
@@ -61,6 +67,15 @@ CameraView3D::CameraView3D(ftl::gui2::Screen *parent, ftl::gui2::Camera *ctrl) :
 			ctrl_->resetOrigin();
 			tools_->setTool(Tools::MOVEMENT);
 			return true;
+		} else if (tool == Tools::SAVE_CURSOR) {
+			ctrl_->saveCursorToPoser();
+			tools_->setTool(Tools::MOVEMENT);
+			return true;
+		} else if (tool == Tools::ROTATE_X || tool == Tools::ROTATE_Y || tool == Tools::ROTATE_Z ||
+					tool == Tools::TRANSLATE_X || tool == Tools::TRANSLATE_Y || tool == Tools::TRANSLATE_Z) {
+			LOG(INFO) << "Loading cache pose";
+			cache_pose_ = ctrl_->getActivePose();
+			cache_screen_ = ctrl_->getActivePoseScreenCoord();
 		}
 		return false;
 	});
@@ -103,6 +118,9 @@ bool CameraView3D::mouseButtonEvent(const Eigen::Vector2i &p, int button, bool d
 			return true;
 		} else if (tools_->isActive(Tools::ROTATE_CURSOR)) {
 			tools_->setTool(Tools::MOVEMENT);
+		} else if (tools_->isActive(Tools::ROTATE_X) || tools_->isActive(Tools::ROTATE_Y) || tools_->isActive(Tools::ROTATE_Z) ||
+					tools_->isActive(Tools::TRANSLATE_X) || tools_->isActive(Tools::TRANSLATE_Y) || tools_->isActive(Tools::TRANSLATE_Z)) {
+			tools_->setTool(Tools::MOVEMENT);
 		}
 	}
 
@@ -126,6 +144,45 @@ bool CameraView3D::mouseMotionEvent(const Eigen::Vector2i &p, const Eigen::Vecto
 		Eigen::Vector3f world = ctrl_->worldAt(pos.x(), pos.y());
 		ctrl_->setCursorTarget(world);
 		return true;
+	} else if (tools_->isActive(Tools::ROTATE_X)) {
+		auto screen_origin = ctrl_->getActivePoseScreenCoord();
+		double angle = atan2(float(screen_origin[1] - p[1]), float(screen_origin[0] - p[0]));
+		Eigen::Affine3d rx = Eigen::Affine3d(Eigen::AngleAxisd(angle, Eigen::Vector3d(1, 0, 0)));
+		ctrl_->setActivePose(rx.matrix() * cache_pose_);
+	} else if (tools_->isActive(Tools::ROTATE_Y)) {
+		auto screen_origin = ctrl_->getActivePoseScreenCoord();
+		double angle = -atan2(float(screen_origin[1] - p[1]), float(screen_origin[0] - p[0]));
+		Eigen::Affine3d ry = Eigen::Affine3d(Eigen::AngleAxisd(angle, Eigen::Vector3d(0, 1, 0)));
+		ctrl_->setActivePose(ry.matrix() * cache_pose_);
+	} else if (tools_->isActive(Tools::ROTATE_Z)) {
+		auto screen_origin = ctrl_->getActivePoseScreenCoord();
+		double angle = atan2(float(screen_origin[1] - p[1]), float(screen_origin[0] - p[0]));
+		Eigen::Affine3d rz = Eigen::Affine3d(Eigen::AngleAxisd(angle, Eigen::Vector3d(0, 0, 1)));
+		ctrl_->setActivePose(rz.matrix() * cache_pose_);
+	} else if (tools_->isActive(Tools::TRANSLATE_X)) {
+		auto mouse = screen()->mousePos();
+		auto pos = imview_->imageCoordinateAt((mouse - mPos).cast<float>());
+		double dx = pos[0] - double(cache_screen_[0]);
+		//double dy = pos[1] - double(cache_screen_[1]);
+		double dist = dx; //(std::abs(dx) > std::abs(dy)) ? dx : dy;
+		Eigen::Affine3d rx = Eigen::Affine3d(Eigen::Translation3d(dist*0.001, 0.0, 0.0));
+		ctrl_->setActivePose(rx.matrix() * cache_pose_);
+	} else if (tools_->isActive(Tools::TRANSLATE_Y)) {
+		auto mouse = screen()->mousePos();
+		auto pos = imview_->imageCoordinateAt((mouse - mPos).cast<float>());
+		double dx = pos[0] - double(cache_screen_[0]);
+		//double dy = pos[1] - double(cache_screen_[1]);
+		double dist = dx; //(std::abs(dx) > std::abs(dy)) ? dx : dy;
+		Eigen::Affine3d rx = Eigen::Affine3d(Eigen::Translation3d(0.0, dist*0.001, 0.0));
+		ctrl_->setActivePose(rx.matrix() * cache_pose_);
+	} else if (tools_->isActive(Tools::TRANSLATE_Z)) {
+		auto mouse = screen()->mousePos();
+		auto pos = imview_->imageCoordinateAt((mouse - mPos).cast<float>());
+		double dx = pos[0] - double(cache_screen_[0]);
+		//double dy = pos[1] - double(cache_screen_[1]);
+		double dist = dx; //(std::abs(dx) > std::abs(dy)) ? dx : dy;
+		Eigen::Affine3d rx = Eigen::Affine3d(Eigen::Translation3d(0.0, 0.0, dist*0.001));
+		ctrl_->setActivePose(rx.matrix() * cache_pose_);
 	}
 
 	//LOG(INFO) << "New pose: \n" << getUpdatedPose();

applications/gui2/src/views/camera3d.hpp

@@ -28,6 +28,8 @@ protected:
 	// current
 	Eigen::Vector3d eye_;
 	Eigen::Matrix4d rotmat_;
+	Eigen::Matrix4d cache_pose_;
+	Eigen::Vector2i cache_screen_;
 
 	// updates from mouse
 	double rx_;

applications/reconstruct2/src/main.cpp

@@ -129,6 +129,8 @@ static void run(ftl::Configurable *root) {
 	feed->stopRecording();
 	feed->removeFilter(filter);
 
+	ftl::config::save();
+
 	net->shutdown();
 	LOG(INFO) << "Stopping...";
 	ftl::timer::stop(true);

applications/vision/src/main.cpp

@@ -164,6 +164,8 @@ static void run(ftl::Configurable *root) {
 
 	// Send channels on flush
 	auto flushhandle = pool.onFlushSet([sender,&encodable](ftl::data::FrameSet &fs, ftl::codecs::Channel c) {
+		if (!fs.test(ftl::data::FSFlag::AUTO_SEND)) return true;
+
 		// Always send data channels
 		if ((int)c >= 32) sender->post(fs, c);
 		else {
@@ -212,19 +214,18 @@ static void run(ftl::Configurable *root) {
 		if (encodable != previous_encodable) sender->resetEncoders(fs->frameset());
 		previous_encodable = encodable;
 
-		// Must touch the depth channel to indicate it should be waited for
-		//for (auto &f : fs->frames) f.touch(Channel::Depth);
+		if (busy.test_and_set()) {
+			LOG(WARNING) << "Depth pipeline drop: " << fs->timestamp();
+			fs->firstFrame().message(ftl::data::Message::Warning_PIPELINE_DROP, "Depth pipeline drop");
+			return true;
+		}
+
+		fs->set(ftl::data::FSFlag::AUTO_SEND);
 
 		// Do all processing in another thread...
 		ftl::pool.push([sender,&stats_count,&latency,&frames,&stats_time,pipeline,&busy,fs](int id) mutable {
-			// Do pipeline here... if not still busy doing it
-			if (busy.test_and_set()) {
-				LOG(WARNING) << "Depth pipeline drop: " << fs->timestamp();
-				fs->firstFrame().message(ftl::data::Message::Warning_PIPELINE_DROP, "Depth pipeline drop");
-				return;
-			}
+			
 			pipeline->apply(*fs, *fs);
-			busy.clear();
 
 			++frames;
 			latency += float(ftl::timer::get_time() - fs->timestamp());
@@ -232,6 +233,7 @@ static void run(ftl::Configurable *root) {
 			// Destruct frameset as soon as possible to send the data...
 			if (fs->hasAnyChanged(Channel::Depth)) fs->flush(Channel::Depth);
 			const_cast<ftl::data::FrameSetPtr&>(fs).reset();
+			busy.clear();
 
 			if (!quiet && --stats_count <= 0) {
 				latency /= float(frames);
@@ -266,6 +268,8 @@ static void run(ftl::Configurable *root) {
 	ftl::timer::start(true);  // Blocks
 	LOG(INFO) << "Stopping...";
 	ctrl.stop();
+
+	ftl::config::save();
 	
 	net->shutdown();
 

components/codecs/include/ftl/codecs/shapes.hpp

@@ -17,7 +17,8 @@ enum class Shape3DType {
 	CLIPPING,
 	CAMERA,
 	FEATURE,
-	ARUCO
+	ARUCO,
+	CURSOR
 };
 
 struct Shape3D {

components/common/cpp/include/ftl/configuration.hpp

@@ -48,6 +48,8 @@ nlohmann::json &getDefault(const std::string &key);
 
 void cleanup();
 
+void save();
+
 void removeConfigurable(Configurable *cfg);
 
 /**

components/common/cpp/src/configuration.cpp

@@ -685,6 +685,15 @@ static void stripJSON(nlohmann::json &j) {
 	}
 }
 
+void ftl::config::save() {
+	for (auto &f : config_instance) {
+		//LOG(INFO) << "Saving: " << f.second->getID();
+		f.second->save();
+	}
+	stripJSON(config_restore);
+	ftl::saveJSON(std::string(FTL_LOCAL_CONFIG_ROOT "/")+cfg_root_str+std::string("_session.json"), config_restore);
+}
+
 static std::atomic_bool doing_cleanup = false;
 void ftl::config::cleanup() {
 	if (doing_cleanup) return;
@@ -702,8 +711,7 @@ void ftl::config::cleanup() {
 	}
 	config_instance.clear();
 
-	stripJSON(config_restore);
-	ftl::saveJSON(std::string(FTL_LOCAL_CONFIG_ROOT "/")+cfg_root_str+std::string("_session.json"), config_restore);
+	ftl::config::save();
 
 	doing_cleanup = false;
 }

components/net/cpp/src/universe.cpp

@@ -41,8 +41,8 @@ struct NetImplDetail {
 //#define TCP_SEND_BUFFER_SIZE	(512*1024)
 //#define TCP_RECEIVE_BUFFER_SIZE	(1024*1024*1)
 
-#define TCP_SEND_BUFFER_SIZE	(256*1024)
-#define TCP_RECEIVE_BUFFER_SIZE	(256*1024)
+#define TCP_SEND_BUFFER_SIZE	(52*1024)  // Was 256
+#define TCP_RECEIVE_BUFFER_SIZE	(52*1024)  // Was 256
 
 callback_t ftl::net::Universe::cbid__ = 0;
 

components/operators/include/ftl/operators/poser.hpp

@@ -22,19 +22,24 @@ class Poser : public ftl::operators::Operator {
 
 	bool apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) override;
 
+	bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, cudaStream_t stream) override;
+
 	static bool get(const std::string &name, Eigen::Matrix4d &pose);
-	static bool set(const std::string &name, const Eigen::Matrix4d &pose);
+	static bool get(const std::string &name, ftl::codecs::Shape3D &shape);
+	static std::list<ftl::codecs::Shape3D*> getAll(int32_t fsid);
+
+	//static bool set(const ftl::codecs::Shape3D &shape);
+
+	static void add(const ftl::codecs::Shape3D &t, ftl::data::FrameID id);
 
     private:
 	struct PoseState {
-		Eigen::Matrix4d pose;
+		ftl::codecs::Shape3D shape;
 		bool locked;
 	};
 
     static std::unordered_map<std::string,PoseState> pose_db__;
-
-	void add(const ftl::codecs::Shape3D &t, int frameset, int frame);
-
+	static std::unordered_map<int,std::list<ftl::codecs::Shape3D*>> fs_shapes__;
 };
 
 }

components/operators/src/poser.cpp

@@ -8,7 +8,9 @@ using ftl::codecs::Channel;
 using ftl::codecs::Shape3DType;
 using std::string;
 
+static SHARED_MUTEX smtx;
 std::unordered_map<std::string,ftl::operators::Poser::PoseState> Poser::pose_db__;
+std::unordered_map<int,std::list<ftl::codecs::Shape3D*>> Poser::fs_shapes__;
 
 Poser::Poser(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) {
 
@@ -18,36 +20,48 @@ Poser::~Poser() {
 
 }
 
-void Poser::add(const ftl::codecs::Shape3D &t, int frameset, int frame) {
+void Poser::add(const ftl::codecs::Shape3D &t, ftl::data::FrameID id) {
 	std::string idstr;
 	switch(t.type) {
 	case Shape3DType::ARUCO         : idstr = "aruco-"; break;
 	case Shape3DType::CAMERA		: idstr = "camera-"; break;
+	case Shape3DType::CURSOR		: idstr = "cursor-"; break;
 	default                         : idstr = "unk-"; break;
 	}
 
-	idstr += std::to_string(frameset) + string("-") + std::to_string(frame) + string("-") + std::to_string(t.id);
+	idstr += std::to_string(id.frameset()) + string("-") + std::to_string(id.source()) + string("-") + std::to_string(t.id);
 
-	auto pose = t.pose.cast<double>();  // f.getPose() * 
+	//auto pose = t.pose.cast<double>();  // f.getPose() * 
 
+	UNIQUE_LOCK(smtx, lk);
 	auto p = pose_db__.find(idstr);
 	if (p == pose_db__.end()) {
 		ftl::operators::Poser::PoseState ps;
-		ps.pose = pose;
+		ps.shape = t;
 		ps.locked = false;
 		pose_db__.emplace(std::make_pair(idstr,ps));
 		LOG(INFO) << "POSE ID: " << idstr;
+		fs_shapes__[id.frameset()].push_back(&pose_db__[idstr].shape);
 	} else {
 		// TODO: Merge poses
-		if (!(*p).second.locked) (*p).second.pose = pose;
+		if (!(*p).second.locked) (*p).second.shape = t;
 		//LOG(INFO) << "POSE ID: " << idstr;
 	}
 }
 
+std::list<ftl::codecs::Shape3D*> Poser::getAll(int32_t fsid) {
+	SHARED_LOCK(smtx, lk);
+	if (fs_shapes__.count(fsid)) {
+		return fs_shapes__[fsid];
+	}
+	return {};
+}
+
 bool Poser::get(const std::string &name, Eigen::Matrix4d &pose) {
+	SHARED_LOCK(smtx, lk);
 	auto p = pose_db__.find(name);
 	if (p != pose_db__.end()) {
-		pose = (*p).second.pose;
+		pose = (*p).second.shape.pose.cast<double>();
 		return true;
 	} else {
 		LOG(WARNING) << "Pose not found: " << name;
@@ -55,22 +69,6 @@ bool Poser::get(const std::string &name, Eigen::Matrix4d &pose) {
 	}
 }
 
-bool Poser::set(const std::string &name, const Eigen::Matrix4d &pose) {
-	auto p = pose_db__.find(name);
-	if (p == pose_db__.end()) {
-		ftl::operators::Poser::PoseState ps;
-		ps.pose = pose;
-		ps.locked = false;
-		pose_db__.emplace(std::make_pair(name,ps));
-		LOG(INFO) << "POSE ID: " << name;
-	} else {
-		// TODO: Merge poses
-		if (!(*p).second.locked) (*p).second.pose = pose;
-		//LOG(INFO) << "POSE ID: " << idstr;
-	}
-	return true;
-}
-
 bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) {
     if (in.hasChannel(Channel::Shapes3D)) {
         const auto &transforms = in.get<std::list<ftl::codecs::Shape3D>>(Channel::Shapes3D);
@@ -79,7 +77,7 @@ bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_
 
         for (auto &t : transforms) {
         //    LOG(INFO) << "Have FS transform: " << t.label;
-			add(t, in.frameset(), 255);
+			add(t, in.id());
         }
     }
 
@@ -93,7 +91,7 @@ bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_
 				//LOG(INFO) << "Found shapes 3D: " << (int)transforms.size();
 
                 for (auto &t : transforms) {
-                    add(t, in.frameset(), i);
+                    add(t, f.id());
                 }
             }
 
@@ -103,21 +101,46 @@ bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_
 				cam.label = f.name();
 				cam.pose = f.getPose().cast<float>();
 				cam.type = ftl::codecs::Shape3DType::CAMERA;
-				add(cam, in.frameset(), i);
+				add(cam, f.id());
 			}
         //}
     }
 
+	SHARED_LOCK(smtx, lk);
     string pose_ident = config()->value("pose_ident",string("default"));
     if (pose_ident != "default") {
         auto p = pose_db__.find(pose_ident);
         if (p != pose_db__.end()) {
 			(*p).second.locked = config()->value("locked",false);
-            in.cast<ftl::rgbd::Frame>().setPose() = (config()->value("inverse",false)) ? (*p).second.pose.inverse() : (*p).second.pose;
+
+			Eigen::Matrix4d pose = (*p).second.shape.pose.cast<double>();
+            in.cast<ftl::rgbd::Frame>().setPose() = (config()->value("inverse",false)) ? pose.inverse() : pose;
         } else {
             LOG(WARNING) << "Pose not found: " << pose_ident;
         }
     }
 
+	return true;
+}
+
+bool Poser::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, cudaStream_t stream) {
+	auto &f = in;
+
+	if (f.hasChannel(Channel::Shapes3D)) {
+		const auto &transforms = f.get<std::list<ftl::codecs::Shape3D>>(Channel::Shapes3D);
+
+		for (auto &t : transforms) {
+			add(t, f.id());
+		}
+	}
+
+	if (f.hasChannel(Channel::Pose)) {
+		ftl::codecs::Shape3D cam;
+		cam.id = 0;
+		cam.label = f.name();
+		cam.pose = f.getPose().cast<float>();
+		cam.type = ftl::codecs::Shape3DType::CAMERA;
+		add(cam, f.id());
+	}
 	return true;
 }
\ No newline at end of file

components/renderers/cpp/src/overlay.cpp

@@ -5,6 +5,7 @@
 #include <opencv2/imgproc.hpp>
 
 #include <ftl/codecs/shapes.hpp>
+#include <ftl/operators/poser.hpp>
 
 #define LOGURU_REPLACE_GLOG 1
 #include <loguru.hpp>
@@ -365,40 +366,39 @@ void Overlay::draw(NVGcontext *ctx, ftl::data::FrameSet &fs, ftl::rgbd::Frame &f
 			}
 		}*/
 
-		for (size_t i=0; i<fs.frames.size(); ++i) {
-			if (fs.frames[i].hasChannel(Channel::Shapes3D)) {
-				const auto &shapes = fs.frames[i].get<std::list<ftl::codecs::Shape3D>>(Channel::Shapes3D);
+		auto shapes = ftl::operators::Poser::getAll(fs.frameset());
 
-				for (auto &s : shapes) {
-					auto pose = s.pose.cast<double>(); //.inverse() * state.getPose();
-					//Eigen::Vector4d pos = pose.inverse() * Eigen::Vector4d(0,0,0,1);
-					//pos /= pos[3];
+		for (auto *ps : shapes) {
+			auto &s = *ps;
 
-					Eigen::Vector3f scale(s.size[0]/2.0f, s.size[1]/2.0f, s.size[2]/2.0f);
+			auto pose = s.pose.cast<double>(); //.inverse() * state.getPose();
+			//Eigen::Vector4d pos = pose.inverse() * Eigen::Vector4d(0,0,0,1);
+			//pos /= pos[3];
 
-					auto tpose = frame.getPose().inverse() * pose;
+			Eigen::Vector3f scale(s.size[0]/2.0f, s.size[1]/2.0f, s.size[2]/2.0f);
 
-					switch (s.type) {
-					case ftl::codecs::Shape3DType::CAMERA: _drawOutlinedShape(Shape::CAMERA, tpose, scale, make_uchar4(255,0,0,80), make_uchar4(255,0,0,255)); break;
-					case ftl::codecs::Shape3DType::CLIPPING: _drawOutlinedShape(Shape::BOX, tpose, scale, make_uchar4(255,0,255,80), make_uchar4(255,0,255,255)); break;
-					case ftl::codecs::Shape3DType::ARUCO: _drawAxis(tpose, Eigen::Vector3f(0.2f, 0.2f, 0.2f)); break;
-					default: break;
-					}
+			auto tpose = frame.getPose().inverse() * pose;
 
-					if (s.label.size() > 0) {
-						float3 textpos;
-						textpos.x = tpose(0,3);
-						textpos.y = tpose(1,3);
-						textpos.z = tpose(2,3);
+			switch (s.type) {
+			case ftl::codecs::Shape3DType::CAMERA: _drawOutlinedShape(Shape::CAMERA, tpose, scale, make_uchar4(255,0,0,80), make_uchar4(255,0,0,255)); break;
+			case ftl::codecs::Shape3DType::CLIPPING: _drawOutlinedShape(Shape::BOX, tpose, scale, make_uchar4(255,0,255,80), make_uchar4(255,0,255,255)); break;
+			case ftl::codecs::Shape3DType::ARUCO: _drawAxis(tpose, Eigen::Vector3f(0.2f, 0.2f, 0.2f)); break;
+			case ftl::codecs::Shape3DType::CURSOR: _drawAxis(tpose, Eigen::Vector3f(0.2f, 0.2f, 0.2f)); break;
+			default: break;
+			}
 
-						float2 textscreen = frame.getLeft().camToScreen<float2>(textpos);
-						if (textpos.z > 0.1f) nvgText(ctx, textscreen.x, textscreen.y, s.label.c_str(), nullptr);
-					}
+			if (s.label.size() > 0) {
+				float3 textpos;
+				textpos.x = tpose(0,3);
+				textpos.y = tpose(1,3);
+				textpos.z = tpose(2,3);
 
-					//ftl::overlay::drawBox(state.getLeft(), out, over_depth_, pose, cv::Scalar(0,0,255,100), s.size.cast<double>());
-					//ftl::overlay::drawText(state.getLeft(), out, over_depth_, s.label, pos, 0.5, cv::Scalar(0,0,255,100));
-				}
+				float2 textscreen = frame.getLeft().camToScreen<float2>(textpos);
+				if (textpos.z > 0.1f) nvgText(ctx, textscreen.x, textscreen.y, s.label.c_str(), nullptr);
 			}
+
+			//ftl::overlay::drawBox(state.getLeft(), out, over_depth_, pose, cv::Scalar(0,0,255,100), s.size.cast<double>());
+			//ftl::overlay::drawText(state.getLeft(), out, over_depth_, s.label, pos, 0.5, cv::Scalar(0,0,255,100));
 		}
 	}
 

components/streams/src/builder.cpp

@@ -394,7 +394,7 @@ std::shared_ptr<ftl::data::FrameSet> ForeignBuilder::_getFrameset() {
 			ftl::data::FrameSetPtr &f2 = framesets_.back();
 			if (f2.get() == f.get()) break;
 
-			LOG(WARNING) << "FrameSet discarded: " << f2->timestamp();
+			LOG(WARNING) << "FrameSet discarded: " << f2->timestamp() << " (" << f->timestamp() << ")";
 			f2->set(ftl::data::FSFlag::DISCARD);
 			{
 				// Ensure frame processing is finished first

components/streams/src/receiver.cpp

@@ -319,6 +319,7 @@ void Receiver::_finishPacket(ftl::streams::LockedFrameSet &fs, size_t fix) {
 
 	if (frame.packet_tx > 0 && frame.packet_tx == frame.packet_rx) {
 		fs->completed(fix);
+		if (fs->isComplete()) timestamp_ = fs->timestamp();
 		frame.packet_tx = 0;
 		frame.packet_rx = 0;
 	}
@@ -329,7 +330,6 @@ void Receiver::processPackets(const StreamPacket &spkt, const Packet &pkt) {
 
 	if (spkt.channel == Channel::EndFrame) {
 		auto fs = builder(spkt.streamID).get(spkt.timestamp, spkt.frame_number+pkt.frame_count-1);
-		timestamp_ = spkt.timestamp;
 		fs->frames[spkt.frame_number].packet_tx = static_cast<int>(pkt.packet_count);
 		_finishPacket(fs, spkt.frame_number);
 		return;

components/streams/src/renderers/screen_render.cpp

@@ -4,6 +4,7 @@
 #include <ftl/rgbd/capabilities.hpp>
 #include <ftl/operators/antialiasing.hpp>
 #include <ftl/operators/gt_analysis.hpp>
+#include <ftl/operators/poser.hpp>
 #include <ftl/utility/matrix_conversion.hpp>
 #include <ftl/codecs/shapes.hpp>
 
@@ -213,6 +214,7 @@ bool ScreenRender::retrieve(ftl::data::Frame &frame_out) {
 		if (!data_only) {
 			if (!post_pipe_) {
 				post_pipe_ = ftl::config::create<ftl::operators::Graph>(host(), "post_filters");
+				post_pipe_->append<ftl::operators::Poser>("poser");
 				post_pipe_->append<ftl::operators::FXAA>("fxaa");
 				post_pipe_->append<ftl::operators::GTAnalysis>("gtanalyse");
 			}