diff --git a/components/common/cpp/src/configuration.cpp b/components/common/cpp/src/configuration.cpp index 6f717526f068a182de4ff035ea31fea00130fd52..698eee780aa648d07f00e25ac3566b1efeeb5341 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 8c748e041651947ab4e2ce39fb1efabcaa4a02c5..4d220b90404930198f766cc6fd2fefca2874e726 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 564b95043345ea07a89b754e645052d3969209a7..83792c34cc16f157691d86cf72dfe64aa576e741 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 ab3465af118d6c4f80c41fdeae3044badf29e7c2..dff3510141530c014c22414e8cd79b23e495cc59 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_;