From 532d823dddcea9f42ad23d54db185303bd952619 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nicolas.pope@utu.fi>
Date: Mon, 10 Aug 2020 19:37:28 +0300
Subject: [PATCH] Improve Poser and fix Net Bugs
---
applications/gui2/src/main.cpp | 2 +
applications/gui2/src/modules/camera.cpp | 60 ++++++++++++++-
applications/gui2/src/modules/camera.hpp | 17 ++++-
.../gui2/src/modules/camera_tools.hpp | 3 +
applications/gui2/src/screen.cpp | 7 +-
applications/gui2/src/views/camera.cpp | 19 ++++-
applications/gui2/src/views/camera3d.cpp | 59 ++++++++++++++-
applications/gui2/src/views/camera3d.hpp | 2 +
applications/reconstruct2/src/main.cpp | 2 +
applications/vision/src/main.cpp | 22 +++---
.../codecs/include/ftl/codecs/shapes.hpp | 3 +-
.../common/cpp/include/ftl/configuration.hpp | 2 +
components/common/cpp/src/configuration.cpp | 12 ++-
components/net/cpp/src/universe.cpp | 4 +-
.../operators/include/ftl/operators/poser.hpp | 15 ++--
components/operators/src/poser.cpp | 75 ++++++++++++-------
components/renderers/cpp/src/overlay.cpp | 52 ++++++-------
components/streams/src/builder.cpp | 2 +-
components/streams/src/receiver.cpp | 2 +-
.../streams/src/renderers/screen_render.cpp | 2 +
components/streams/src/sender.cpp | 5 ++
.../include/ftl/data/new_frameset.hpp | 5 +-
components/structures/src/frameset.cpp | 2 +-
23 files changed, 285 insertions(+), 89 deletions(-)
diff --git a/applications/gui2/src/main.cpp b/applications/gui2/src/main.cpp
index 2367bd03f..ee07557d2 100644
--- a/applications/gui2/src/main.cpp
+++ b/applications/gui2/src/main.cpp
@@ -153,6 +153,8 @@ void FTLGui::mainloop() {
// Process events once more
glfwPollEvents();
+ ftl::config::save();
+
// Stop everything before deleting feed etc
LOG(INFO) << "Stopping...";
ftl::timer::stop(true);
diff --git a/applications/gui2/src/modules/camera.cpp b/applications/gui2/src/modules/camera.cpp
index f527a4cc1..72d91779c 100644
--- a/applications/gui2/src/modules/camera.cpp
+++ b/applications/gui2/src/modules/camera.cpp
@@ -8,6 +8,8 @@
#include <ftl/utility/matrix_conversion.hpp>
#include <ftl/calibration/structures.hpp>
#include <ftl/calibration/parameters.hpp>
+#include <ftl/codecs/shapes.hpp>
+#include <ftl/operators/poser.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
@@ -427,7 +429,11 @@ bool Camera::hasFrame() {
return false;
}
-Eigen::Matrix4d Camera::cursor() const {
+const Eigen::Matrix4d &Camera::cursor() const {
+ return cursor_;
+}
+
+Eigen::Matrix4d Camera::_cursor() const {
if (cursor_normal_.norm() > 0.0f) return nanogui::lookAt(cursor_pos_, cursor_target_, cursor_normal_).cast<double>();
Eigen::Matrix4d ident;
@@ -573,17 +579,19 @@ void Camera::setCursor(int x, int y) {
cursor_target_[2] = CP.z;
}
}
+
+ cursor_ = _cursor();
}
void Camera::setOriginToCursor() {
using ftl::calibration::transform::inverse;
// Check for valid cursor
- if (cursor_normal_.norm() == 0.0f) return;
+ /*if (cursor_normal_.norm() == 0.0f) return;
float cursor_length = (cursor_target_ - cursor_pos_).norm();
float cursor_dist = cursor_pos_.norm();
if (cursor_length < 0.01f || cursor_length > 5.0f) return;
- if (cursor_dist > 10.0f) return;
+ if (cursor_dist > 10.0f) return;*/
if (movable_) {
auto *rend = io->feed()->getRenderer(frame_id_);
@@ -612,12 +620,14 @@ void Camera::setOriginToCursor() {
cursor_target_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
cursor_pos_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
cursor_normal_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
+ cursor_ = _cursor();
}
void Camera::resetOrigin() {
cursor_target_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
cursor_pos_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
- cursor_normal_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
+ cursor_normal_ = Eigen::Vector3f(0.0f,0.0f,0.0f);
+ cursor_ = _cursor();
if (movable_) {
auto *rend = io->feed()->getRenderer(frame_id_);
@@ -642,3 +652,45 @@ void Camera::resetOrigin() {
}
}
}
+
+void Camera::saveCursorToPoser() {
+ ftl::codecs::Shape3D shape;
+ shape.type = ftl::codecs::Shape3DType::CURSOR;
+ shape.id = cursor_save_id_++;
+ shape.label = std::string("Cursor") + std::to_string(shape.id);
+ shape.pose = cursor().inverse().cast<float>();
+ shape.size = Eigen::Vector3f(0.1f,0.1f,0.1f);
+
+ ftl::operators::Poser::add(shape, frame_id_);
+}
+
+Eigen::Matrix4d Camera::getActivePose() {
+ return cursor(); //.inverse();
+}
+
+nanogui::Vector2i Camera::getActivePoseScreenCoord() {
+ Eigen::Matrix4d pose = getActivePose().inverse();
+
+ auto ptr = std::atomic_load(&latest_);
+ if (ptr) {
+ const auto &frame = ptr->frames[frame_idx].cast<ftl::rgbd::Frame>();
+ auto campose = frame.getPose().inverse() * pose;
+ float3 campos;
+ campos.x = campose(0,3);
+ campos.y = campose(1,3);
+ campos.z = campose(2,3);
+
+ int2 spos = frame.getLeft().camToScreen<int2>(campos);
+ return nanogui::Vector2i(spos.x, spos.y);
+ }
+
+ return nanogui::Vector2i(-1,-1);
+}
+
+void Camera::transformActivePose(const Eigen::Matrix4d &pose) {
+ cursor_ = pose * cursor_;
+}
+
+void Camera::setActivePose(const Eigen::Matrix4d &pose) {
+ cursor_ = pose; //.inverse();
+}
diff --git a/applications/gui2/src/modules/camera.hpp b/applications/gui2/src/modules/camera.hpp
index d314f0a7a..21ed70dc3 100644
--- a/applications/gui2/src/modules/camera.hpp
+++ b/applications/gui2/src/modules/camera.hpp
@@ -71,17 +71,23 @@ public:
void snapshot(const std::string &filename);
- Eigen::Matrix4d cursor() const;
+ const Eigen::Matrix4d &cursor() const;
- void setCursorPosition(const Eigen::Vector3f &pos) { cursor_pos_ = pos; }
- void setCursorNormal(const Eigen::Vector3f &norm) { cursor_normal_ = norm; }
- void setCursorTarget(const Eigen::Vector3f &targ) { cursor_target_ = targ; }
+ void setCursorPosition(const Eigen::Vector3f &pos) { cursor_pos_ = pos; cursor_ = _cursor(); }
+ void setCursorNormal(const Eigen::Vector3f &norm) { cursor_normal_ = norm; cursor_ = _cursor(); }
+ void setCursorTarget(const Eigen::Vector3f &targ) { cursor_target_ = targ; cursor_ = _cursor(); }
void setCursor(int x, int y);
const Eigen::Vector3f getCursorPosition() const { return cursor_pos_; }
void setOriginToCursor();
void resetOrigin();
+ void saveCursorToPoser();
+
+ Eigen::Matrix4d getActivePose();
+ nanogui::Vector2i getActivePoseScreenCoord();
+ void transformActivePose(const Eigen::Matrix4d &pose);
+ void setActivePose(const Eigen::Matrix4d &pose);
private:
int frame_idx = -1;
@@ -102,6 +108,8 @@ private:
Eigen::Vector3f cursor_pos_;
Eigen::Vector3f cursor_target_;
Eigen::Vector3f cursor_normal_;
+ int cursor_save_id_=0;
+ Eigen::Matrix4d cursor_;
ftl::data::FrameSetPtr current_fs_;
ftl::data::FrameSetPtr latest_;
@@ -120,6 +128,7 @@ private:
void initiate_(ftl::data::Frame &frame);
void _updateCapabilities(ftl::data::Frame &frame);
+ Eigen::Matrix4d _cursor() const;
};
}
diff --git a/applications/gui2/src/modules/camera_tools.hpp b/applications/gui2/src/modules/camera_tools.hpp
index 8e665b842..ebe8386f1 100644
--- a/applications/gui2/src/modules/camera_tools.hpp
+++ b/applications/gui2/src/modules/camera_tools.hpp
@@ -24,6 +24,9 @@ enum class Tools {
ROTATE_X,
ROTATE_Y,
ROTATE_Z,
+ TRANSLATE_X,
+ TRANSLATE_Y,
+ TRANSLATE_Z,
INSPECT_POINT
};
diff --git a/applications/gui2/src/screen.cpp b/applications/gui2/src/screen.cpp
index 6fbd703b8..6925df289 100644
--- a/applications/gui2/src/screen.cpp
+++ b/applications/gui2/src/screen.cpp
@@ -25,7 +25,7 @@ using Eigen::Vector2i;
using ftl::gui2::Screen;
static const int toolbar_w = 50;
-static const Vector2i wsize(1280+toolbar_w,720);
+static const Vector2i wsize(1280,720);
Screen::Screen() :
nanogui::Screen(wsize, "FT-Lab Remote Presence"),
@@ -40,6 +40,7 @@ Screen::Screen() :
toolbar_->setPosition({0, 0});
toolbar_->setWidth(toolbar_w);
toolbar_->setHeight(height());
+ toolbar_->setTheme(getTheme("media"));
setResizeCallback([this](const Vector2i &s) {
toolbar_->setFixedSize({toolbar_->width(), s[1]});
@@ -98,7 +99,7 @@ void Screen::redraw() {
}
nanogui::Vector2i Screen::viewSize(const nanogui::Vector2i &ws) {
- return {ws.x() - toolbar_->width(), ws.y()};
+ return {ws.x(), ws.y()};
}
nanogui::Vector2i Screen::viewSize() {
@@ -119,7 +120,7 @@ void Screen::showError(const std::string&title, const std::string& msg) {
void Screen::setView(ftl::gui2::View *view) {
- view->setPosition({toolbar_->width(), 0});
+ view->setPosition({0, 0});
view->setTheme(getTheme("view"));
view->setVisible(true);
diff --git a/applications/gui2/src/views/camera.cpp b/applications/gui2/src/views/camera.cpp
index 451a032ee..016ce424c 100644
--- a/applications/gui2/src/views/camera.cpp
+++ b/applications/gui2/src/views/camera.cpp
@@ -397,7 +397,13 @@ ToolPanel::ToolPanel(nanogui::Widget *parent, ftl::gui2::Camera* ctrl, CameraVie
Tools::PAN,
Tools::INSPECT_POINT,
Tools::ZOOM_IN,
- Tools::ZOOM_OUT
+ Tools::ZOOM_OUT,
+ Tools::ROTATE_X,
+ Tools::ROTATE_Y,
+ Tools::ROTATE_Z,
+ Tools::TRANSLATE_X,
+ Tools::TRANSLATE_Y,
+ Tools::TRANSLATE_Z
});
_addButton(mouse_group, Tools::SELECT_POINT, ENTYPO_ICON_MOUSE_POINTER, "Select Point");
_addButton(mouse_group, Tools::MOVEMENT, ENTYPO_ICON_MAN, "First Person Camera");
@@ -406,6 +412,17 @@ ToolPanel::ToolPanel(nanogui::Widget *parent, ftl::gui2::Camera* ctrl, CameraVie
_addButton(mouse_group, Tools::INSPECT_POINT, ENTYPO_ICON_MAGNIFYING_GLASS, "Inspect Point");
_addButton(mouse_group, Tools::ZOOM_IN, ENTYPO_ICON_CIRCLE_WITH_PLUS, "Zoom In (+)");
_addButton(mouse_group, Tools::ZOOM_OUT, ENTYPO_ICON_CIRCLE_WITH_MINUS, "Zoom Out (-)");
+ auto *trans_but = _addButton(mouse_group, {
+ Tools::ROTATE_X,
+ Tools::ROTATE_Y,
+ Tools::ROTATE_Z
+ }, ENTYPO_ICON_CYCLE, "Transform Pose");
+ _addButton(trans_but, Tools::ROTATE_X, "Rotate X");
+ _addButton(trans_but, Tools::ROTATE_Y, "Rotate Y");
+ _addButton(trans_but, Tools::ROTATE_Z, "Rotate Z");
+ _addButton(trans_but, Tools::TRANSLATE_X, "Translate X");
+ _addButton(trans_but, Tools::TRANSLATE_Y, "Translate Y");
+ _addButton(trans_but, Tools::TRANSLATE_Z, "Translate Z");
auto *view2d_group = _addGroup(ToolGroup::VIEW_2D_ACTIONS, Button::Flags::NormalButton, {
Tools::CENTRE_VIEW,
diff --git a/applications/gui2/src/views/camera3d.cpp b/applications/gui2/src/views/camera3d.cpp
index 29bb0ae4d..599fc88a9 100644
--- a/applications/gui2/src/views/camera3d.cpp
+++ b/applications/gui2/src/views/camera3d.cpp
@@ -44,7 +44,13 @@ CameraView3D::CameraView3D(ftl::gui2::Screen *parent, ftl::gui2::Camera *ctrl) :
Tools::MOVE_CURSOR,
Tools::ORIGIN_TO_CURSOR,
Tools::RESET_ORIGIN,
- Tools::SAVE_CURSOR
+ Tools::SAVE_CURSOR,
+ Tools::ROTATE_X,
+ Tools::ROTATE_Y,
+ Tools::ROTATE_Z,
+ Tools::TRANSLATE_X,
+ Tools::TRANSLATE_Y,
+ Tools::TRANSLATE_Z
});
setZoom(false);
@@ -61,6 +67,15 @@ CameraView3D::CameraView3D(ftl::gui2::Screen *parent, ftl::gui2::Camera *ctrl) :
ctrl_->resetOrigin();
tools_->setTool(Tools::MOVEMENT);
return true;
+ } else if (tool == Tools::SAVE_CURSOR) {
+ ctrl_->saveCursorToPoser();
+ tools_->setTool(Tools::MOVEMENT);
+ return true;
+ } else if (tool == Tools::ROTATE_X || tool == Tools::ROTATE_Y || tool == Tools::ROTATE_Z ||
+ tool == Tools::TRANSLATE_X || tool == Tools::TRANSLATE_Y || tool == Tools::TRANSLATE_Z) {
+ LOG(INFO) << "Loading cache pose";
+ cache_pose_ = ctrl_->getActivePose();
+ cache_screen_ = ctrl_->getActivePoseScreenCoord();
}
return false;
});
@@ -103,6 +118,9 @@ bool CameraView3D::mouseButtonEvent(const Eigen::Vector2i &p, int button, bool d
return true;
} else if (tools_->isActive(Tools::ROTATE_CURSOR)) {
tools_->setTool(Tools::MOVEMENT);
+ } else if (tools_->isActive(Tools::ROTATE_X) || tools_->isActive(Tools::ROTATE_Y) || tools_->isActive(Tools::ROTATE_Z) ||
+ tools_->isActive(Tools::TRANSLATE_X) || tools_->isActive(Tools::TRANSLATE_Y) || tools_->isActive(Tools::TRANSLATE_Z)) {
+ tools_->setTool(Tools::MOVEMENT);
}
}
@@ -126,6 +144,45 @@ bool CameraView3D::mouseMotionEvent(const Eigen::Vector2i &p, const Eigen::Vecto
Eigen::Vector3f world = ctrl_->worldAt(pos.x(), pos.y());
ctrl_->setCursorTarget(world);
return true;
+ } else if (tools_->isActive(Tools::ROTATE_X)) {
+ auto screen_origin = ctrl_->getActivePoseScreenCoord();
+ double angle = atan2(float(screen_origin[1] - p[1]), float(screen_origin[0] - p[0]));
+ Eigen::Affine3d rx = Eigen::Affine3d(Eigen::AngleAxisd(angle, Eigen::Vector3d(1, 0, 0)));
+ ctrl_->setActivePose(rx.matrix() * cache_pose_);
+ } else if (tools_->isActive(Tools::ROTATE_Y)) {
+ auto screen_origin = ctrl_->getActivePoseScreenCoord();
+ double angle = -atan2(float(screen_origin[1] - p[1]), float(screen_origin[0] - p[0]));
+ Eigen::Affine3d ry = Eigen::Affine3d(Eigen::AngleAxisd(angle, Eigen::Vector3d(0, 1, 0)));
+ ctrl_->setActivePose(ry.matrix() * cache_pose_);
+ } else if (tools_->isActive(Tools::ROTATE_Z)) {
+ auto screen_origin = ctrl_->getActivePoseScreenCoord();
+ double angle = atan2(float(screen_origin[1] - p[1]), float(screen_origin[0] - p[0]));
+ Eigen::Affine3d rz = Eigen::Affine3d(Eigen::AngleAxisd(angle, Eigen::Vector3d(0, 0, 1)));
+ ctrl_->setActivePose(rz.matrix() * cache_pose_);
+ } else if (tools_->isActive(Tools::TRANSLATE_X)) {
+ auto mouse = screen()->mousePos();
+ auto pos = imview_->imageCoordinateAt((mouse - mPos).cast<float>());
+ double dx = pos[0] - double(cache_screen_[0]);
+ //double dy = pos[1] - double(cache_screen_[1]);
+ double dist = dx; //(std::abs(dx) > std::abs(dy)) ? dx : dy;
+ Eigen::Affine3d rx = Eigen::Affine3d(Eigen::Translation3d(dist*0.001, 0.0, 0.0));
+ ctrl_->setActivePose(rx.matrix() * cache_pose_);
+ } else if (tools_->isActive(Tools::TRANSLATE_Y)) {
+ auto mouse = screen()->mousePos();
+ auto pos = imview_->imageCoordinateAt((mouse - mPos).cast<float>());
+ double dx = pos[0] - double(cache_screen_[0]);
+ //double dy = pos[1] - double(cache_screen_[1]);
+ double dist = dx; //(std::abs(dx) > std::abs(dy)) ? dx : dy;
+ Eigen::Affine3d rx = Eigen::Affine3d(Eigen::Translation3d(0.0, dist*0.001, 0.0));
+ ctrl_->setActivePose(rx.matrix() * cache_pose_);
+ } else if (tools_->isActive(Tools::TRANSLATE_Z)) {
+ auto mouse = screen()->mousePos();
+ auto pos = imview_->imageCoordinateAt((mouse - mPos).cast<float>());
+ double dx = pos[0] - double(cache_screen_[0]);
+ //double dy = pos[1] - double(cache_screen_[1]);
+ double dist = dx; //(std::abs(dx) > std::abs(dy)) ? dx : dy;
+ Eigen::Affine3d rx = Eigen::Affine3d(Eigen::Translation3d(0.0, 0.0, dist*0.001));
+ ctrl_->setActivePose(rx.matrix() * cache_pose_);
}
//LOG(INFO) << "New pose: \n" << getUpdatedPose();
diff --git a/applications/gui2/src/views/camera3d.hpp b/applications/gui2/src/views/camera3d.hpp
index b5dda23c2..1d0771ca6 100644
--- a/applications/gui2/src/views/camera3d.hpp
+++ b/applications/gui2/src/views/camera3d.hpp
@@ -28,6 +28,8 @@ protected:
// current
Eigen::Vector3d eye_;
Eigen::Matrix4d rotmat_;
+ Eigen::Matrix4d cache_pose_;
+ Eigen::Vector2i cache_screen_;
// updates from mouse
double rx_;
diff --git a/applications/reconstruct2/src/main.cpp b/applications/reconstruct2/src/main.cpp
index 7aa75b51f..07a0c258c 100644
--- a/applications/reconstruct2/src/main.cpp
+++ b/applications/reconstruct2/src/main.cpp
@@ -129,6 +129,8 @@ static void run(ftl::Configurable *root) {
feed->stopRecording();
feed->removeFilter(filter);
+ ftl::config::save();
+
net->shutdown();
LOG(INFO) << "Stopping...";
ftl::timer::stop(true);
diff --git a/applications/vision/src/main.cpp b/applications/vision/src/main.cpp
index f74fb46da..9c829d1cb 100644
--- a/applications/vision/src/main.cpp
+++ b/applications/vision/src/main.cpp
@@ -164,6 +164,8 @@ static void run(ftl::Configurable *root) {
// Send channels on flush
auto flushhandle = pool.onFlushSet([sender,&encodable](ftl::data::FrameSet &fs, ftl::codecs::Channel c) {
+ if (!fs.test(ftl::data::FSFlag::AUTO_SEND)) return true;
+
// Always send data channels
if ((int)c >= 32) sender->post(fs, c);
else {
@@ -212,19 +214,18 @@ static void run(ftl::Configurable *root) {
if (encodable != previous_encodable) sender->resetEncoders(fs->frameset());
previous_encodable = encodable;
- // Must touch the depth channel to indicate it should be waited for
- //for (auto &f : fs->frames) f.touch(Channel::Depth);
+ if (busy.test_and_set()) {
+ LOG(WARNING) << "Depth pipeline drop: " << fs->timestamp();
+ fs->firstFrame().message(ftl::data::Message::Warning_PIPELINE_DROP, "Depth pipeline drop");
+ return true;
+ }
+
+ fs->set(ftl::data::FSFlag::AUTO_SEND);
// Do all processing in another thread...
ftl::pool.push([sender,&stats_count,&latency,&frames,&stats_time,pipeline,&busy,fs](int id) mutable {
- // Do pipeline here... if not still busy doing it
- if (busy.test_and_set()) {
- LOG(WARNING) << "Depth pipeline drop: " << fs->timestamp();
- fs->firstFrame().message(ftl::data::Message::Warning_PIPELINE_DROP, "Depth pipeline drop");
- return;
- }
+
pipeline->apply(*fs, *fs);
- busy.clear();
++frames;
latency += float(ftl::timer::get_time() - fs->timestamp());
@@ -232,6 +233,7 @@ static void run(ftl::Configurable *root) {
// Destruct frameset as soon as possible to send the data...
if (fs->hasAnyChanged(Channel::Depth)) fs->flush(Channel::Depth);
const_cast<ftl::data::FrameSetPtr&>(fs).reset();
+ busy.clear();
if (!quiet && --stats_count <= 0) {
latency /= float(frames);
@@ -266,6 +268,8 @@ static void run(ftl::Configurable *root) {
ftl::timer::start(true); // Blocks
LOG(INFO) << "Stopping...";
ctrl.stop();
+
+ ftl::config::save();
net->shutdown();
diff --git a/components/codecs/include/ftl/codecs/shapes.hpp b/components/codecs/include/ftl/codecs/shapes.hpp
index 313381887..2368660e5 100644
--- a/components/codecs/include/ftl/codecs/shapes.hpp
+++ b/components/codecs/include/ftl/codecs/shapes.hpp
@@ -17,7 +17,8 @@ enum class Shape3DType {
CLIPPING,
CAMERA,
FEATURE,
- ARUCO
+ ARUCO,
+ CURSOR
};
struct Shape3D {
diff --git a/components/common/cpp/include/ftl/configuration.hpp b/components/common/cpp/include/ftl/configuration.hpp
index d555d6dc5..3f9118947 100644
--- a/components/common/cpp/include/ftl/configuration.hpp
+++ b/components/common/cpp/include/ftl/configuration.hpp
@@ -48,6 +48,8 @@ nlohmann::json &getDefault(const std::string &key);
void cleanup();
+void save();
+
void removeConfigurable(Configurable *cfg);
/**
diff --git a/components/common/cpp/src/configuration.cpp b/components/common/cpp/src/configuration.cpp
index f756d1d57..0e327b539 100644
--- a/components/common/cpp/src/configuration.cpp
+++ b/components/common/cpp/src/configuration.cpp
@@ -685,6 +685,15 @@ static void stripJSON(nlohmann::json &j) {
}
}
+void ftl::config::save() {
+ for (auto &f : config_instance) {
+ //LOG(INFO) << "Saving: " << f.second->getID();
+ f.second->save();
+ }
+ stripJSON(config_restore);
+ ftl::saveJSON(std::string(FTL_LOCAL_CONFIG_ROOT "/")+cfg_root_str+std::string("_session.json"), config_restore);
+}
+
static std::atomic_bool doing_cleanup = false;
void ftl::config::cleanup() {
if (doing_cleanup) return;
@@ -702,8 +711,7 @@ void ftl::config::cleanup() {
}
config_instance.clear();
- stripJSON(config_restore);
- ftl::saveJSON(std::string(FTL_LOCAL_CONFIG_ROOT "/")+cfg_root_str+std::string("_session.json"), config_restore);
+ ftl::config::save();
doing_cleanup = false;
}
diff --git a/components/net/cpp/src/universe.cpp b/components/net/cpp/src/universe.cpp
index 08e4820ff..84633c833 100644
--- a/components/net/cpp/src/universe.cpp
+++ b/components/net/cpp/src/universe.cpp
@@ -41,8 +41,8 @@ struct NetImplDetail {
//#define TCP_SEND_BUFFER_SIZE (512*1024)
//#define TCP_RECEIVE_BUFFER_SIZE (1024*1024*1)
-#define TCP_SEND_BUFFER_SIZE (256*1024)
-#define TCP_RECEIVE_BUFFER_SIZE (256*1024)
+#define TCP_SEND_BUFFER_SIZE (52*1024) // Was 256
+#define TCP_RECEIVE_BUFFER_SIZE (52*1024) // Was 256
callback_t ftl::net::Universe::cbid__ = 0;
diff --git a/components/operators/include/ftl/operators/poser.hpp b/components/operators/include/ftl/operators/poser.hpp
index 732ee47ed..9c642e477 100644
--- a/components/operators/include/ftl/operators/poser.hpp
+++ b/components/operators/include/ftl/operators/poser.hpp
@@ -22,19 +22,24 @@ class Poser : public ftl::operators::Operator {
bool apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) override;
+ bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, cudaStream_t stream) override;
+
static bool get(const std::string &name, Eigen::Matrix4d &pose);
- static bool set(const std::string &name, const Eigen::Matrix4d &pose);
+ static bool get(const std::string &name, ftl::codecs::Shape3D &shape);
+ static std::list<ftl::codecs::Shape3D*> getAll(int32_t fsid);
+
+ //static bool set(const ftl::codecs::Shape3D &shape);
+
+ static void add(const ftl::codecs::Shape3D &t, ftl::data::FrameID id);
private:
struct PoseState {
- Eigen::Matrix4d pose;
+ ftl::codecs::Shape3D shape;
bool locked;
};
static std::unordered_map<std::string,PoseState> pose_db__;
-
- void add(const ftl::codecs::Shape3D &t, int frameset, int frame);
-
+ static std::unordered_map<int,std::list<ftl::codecs::Shape3D*>> fs_shapes__;
};
}
diff --git a/components/operators/src/poser.cpp b/components/operators/src/poser.cpp
index 2f8a166cf..a03682767 100644
--- a/components/operators/src/poser.cpp
+++ b/components/operators/src/poser.cpp
@@ -8,7 +8,9 @@ using ftl::codecs::Channel;
using ftl::codecs::Shape3DType;
using std::string;
+static SHARED_MUTEX smtx;
std::unordered_map<std::string,ftl::operators::Poser::PoseState> Poser::pose_db__;
+std::unordered_map<int,std::list<ftl::codecs::Shape3D*>> Poser::fs_shapes__;
Poser::Poser(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) {
@@ -18,36 +20,48 @@ Poser::~Poser() {
}
-void Poser::add(const ftl::codecs::Shape3D &t, int frameset, int frame) {
+void Poser::add(const ftl::codecs::Shape3D &t, ftl::data::FrameID id) {
std::string idstr;
switch(t.type) {
case Shape3DType::ARUCO : idstr = "aruco-"; break;
case Shape3DType::CAMERA : idstr = "camera-"; break;
+ case Shape3DType::CURSOR : idstr = "cursor-"; break;
default : idstr = "unk-"; break;
}
- idstr += std::to_string(frameset) + string("-") + std::to_string(frame) + string("-") + std::to_string(t.id);
+ idstr += std::to_string(id.frameset()) + string("-") + std::to_string(id.source()) + string("-") + std::to_string(t.id);
- auto pose = t.pose.cast<double>(); // f.getPose() *
+ //auto pose = t.pose.cast<double>(); // f.getPose() *
+ UNIQUE_LOCK(smtx, lk);
auto p = pose_db__.find(idstr);
if (p == pose_db__.end()) {
ftl::operators::Poser::PoseState ps;
- ps.pose = pose;
+ ps.shape = t;
ps.locked = false;
pose_db__.emplace(std::make_pair(idstr,ps));
LOG(INFO) << "POSE ID: " << idstr;
+ fs_shapes__[id.frameset()].push_back(&pose_db__[idstr].shape);
} else {
// TODO: Merge poses
- if (!(*p).second.locked) (*p).second.pose = pose;
+ if (!(*p).second.locked) (*p).second.shape = t;
//LOG(INFO) << "POSE ID: " << idstr;
}
}
+std::list<ftl::codecs::Shape3D*> Poser::getAll(int32_t fsid) {
+ SHARED_LOCK(smtx, lk);
+ if (fs_shapes__.count(fsid)) {
+ return fs_shapes__[fsid];
+ }
+ return {};
+}
+
bool Poser::get(const std::string &name, Eigen::Matrix4d &pose) {
+ SHARED_LOCK(smtx, lk);
auto p = pose_db__.find(name);
if (p != pose_db__.end()) {
- pose = (*p).second.pose;
+ pose = (*p).second.shape.pose.cast<double>();
return true;
} else {
LOG(WARNING) << "Pose not found: " << name;
@@ -55,22 +69,6 @@ bool Poser::get(const std::string &name, Eigen::Matrix4d &pose) {
}
}
-bool Poser::set(const std::string &name, const Eigen::Matrix4d &pose) {
- auto p = pose_db__.find(name);
- if (p == pose_db__.end()) {
- ftl::operators::Poser::PoseState ps;
- ps.pose = pose;
- ps.locked = false;
- pose_db__.emplace(std::make_pair(name,ps));
- LOG(INFO) << "POSE ID: " << name;
- } else {
- // TODO: Merge poses
- if (!(*p).second.locked) (*p).second.pose = pose;
- //LOG(INFO) << "POSE ID: " << idstr;
- }
- return true;
-}
-
bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) {
if (in.hasChannel(Channel::Shapes3D)) {
const auto &transforms = in.get<std::list<ftl::codecs::Shape3D>>(Channel::Shapes3D);
@@ -79,7 +77,7 @@ bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_
for (auto &t : transforms) {
// LOG(INFO) << "Have FS transform: " << t.label;
- add(t, in.frameset(), 255);
+ add(t, in.id());
}
}
@@ -93,7 +91,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) {
- add(t, in.frameset(), i);
+ add(t, f.id());
}
}
@@ -103,21 +101,46 @@ bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_
cam.label = f.name();
cam.pose = f.getPose().cast<float>();
cam.type = ftl::codecs::Shape3DType::CAMERA;
- add(cam, in.frameset(), i);
+ add(cam, f.id());
}
//}
}
+ SHARED_LOCK(smtx, lk);
string pose_ident = config()->value("pose_ident",string("default"));
if (pose_ident != "default") {
auto p = pose_db__.find(pose_ident);
if (p != pose_db__.end()) {
(*p).second.locked = config()->value("locked",false);
- in.cast<ftl::rgbd::Frame>().setPose() = (config()->value("inverse",false)) ? (*p).second.pose.inverse() : (*p).second.pose;
+
+ Eigen::Matrix4d pose = (*p).second.shape.pose.cast<double>();
+ in.cast<ftl::rgbd::Frame>().setPose() = (config()->value("inverse",false)) ? pose.inverse() : pose;
} else {
LOG(WARNING) << "Pose not found: " << pose_ident;
}
}
+ return true;
+}
+
+bool Poser::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, cudaStream_t stream) {
+ auto &f = in;
+
+ if (f.hasChannel(Channel::Shapes3D)) {
+ const auto &transforms = f.get<std::list<ftl::codecs::Shape3D>>(Channel::Shapes3D);
+
+ for (auto &t : transforms) {
+ add(t, f.id());
+ }
+ }
+
+ if (f.hasChannel(Channel::Pose)) {
+ ftl::codecs::Shape3D cam;
+ cam.id = 0;
+ cam.label = f.name();
+ cam.pose = f.getPose().cast<float>();
+ cam.type = ftl::codecs::Shape3DType::CAMERA;
+ add(cam, f.id());
+ }
return true;
}
\ No newline at end of file
diff --git a/components/renderers/cpp/src/overlay.cpp b/components/renderers/cpp/src/overlay.cpp
index 611e9737c..7562ffbf9 100644
--- a/components/renderers/cpp/src/overlay.cpp
+++ b/components/renderers/cpp/src/overlay.cpp
@@ -5,6 +5,7 @@
#include <opencv2/imgproc.hpp>
#include <ftl/codecs/shapes.hpp>
+#include <ftl/operators/poser.hpp>
#define LOGURU_REPLACE_GLOG 1
#include <loguru.hpp>
@@ -365,40 +366,39 @@ void Overlay::draw(NVGcontext *ctx, ftl::data::FrameSet &fs, ftl::rgbd::Frame &f
}
}*/
- for (size_t i=0; i<fs.frames.size(); ++i) {
- if (fs.frames[i].hasChannel(Channel::Shapes3D)) {
- const auto &shapes = fs.frames[i].get<std::list<ftl::codecs::Shape3D>>(Channel::Shapes3D);
+ auto shapes = ftl::operators::Poser::getAll(fs.frameset());
- for (auto &s : shapes) {
- auto pose = s.pose.cast<double>(); //.inverse() * state.getPose();
- //Eigen::Vector4d pos = pose.inverse() * Eigen::Vector4d(0,0,0,1);
- //pos /= pos[3];
+ for (auto *ps : shapes) {
+ auto &s = *ps;
- Eigen::Vector3f scale(s.size[0]/2.0f, s.size[1]/2.0f, s.size[2]/2.0f);
+ auto pose = s.pose.cast<double>(); //.inverse() * state.getPose();
+ //Eigen::Vector4d pos = pose.inverse() * Eigen::Vector4d(0,0,0,1);
+ //pos /= pos[3];
- auto tpose = frame.getPose().inverse() * pose;
+ Eigen::Vector3f scale(s.size[0]/2.0f, s.size[1]/2.0f, s.size[2]/2.0f);
- switch (s.type) {
- case ftl::codecs::Shape3DType::CAMERA: _drawOutlinedShape(Shape::CAMERA, tpose, scale, make_uchar4(255,0,0,80), make_uchar4(255,0,0,255)); break;
- case ftl::codecs::Shape3DType::CLIPPING: _drawOutlinedShape(Shape::BOX, tpose, scale, make_uchar4(255,0,255,80), make_uchar4(255,0,255,255)); break;
- case ftl::codecs::Shape3DType::ARUCO: _drawAxis(tpose, Eigen::Vector3f(0.2f, 0.2f, 0.2f)); break;
- default: break;
- }
+ auto tpose = frame.getPose().inverse() * pose;
- if (s.label.size() > 0) {
- float3 textpos;
- textpos.x = tpose(0,3);
- textpos.y = tpose(1,3);
- textpos.z = tpose(2,3);
+ switch (s.type) {
+ case ftl::codecs::Shape3DType::CAMERA: _drawOutlinedShape(Shape::CAMERA, tpose, scale, make_uchar4(255,0,0,80), make_uchar4(255,0,0,255)); break;
+ case ftl::codecs::Shape3DType::CLIPPING: _drawOutlinedShape(Shape::BOX, tpose, scale, make_uchar4(255,0,255,80), make_uchar4(255,0,255,255)); break;
+ case ftl::codecs::Shape3DType::ARUCO: _drawAxis(tpose, Eigen::Vector3f(0.2f, 0.2f, 0.2f)); break;
+ case ftl::codecs::Shape3DType::CURSOR: _drawAxis(tpose, Eigen::Vector3f(0.2f, 0.2f, 0.2f)); break;
+ default: break;
+ }
- float2 textscreen = frame.getLeft().camToScreen<float2>(textpos);
- if (textpos.z > 0.1f) nvgText(ctx, textscreen.x, textscreen.y, s.label.c_str(), nullptr);
- }
+ if (s.label.size() > 0) {
+ float3 textpos;
+ textpos.x = tpose(0,3);
+ textpos.y = tpose(1,3);
+ textpos.z = tpose(2,3);
- //ftl::overlay::drawBox(state.getLeft(), out, over_depth_, pose, cv::Scalar(0,0,255,100), s.size.cast<double>());
- //ftl::overlay::drawText(state.getLeft(), out, over_depth_, s.label, pos, 0.5, cv::Scalar(0,0,255,100));
- }
+ float2 textscreen = frame.getLeft().camToScreen<float2>(textpos);
+ if (textpos.z > 0.1f) nvgText(ctx, textscreen.x, textscreen.y, s.label.c_str(), nullptr);
}
+
+ //ftl::overlay::drawBox(state.getLeft(), out, over_depth_, pose, cv::Scalar(0,0,255,100), s.size.cast<double>());
+ //ftl::overlay::drawText(state.getLeft(), out, over_depth_, s.label, pos, 0.5, cv::Scalar(0,0,255,100));
}
}
diff --git a/components/streams/src/builder.cpp b/components/streams/src/builder.cpp
index 33d19b946..58f2dd3c9 100644
--- a/components/streams/src/builder.cpp
+++ b/components/streams/src/builder.cpp
@@ -394,7 +394,7 @@ std::shared_ptr<ftl::data::FrameSet> ForeignBuilder::_getFrameset() {
ftl::data::FrameSetPtr &f2 = framesets_.back();
if (f2.get() == f.get()) break;
- LOG(WARNING) << "FrameSet discarded: " << f2->timestamp();
+ LOG(WARNING) << "FrameSet discarded: " << f2->timestamp() << " (" << f->timestamp() << ")";
f2->set(ftl::data::FSFlag::DISCARD);
{
// Ensure frame processing is finished first
diff --git a/components/streams/src/receiver.cpp b/components/streams/src/receiver.cpp
index 9db6ee792..aee169927 100644
--- a/components/streams/src/receiver.cpp
+++ b/components/streams/src/receiver.cpp
@@ -319,6 +319,7 @@ void Receiver::_finishPacket(ftl::streams::LockedFrameSet &fs, size_t fix) {
if (frame.packet_tx > 0 && frame.packet_tx == frame.packet_rx) {
fs->completed(fix);
+ if (fs->isComplete()) timestamp_ = fs->timestamp();
frame.packet_tx = 0;
frame.packet_rx = 0;
}
@@ -329,7 +330,6 @@ void Receiver::processPackets(const StreamPacket &spkt, const Packet &pkt) {
if (spkt.channel == Channel::EndFrame) {
auto fs = builder(spkt.streamID).get(spkt.timestamp, spkt.frame_number+pkt.frame_count-1);
- timestamp_ = spkt.timestamp;
fs->frames[spkt.frame_number].packet_tx = static_cast<int>(pkt.packet_count);
_finishPacket(fs, spkt.frame_number);
return;
diff --git a/components/streams/src/renderers/screen_render.cpp b/components/streams/src/renderers/screen_render.cpp
index 3df19e7a3..7b74511b9 100644
--- a/components/streams/src/renderers/screen_render.cpp
+++ b/components/streams/src/renderers/screen_render.cpp
@@ -4,6 +4,7 @@
#include <ftl/rgbd/capabilities.hpp>
#include <ftl/operators/antialiasing.hpp>
#include <ftl/operators/gt_analysis.hpp>
+#include <ftl/operators/poser.hpp>
#include <ftl/utility/matrix_conversion.hpp>
#include <ftl/codecs/shapes.hpp>
@@ -213,6 +214,7 @@ bool ScreenRender::retrieve(ftl::data::Frame &frame_out) {
if (!data_only) {
if (!post_pipe_) {
post_pipe_ = ftl::config::create<ftl::operators::Graph>(host(), "post_filters");
+ post_pipe_->append<ftl::operators::Poser>("poser");
post_pipe_->append<ftl::operators::FXAA>("fxaa");
post_pipe_->append<ftl::operators::GTAnalysis>("gtanalyse");
}
diff --git a/components/streams/src/sender.cpp b/components/streams/src/sender.cpp
index 62e48bd0b..5e153f82e 100644
--- a/components/streams/src/sender.cpp
+++ b/components/streams/src/sender.cpp
@@ -164,6 +164,11 @@ void Sender::post(ftl::data::FrameSet &fs, ftl::codecs::Channel c, bool noencode
// Send quick message for this special channel.
if (c == Channel::EndFrame) {
+ if (timestamp_ >= fs.timestamp()) {
+ LOG(WARNING) << "Sending old frame! " << fs.timestamp() << " vs " << timestamp_ << " (size = " << fs.frames[0].packet_tx+1 << ")";
+ }
+ timestamp_ = fs.timestamp();
+
StreamPacket spkt;
spkt.version = 5;
spkt.timestamp = fs.timestamp();
diff --git a/components/structures/include/ftl/data/new_frameset.hpp b/components/structures/include/ftl/data/new_frameset.hpp
index c471eb5a0..90be7f7be 100644
--- a/components/structures/include/ftl/data/new_frameset.hpp
+++ b/components/structures/include/ftl/data/new_frameset.hpp
@@ -20,7 +20,8 @@ static const size_t kMaxFramesInSet = 32;
enum class FSFlag : int {
STALE = 0,
PARTIAL = 1,
- DISCARD = 4
+ DISCARD = 4,
+ AUTO_SEND = 8
};
/**
@@ -77,7 +78,7 @@ class FrameSet : public ftl::data::Frame {
/**
* Are all frames complete within this frameset?
*/
- inline bool isComplete() { return ftl::popcount(mask) == frames.size(); }
+ inline bool isComplete() { return mask != 0 && ftl::popcount(mask) == frames.size(); }
/**
* Check that a given frame is valid in this frameset.
diff --git a/components/structures/src/frameset.cpp b/components/structures/src/frameset.cpp
index f6c9ff458..81848289e 100644
--- a/components/structures/src/frameset.cpp
+++ b/components/structures/src/frameset.cpp
@@ -12,6 +12,7 @@ FrameSet::FrameSet(Pool *ppool, FrameID pid, int64_t ts, size_t psize) :
FrameSet::~FrameSet() {
if (status() == ftl::data::FrameStatus::CREATED) store();
if (status() == ftl::data::FrameStatus::STORED) flush();
+ pool()->flush_fs_.trigger(*this, ftl::codecs::Channel::EndFrame);
}
void ftl::data::FrameSet::completed(size_t ix) {
@@ -103,7 +104,6 @@ void FrameSet::flush() {
for (auto c : unflushed) {
pool()->flush_fs_.trigger(*this, c);
}
- pool()->flush_fs_.trigger(*this, ftl::codecs::Channel::EndFrame);
}
void FrameSet::flush(ftl::codecs::Channel c) {
--
GitLab