From 41a3956bbdd8371a9cef96847e186dd546900999 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Sat, 16 May 2020 19:54:39 +0300
Subject: [PATCH] Allow a pose shape to be used for v cam
---
applications/gui/src/camera.cpp | 42 ++++++++----
applications/gui/src/camera.hpp | 1 +
.../operators/include/ftl/operators/poser.hpp | 5 ++
components/operators/src/poser.cpp | 66 ++++++++++++-------
4 files changed, 75 insertions(+), 39 deletions(-)
diff --git a/applications/gui/src/camera.cpp b/applications/gui/src/camera.cpp
index 03306b423..c207073c7 100644
--- a/applications/gui/src/camera.cpp
+++ b/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_;
+ }
+ }
}
{
diff --git a/applications/gui/src/camera.hpp b/applications/gui/src/camera.hpp
index a14be6294..cb34ff0f7 100644
--- a/applications/gui/src/camera.hpp
+++ b/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_;
diff --git a/components/operators/include/ftl/operators/poser.hpp b/components/operators/include/ftl/operators/poser.hpp
index 2b11bf9a9..96e6b1926 100644
--- a/components/operators/include/ftl/operators/poser.hpp
+++ b/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);
+
};
}
diff --git a/components/operators/src/poser.cpp b/components/operators/src/poser.cpp
index 3d136e1c9..d03ef35fe 100644
--- a/components/operators/src/poser.cpp
+++ b/components/operators/src/poser.cpp
@@ -1,5 +1,4 @@
#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);
}
}
}
--
GitLab