Allow a pose shape to be used for v cam

applications/gui/src/camera.cpp

@@ -14,6 +14,7 @@
 #include <ftl/render/colouriser.hpp>
 #include <ftl/cuda/transform.hpp>
 #include <ftl/operators/gt_analysis.hpp>
+#include <ftl/operators/poser.hpp>
 #include <ftl/cuda/colour_cuda.hpp>
 
 #include <ftl/render/overlay.hpp>
@@ -99,6 +100,12 @@ ftl::gui::Camera::Camera(ftl::gui::Screen *screen, int fsmask, int fid, ftl::cod
 			fsmask_ = renderer_->value("fsmask", fsmask_);
 		});
 
+		// Allow Pose origin to be changed
+		pose_source_ = renderer_->value("pose_source", pose_source_);
+		renderer_->on("pose_source", [this](const ftl::config::Event &e) {
+			pose_source_ = renderer_->value("pose_source", pose_source_);
+		});
+
 		intrinsics_ = ftl::create<ftl::Configurable>(renderer_, "intrinsics");
 
 		state_.getLeft() = ftl::rgbd::Camera::from(intrinsics_);
@@ -758,25 +765,32 @@ const void ftl::gui::Camera::captureFrame() {
 			}
 			#endif
 		} else {
-			// Use mouse to move camera
+			if (pose_source_.size() == 0) {
+				// Use mouse to move camera
 
-			float rrx = ((float)ry_ * 0.2f * delta_);
-			float rry = (float)rx_ * 0.2f * delta_;
-			float rrz = 0.0;
+				float rrx = ((float)ry_ * 0.2f * delta_);
+				float rry = (float)rx_ * 0.2f * delta_;
+				float rrz = 0.0;
 
-			Eigen::Affine3d r = create_rotation_matrix(rrx, -rry, rrz);
-			rotmat_ = rotmat_ * r.matrix();
+				Eigen::Affine3d r = create_rotation_matrix(rrx, -rry, rrz);
+				rotmat_ = rotmat_ * r.matrix();
 
-			rx_ = 0;
-			ry_ = 0;
+				rx_ = 0;
+				ry_ = 0;
 
-			eye_[0] += (neye_[0] - eye_[0]) * lerpSpeed_ * delta_;
-			eye_[1] += (neye_[1] - eye_[1]) * lerpSpeed_ * delta_;
-			eye_[2] += (neye_[2] - eye_[2]) * lerpSpeed_ * delta_;
+				eye_[0] += (neye_[0] - eye_[0]) * lerpSpeed_ * delta_;
+				eye_[1] += (neye_[1] - eye_[1]) * lerpSpeed_ * delta_;
+				eye_[2] += (neye_[2] - eye_[2]) * lerpSpeed_ * delta_;
 
-			Eigen::Translation3d trans(eye_);
-			Eigen::Affine3d t(trans);
-			viewPose = t.matrix() * rotmat_;
+				Eigen::Translation3d trans(eye_);
+				Eigen::Affine3d t(trans);
+				viewPose = t.matrix() * rotmat_;
+			} else {
+				// Use some other pose source.
+				if (!ftl::operators::Poser::get(pose_source_, viewPose)) {
+					LOG(ERROR) << "Missing pose: " << pose_source_;
+				}
+			}
 		}
 
 		{

applications/gui/src/camera.hpp

@@ -148,6 +148,7 @@ class Camera {
 	bool sdepth_;
 	bool pause_;
 	bool do_snapshot_ = false;
+	std::string pose_source_;
 	std::string snapshot_filename_;
 	ftl::codecs::Channel channel_;
 	ftl::codecs::Channels<0> channels_;

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

@@ -3,6 +3,7 @@
 
 #include <ftl/operators/operator.hpp>
 #include <ftl/cuda_common.hpp>
+#include <ftl/codecs/shapes.hpp>
 
 #include <unordered_map>
 
@@ -21,6 +22,8 @@ class Poser : public ftl::operators::Operator {
 
 	bool apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) override;
 
+	static bool get(const std::string &name, Eigen::Matrix4d &pose);
+
     private:
 	struct PoseState {
 		Eigen::Matrix4d pose;
@@ -29,6 +32,8 @@ class Poser : public ftl::operators::Operator {
 
     static std::unordered_map<std::string,PoseState> pose_db__;
 
+	void add(const ftl::codecs::Shape3D &t, int frameset, int frame);
+
 };
 
 }

components/operators/src/poser.cpp

 #include <ftl/operators/poser.hpp>
-#include <ftl/codecs/shapes.hpp>
 
 #define LOGURU_REPLACE_GLOG 1
 #include <loguru.hpp>
@@ -19,6 +18,43 @@ Poser::~Poser() {
 
 }
 
+void Poser::add(const ftl::codecs::Shape3D &t, int frameset, int frame) {
+	std::string idstr;
+	switch(t.type) {
+	case Shape3DType::ARUCO         : idstr = "aruco-"; break;
+	case Shape3DType::CAMERA		: idstr = "camera-"; break;
+	default                         : idstr = "unk-"; break;
+	}
+
+	idstr += std::to_string(frameset) + string("-") + std::to_string(frame) + string("-") + std::to_string(t.id);
+
+	auto pose = t.pose.cast<double>();  // f.getPose() * 
+
+	auto p = pose_db__.find(idstr);
+	if (p == pose_db__.end()) {
+		ftl::operators::Poser::PoseState ps;
+		ps.pose = pose;
+		ps.locked = false;
+		pose_db__.emplace(std::make_pair(idstr,ps));
+		LOG(INFO) << "POSE ID: " << idstr;
+	} else {
+		// TODO: Merge poses
+		if (!(*p).second.locked) (*p).second.pose = pose;
+		//LOG(INFO) << "POSE ID: " << idstr;
+	}
+}
+
+bool Poser::get(const std::string &name, Eigen::Matrix4d &pose) {
+	auto p = pose_db__.find(name);
+	if (p != pose_db__.end()) {
+		pose = (*p).second.pose;
+		return true;
+	} else {
+		LOG(WARNING) << "Pose not found: " << name;
+		return false;
+	}
+}
+
 bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) {
     if (in.hasChannel(Channel::Shapes3D)) {
         std::vector<ftl::codecs::Shape3D> transforms;
@@ -26,9 +62,10 @@ bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_
 
 		//LOG(INFO) << "Found shapes 3D global: " << (int)transforms.size();
 
-        //for (auto &t : transforms) {
+        for (auto &t : transforms) {
         //    LOG(INFO) << "Have FS transform: " << t.label;
-        //}
+			add(t, in.id, 255);
+        }
     }
 
 	for (size_t i=0; i<in.frames.size(); ++i) {
@@ -42,28 +79,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) {
-                    std::string idstr;
-                    switch(t.type) {
-                    case Shape3DType::ARUCO         : idstr = "aruco-"; break;
-                    default                         : idstr = "unk-"; break;
-                    }
-
-                    idstr += std::to_string(in.id) + string("-") + std::to_string(i) + string("-") + std::to_string(t.id);
-
-                    auto pose = t.pose.cast<double>();  // f.getPose() * 
-
-                    auto p = pose_db__.find(idstr);
-                    if (p == pose_db__.end()) {
-						ftl::operators::Poser::PoseState ps;
-						ps.pose = pose;
-						ps.locked = false;
-						pose_db__.emplace(std::make_pair(idstr,ps));
-                        LOG(INFO) << "POSE ID: " << idstr;
-                    } else {
-                        // TODO: Merge poses
-                        if (!(*p).second.locked) (*p).second.pose = pose;
-						//LOG(INFO) << "POSE ID: " << idstr;
-                    }
+                    add(t, in.id, i);
                 }
             }
         }