From 6b238b929aee9d9a3f5bad19341c1563a16e3e1c Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nicolas.pope@utu.fi>
Date: Sat, 22 Aug 2020 14:02:17 +0300
Subject: [PATCH] Allow screen pose saving
---
components/common/cpp/src/configuration.cpp | 2 +-
components/operators/src/poser.cpp | 9 ++++++-
components/rgbd-sources/src/source.cpp | 3 ++-
.../sources/screencapture/screencapture.cpp | 24 ++++++++++++++++++-
4 files changed, 34 insertions(+), 4 deletions(-)
diff --git a/components/common/cpp/src/configuration.cpp b/components/common/cpp/src/configuration.cpp
index 6f717526f..698eee780 100644
--- a/components/common/cpp/src/configuration.cpp
+++ b/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;
diff --git a/components/operators/src/poser.cpp b/components/operators/src/poser.cpp
index 8c748e041..4d220b904 100644
--- a/components/operators/src/poser.cpp
+++ b/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;
}
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 564b95043..83792c34c 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/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());
diff --git a/components/rgbd-sources/src/sources/screencapture/screencapture.cpp b/components/rgbd-sources/src/sources/screencapture/screencapture.cpp
index ab3465af1..dff351014 100644
--- a/components/rgbd-sources/src/sources/screencapture/screencapture.cpp
+++ b/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_;
--
GitLab