Merge branch 'feature/posesave' into 'master'

Allow screen pose saving

See merge request nicolas.pope/ftl!342

components/common/cpp/src/configuration.cpp

@@ -667,7 +667,7 @@ static void stripJSON(nlohmann::json &j) {
 			i = j.erase(i);
 			continue;
 		}
-        if ((*i).is_structured()) {
+        if ((*i).is_object()) {
             stripJSON(*i);
         }
 		++i;

components/operators/src/poser.cpp

@@ -114,7 +114,14 @@ bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_
 			(*p).second.locked = config()->value("locked",false);
 
 			Eigen::Matrix4d pose = (*p).second.shape.pose.cast<double>();
-            in.cast<ftl::rgbd::Frame>().setPose() = (config()->value("inverse",false)) ? pose.inverse() : pose;
+
+			if (in.frames.size() == 1) {
+				auto response = in.frames[0].response();
+				auto &rgbdf = response.cast<ftl::rgbd::Frame>();
+				rgbdf.setPose() = (config()->value("inverse",false)) ? pose.inverse() : pose;
+			} else {
+            	in.cast<ftl::rgbd::Frame>().setPose() = (config()->value("inverse",false)) ? pose.inverse() : pose;
+			}
         } else {
             LOG(WARNING) << "Pose not found: " << pose_ident;
         }

components/rgbd-sources/src/source.cpp

@@ -146,7 +146,8 @@ void Source::reset() {
 		"device_left",
 		"enable_touch",
 		"feed",
-		"pipeline"
+		"pipeline",
+		"pose"
 	});
 
 	uri.to_json(getConfig());

components/rgbd-sources/src/sources/screencapture/screencapture.cpp

@@ -12,6 +12,8 @@
 
 #include <opencv2/imgproc.hpp>
 
+#include <nlohmann/json.hpp>
+
 using ftl::rgbd::detail::ScreenCapture;
 using ftl::codecs::Channel;
 using cv::cuda::GpuMat;
@@ -178,7 +180,16 @@ ScreenCapture::ScreenCapture(ftl::rgbd::Source *host)
 
 	float offsetz = host_->value("offset_z",0.0f);
 	//state_.setPose(matrix(cv::Vec3d(0.0, 3.14159, 0.0), cv::Vec3d(0.0,0.0,params_.maxDepth+offsetz)));
-	pose_ = matrix(cv::Vec3d(0.0, 3.14159, 0.0), cv::Vec3d(0.0,0.0,params_.maxDepth+offsetz));
+
+	if (host_->getConfig().contains("pose") && host_->getConfig()["pose"].is_array()) {
+		LOG(INFO) << "Loading saved screen pose.";
+		std::vector<double> d = host_->getConfig()["pose"].get<std::vector<double>>();
+		for (int i=0; i<16; ++i) {
+			pose_.data()[i] = d[i];
+		}
+	} else {
+		pose_ = matrix(cv::Vec3d(0.0, 3.14159, 0.0), cv::Vec3d(0.0,0.0,params_.maxDepth+offsetz));
+	}
 
 	host_->on("size", [this]() {
 		float offsetz = host_->value("offset_z",0.0f);
@@ -338,6 +349,17 @@ bool ScreenCapture::retrieve(ftl::rgbd::Frame &frame) {
 		}
 	}
 
+	if (frame.changed(Channel::Pose)) {
+		LOG(INFO) << "Pose has been updated...";
+		Eigen::Matrix4d p = frame.getPose();
+		std::vector<double> d;
+		d.resize(16);
+		for (int i=0; i<16; ++i) {
+			d[i] = p.data()[i];
+		}
+		host_->getConfig()["pose"] = d;
+	}
+
 	if (do_update_params_) {
 		frame.setPose() = pose_;
 		frame.setLeft() = params_;