From a30e27703473b7ce3d9a5f688248a5c6c39638ad Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nicolas.pope@utu.fi>
Date: Fri, 7 Aug 2020 10:25:44 +0300
Subject: [PATCH] Add pose adjustment and toolbar
---
applications/gui2/src/modules/camera.cpp | 171 +++++++++-
applications/gui2/src/modules/camera.hpp | 18 ++
.../gui2/src/modules/camera_tools.hpp | 40 +++
applications/gui2/src/modules/themes.cpp | 18 ++
applications/gui2/src/views/camera.cpp | 294 +++++++++++++++++-
applications/gui2/src/views/camera.hpp | 48 +++
applications/gui2/src/views/camera3d.cpp | 63 +++-
components/calibration/CMakeLists.txt | 1 +
.../include/ftl/calibration/stereorectify.hpp | 51 +++
.../include/ftl/calibration/structures.hpp | 11 +-
components/calibration/src/stereorectify.cpp | 43 +++
components/calibration/src/structures.cpp | 12 +-
.../codecs/include/ftl/codecs/channels.hpp | 3 +-
components/control/cpp/src/master.cpp | 3 +-
components/operators/src/operator.cpp | 11 +
.../cpp/include/ftl/render/overlay.hpp | 2 +-
components/renderers/cpp/src/overlay.cpp | 7 +-
.../rgbd-sources/include/ftl/rgbd/camera.hpp | 4 +-
.../rgbd-sources/include/ftl/rgbd/frame.hpp | 7 +
components/rgbd-sources/src/camera.cpp | 11 +
components/rgbd-sources/src/frame.cpp | 10 +-
.../src/sources/stereovideo/rectification.cpp | 10 +-
.../src/sources/stereovideo/rectification.hpp | 3 -
.../src/sources/stereovideo/stereovideo.cpp | 24 +-
components/rgbd-sources/test/CMakeLists.txt | 2 +-
.../streams/include/ftl/streams/feed.hpp | 3 +
.../streams/include/ftl/streams/renderer.hpp | 2 +
.../streams/include/ftl/streams/sender.hpp | 3 +-
components/streams/src/baserender.hpp | 2 +
components/streams/src/renderer.cpp | 5 +
.../streams/src/renderers/openvr_render.hpp | 2 +
.../streams/src/renderers/screen_render.hpp | 2 +
components/streams/src/sender.cpp | 34 +-
.../structures/include/ftl/data/new_frame.hpp | 36 ++-
34 files changed, 886 insertions(+), 70 deletions(-)
create mode 100644 applications/gui2/src/modules/camera_tools.hpp
create mode 100644 components/calibration/include/ftl/calibration/stereorectify.hpp
create mode 100644 components/calibration/src/stereorectify.cpp
diff --git a/applications/gui2/src/modules/camera.cpp b/applications/gui2/src/modules/camera.cpp
index 1ed0d23a5..f527a4cc1 100644
--- a/applications/gui2/src/modules/camera.cpp
+++ b/applications/gui2/src/modules/camera.cpp
@@ -5,10 +5,14 @@
#include <ftl/rgbd/capabilities.hpp>
#include <ftl/streams/renderer.hpp>
#include <chrono>
+#include <ftl/utility/matrix_conversion.hpp>
+#include <ftl/calibration/structures.hpp>
+#include <ftl/calibration/parameters.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/cudaarithm.hpp>
+#include <opencv2/core/eigen.hpp>
#include <loguru.hpp>
@@ -235,6 +239,9 @@ void Camera::activate(ftl::data::FrameID id) {
touch_ = false;
movable_ = false;
vr_ = false;
+ cursor_pos_.setZero();
+ cursor_normal_.setZero();
+ cursor_normal_[2] = 1.0f;
//std::mutex m;
//std::condition_variable cv;
@@ -331,6 +338,10 @@ std::string Camera::getActiveSourceURI() {
return "";
}
+void Camera::toggleOverlay() {
+ overlay_->set("enabled", !overlay_->value<bool>("enabled", false));
+}
+
ftl::audio::StereoMixerF<100> *Camera::mixer() {
if (mixer_) return mixer_;
if (movable_) {
@@ -416,10 +427,18 @@ bool Camera::hasFrame() {
return false;
}
+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;
+ ident.setIdentity();
+ return ident;
+}
+
void Camera::drawOverlay(NVGcontext *ctx, const nanogui::Vector2f &s, const nanogui::Vector2f &is, const Eigen::Vector2f &offset) {
auto ptr = std::atomic_load(&latest_);
// TODO: Need all the source framesets here or all data dumped in by renderer
- overlay_->draw(ctx, *ptr, ptr->frames[frame_idx].cast<ftl::rgbd::Frame>(), s, is, offset); // , view->size().cast<float>()
+ overlay_->draw(ctx, *ptr, ptr->frames[frame_idx].cast<ftl::rgbd::Frame>(), s, is, offset, cursor()); // , view->size().cast<float>()
}
void Camera::sendPose(const Eigen::Matrix4d &pose) {
@@ -461,7 +480,7 @@ void Camera::touch(int id, ftl::codecs::TouchType t, int x, int y, float d, int
}
float Camera::depthAt(int x, int y) {
- if (value("show_depth", true)) {
+ //if (value("show_depth", true)) {
auto ptr = std::atomic_load(&latest_);
if (ptr) {
@@ -474,6 +493,152 @@ float Camera::depthAt(int x, int y) {
}
}
}
- }
+ //}
return 0.0f;
}
+
+static float3 getWorldPoint(const cv::Mat &depth, int x, int y, const ftl::rgbd::Camera &intrins, const Eigen::Matrix4f &pose) {
+ if (x >= 0 && y >= 0 && x < depth.cols && y < depth.rows) {
+ float d = depth.at<float>(y, x);
+
+ if (d > intrins.minDepth && d < intrins.maxDepth) {
+ float3 cam = intrins.screenToCam(x, y, d);
+ float3 world = MatrixConversion::toCUDA(pose) * cam;
+ return world;
+ }
+ }
+ return make_float3(0.0f,0.0f,0.0f);
+}
+
+
+Eigen::Vector3f Camera::worldAt(int x, int y) {
+ auto ptr = std::atomic_load(&latest_);
+
+ Eigen::Vector3f res;
+ res.setZero();
+
+ if (ptr) {
+ const auto &frame = ptr->frames[frame_idx].cast<ftl::rgbd::Frame>();
+
+ if (frame.hasChannel(Channel::Depth)) {
+ const auto &depth = frame.get<cv::Mat>(Channel::Depth);
+ const auto &intrins = frame.getLeft();
+ Eigen::Matrix4f posef = frame.getPose().cast<float>();
+
+ float3 CC = getWorldPoint(depth, x, y, intrins, posef);
+ res[0] = CC.x;
+ res[1] = CC.y;
+ res[2] = CC.z;
+ }
+ }
+
+ return res;
+}
+
+void Camera::setCursor(int x, int y) {
+ auto ptr = std::atomic_load(&latest_);
+
+ cursor_pos_.setZero();
+
+ if (ptr) {
+ const auto &frame = ptr->frames[frame_idx].cast<ftl::rgbd::Frame>();
+
+ if (frame.hasChannel(Channel::Depth)) {
+ const auto &depth = frame.get<cv::Mat>(Channel::Depth);
+ const auto &intrins = frame.getLeft();
+ Eigen::Matrix4f posef = frame.getPose().cast<float>();
+
+
+ float3 CC = getWorldPoint(depth, x, y, intrins, posef);
+ cursor_pos_[0] = CC.x;
+ cursor_pos_[1] = CC.y;
+ cursor_pos_[2] = CC.z;
+
+ // Now find normal
+ float3 PC = getWorldPoint(depth, x, y+4, intrins, posef);
+ float3 CP = getWorldPoint(depth, x+4, y, intrins, posef);
+ float3 MC = getWorldPoint(depth, x, y-4, intrins, posef);
+ float3 CM = getWorldPoint(depth, x-4, y, intrins, posef);
+ const float3 n = cross(PC-MC, CP-CM);
+ const float l = length(n);
+
+ if (l > 0.0f) {
+ cursor_normal_[0] = n.x / -l;
+ cursor_normal_[1] = n.y / -l;
+ cursor_normal_[2] = n.z / -l;
+ }
+
+ cursor_target_[0] = CP.x;
+ cursor_target_[1] = CP.y;
+ cursor_target_[2] = CP.z;
+ }
+ }
+}
+
+void Camera::setOriginToCursor() {
+ using ftl::calibration::transform::inverse;
+
+ // Check for valid cursor
+ 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 (movable_) {
+ auto *rend = io->feed()->getRenderer(frame_id_);
+ if (rend) {
+ auto *filter = rend->filter();
+ if (filter) {
+ cv::Mat cur;
+ cv::eigen2cv(cursor(), cur);
+ auto fss = filter->getLatestFrameSets();
+ for (auto &fs : fss) {
+ if (fs->frameset() == frame_id_.frameset()) continue;
+
+ for (auto &f : fs->frames) {
+ auto response = f.response();
+ auto &rgbdf = response.cast<ftl::rgbd::Frame>();
+ auto &calib = rgbdf.setCalibration();
+ calib = f.cast<ftl::rgbd::Frame>().getCalibration();
+ // apply correction to existing one
+ calib.origin = cur*calib.origin;
+ }
+ };
+ }
+ }
+ }
+
+ 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);
+}
+
+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);
+
+ if (movable_) {
+ auto *rend = io->feed()->getRenderer(frame_id_);
+ if (rend) {
+ auto *filter = rend->filter();
+ if (filter) {
+ cv::Mat cur;
+ cv::eigen2cv(cursor(), cur);
+ auto fss = filter->getLatestFrameSets();
+ for (auto &fs : fss) {
+ if (fs->frameset() == frame_id_.frameset()) continue;
+
+ for (auto &f : fs->frames) {
+ auto response = f.response();
+ auto &rgbdf = response.cast<ftl::rgbd::Frame>();
+ auto &calib = rgbdf.setCalibration();
+ calib = f.cast<ftl::rgbd::Frame>().getCalibration();
+ calib.origin = cur;
+ }
+ };
+ }
+ }
+ }
+}
diff --git a/applications/gui2/src/modules/camera.hpp b/applications/gui2/src/modules/camera.hpp
index c22675f7e..d314f0a7a 100644
--- a/applications/gui2/src/modules/camera.hpp
+++ b/applications/gui2/src/modules/camera.hpp
@@ -24,6 +24,8 @@ public:
void setPaused(bool set) { paused_ = set; };
bool isPaused() { return paused_; }
+ void toggleOverlay();
+
float volume();
void setVolume(float v);
@@ -60,6 +62,7 @@ public:
std::string getActiveSourceURI();
float depthAt(int x, int y);
+ Eigen::Vector3f worldAt(int x, int y);
bool isRecording();
void stopRecording();
@@ -68,6 +71,18 @@ public:
void snapshot(const std::string &filename);
+ 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 setCursor(int x, int y);
+
+ const Eigen::Vector3f getCursorPosition() const { return cursor_pos_; }
+
+ void setOriginToCursor();
+ void resetOrigin();
+
private:
int frame_idx = -1;
ftl::data::FrameID frame_id_;
@@ -84,6 +99,9 @@ private:
std::atomic_int16_t nframes_=0;
std::atomic_int64_t latency_=0;
int update_fps_freq_=30; // fps counter update frequency (frames)
+ Eigen::Vector3f cursor_pos_;
+ Eigen::Vector3f cursor_target_;
+ Eigen::Vector3f cursor_normal_;
ftl::data::FrameSetPtr current_fs_;
ftl::data::FrameSetPtr latest_;
diff --git a/applications/gui2/src/modules/camera_tools.hpp b/applications/gui2/src/modules/camera_tools.hpp
new file mode 100644
index 000000000..8e665b842
--- /dev/null
+++ b/applications/gui2/src/modules/camera_tools.hpp
@@ -0,0 +1,40 @@
+#ifndef _FTL_GUI_CAMERA_TOOLS_HPP_
+#define _FTL_GUI_CAMERA_TOOLS_HPP_
+
+namespace ftl {
+namespace gui2 {
+
+enum class Tools {
+ NONE,
+ SELECT_POINT, // Touch 2D
+ MOVEMENT, // 3D first person camera controls
+ PAN, // 2D Panning
+ CENTRE_VIEW,
+ ZOOM_FIT,
+ ZOOM_IN,
+ ZOOM_OUT,
+ CLIPPING,
+ OVERLAY,
+ LAYOUT,
+ MOVE_CURSOR, // Move 3D Cursor
+ ROTATE_CURSOR,
+ ORIGIN_TO_CURSOR,
+ RESET_ORIGIN,
+ SAVE_CURSOR,
+ ROTATE_X,
+ ROTATE_Y,
+ ROTATE_Z,
+ INSPECT_POINT
+};
+
+enum class ToolGroup {
+ MOUSE_MOTION,
+ VIEW_2D_ACTIONS,
+ VIEW_3D_LAYERS,
+ VIEW_3D_ACTIONS
+};
+
+}
+}
+
+#endif
\ No newline at end of file
diff --git a/applications/gui2/src/modules/themes.cpp b/applications/gui2/src/modules/themes.cpp
index 67f7f7760..8b0d0ef06 100644
--- a/applications/gui2/src/modules/themes.cpp
+++ b/applications/gui2/src/modules/themes.cpp
@@ -94,6 +94,24 @@ void Themes::init() {
mediatheme->mButtonFontSize = 30;
mediatheme->mStandardFontSize = 20;
+ auto* mediatheme2 = screen->getTheme("media_small");
+ mediatheme2->mIconScale = 1.2f;
+ mediatheme2->mWindowDropShadowSize = 0;
+ mediatheme2->mWindowFillFocused = nanogui::Color(45, 150);
+ mediatheme2->mWindowFillUnfocused = nanogui::Color(45, 80);
+ mediatheme2->mButtonGradientTopUnfocused = nanogui::Color(0,0);
+ mediatheme2->mButtonGradientBotUnfocused = nanogui::Color(0,0);
+ mediatheme2->mButtonGradientTopFocused = nanogui::Color(80,230);
+ mediatheme2->mButtonGradientBotFocused = nanogui::Color(80,230);
+ mediatheme2->mIconColor = nanogui::Color(255,255);
+ mediatheme2->mTextColor = nanogui::Color(1.0f,1.0f,1.0f,1.0f);
+ mediatheme2->mBorderDark = nanogui::Color(0,0);
+ mediatheme2->mBorderMedium = nanogui::Color(0,0);
+ mediatheme2->mBorderLight = nanogui::Color(0,0);
+ mediatheme2->mDropShadow = nanogui::Color(0,0);
+ mediatheme2->mButtonFontSize = 16;
+ mediatheme2->mStandardFontSize = 14;
+
// https://flatuicolors.com/palette/defo
screen->setColor("highlight1", nanogui::Color(231, 76, 60, 255)); // red
screen->setColor("highlight2", nanogui::Color(52, 152, 219, 255)); // blue
diff --git a/applications/gui2/src/views/camera.cpp b/applications/gui2/src/views/camera.cpp
index b70f82da0..451a032ee 100644
--- a/applications/gui2/src/views/camera.cpp
+++ b/applications/gui2/src/views/camera.cpp
@@ -19,9 +19,12 @@
using ftl::gui2::Camera;
using ftl::gui2::FixedWindow;
using ftl::gui2::MediaPanel;
+using ftl::gui2::ToolPanel;
using ftl::gui2::CameraView;
using ftl::gui2::PopupButton;
using ftl::gui2::VolumeButton;
+using ftl::gui2::Tools;
+using ftl::gui2::ToolGroup;
using ftl::codecs::Channel;
@@ -368,6 +371,237 @@ nanogui::Button* MediaPanel::addButton(int pos) {
return button;
}
+// === ToolPanel ===============================================================
+
+ToolPanel::ToolPanel(nanogui::Widget *parent, ftl::gui2::Camera* ctrl, CameraView* view) :
+ ftl::gui2::FixedWindow(parent, ""), ctrl_(ctrl), view_(view) {
+
+ LOG(INFO) << __func__ << " (" << this << ")";
+ using namespace nanogui;
+
+ setLayout(new BoxLayout(Orientation::Vertical,
+ Alignment::Middle, 5, 10));
+
+ container_ = new Widget(this);
+ container_->setLayout(new BoxLayout(Orientation::Vertical,
+ Alignment::Middle, 0, 10));
+
+ auto theme = dynamic_cast<ftl::gui2::Screen*>(screen())->getTheme("media_small");
+ this->setTheme(theme);
+
+ auto *mouse_group = _addGroup(ToolGroup::MOUSE_MOTION, Button::Flags::RadioButton, {
+ Tools::SELECT_POINT,
+ Tools::MOVEMENT,
+ Tools::MOVE_CURSOR,
+ Tools::ROTATE_CURSOR,
+ Tools::PAN,
+ Tools::INSPECT_POINT,
+ Tools::ZOOM_IN,
+ Tools::ZOOM_OUT
+ });
+ _addButton(mouse_group, Tools::SELECT_POINT, ENTYPO_ICON_MOUSE_POINTER, "Select Point");
+ _addButton(mouse_group, Tools::MOVEMENT, ENTYPO_ICON_MAN, "First Person Camera");
+ _addButton(mouse_group, Tools::MOVE_CURSOR, ENTYPO_ICON_DIRECTION, "Move 3D Cursor");
+ _addButton(mouse_group, Tools::PAN, ENTYPO_ICON_MOUSE, "Pan Image");
+ _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 *view2d_group = _addGroup(ToolGroup::VIEW_2D_ACTIONS, Button::Flags::NormalButton, {
+ Tools::CENTRE_VIEW,
+ Tools::ZOOM_FIT
+ });
+ _addButton(view2d_group, Tools::CENTRE_VIEW, ENTYPO_ICON_ALIGN_HORIZONTAL_MIDDLE, "Centre the View (c)");
+ _addButton(view2d_group, Tools::ZOOM_FIT, ENTYPO_ICON_RESIZE_FULL_SCREEN, "Zoom to Fit (f)");
+
+
+ //_addButton(CameraTools::ORIGIN_TO_CURSOR, ENTYPO_ICON_LOCATION, "Origin to 3D Cursor");
+ auto *action3d_group = _addGroup(ToolGroup::VIEW_3D_ACTIONS, Button::Flags::NormalButton, {
+ Tools::ORIGIN_TO_CURSOR,
+ Tools::RESET_ORIGIN,
+ Tools::SAVE_CURSOR
+ });
+ auto *cur_but = _addButton(action3d_group, {
+ Tools::ORIGIN_TO_CURSOR,
+ Tools::RESET_ORIGIN,
+ Tools::SAVE_CURSOR
+ }, ENTYPO_ICON_LOCATION, "Use Cursor");
+ _addButton(cur_but, Tools::ORIGIN_TO_CURSOR, "Origin to Cursor");
+ _addButton(cur_but, Tools::RESET_ORIGIN, "Reset Origin");
+ _addButton(cur_but, Tools::SAVE_CURSOR, "Save Cursor as Pose");
+
+ auto *view3d_group = _addGroup(ToolGroup::VIEW_3D_LAYERS, Button::Flags::ToggleButton, {
+ Tools::OVERLAY,
+ Tools::CLIPPING
+ });
+ _addButton(view3d_group, Tools::OVERLAY, ENTYPO_ICON_LINE_GRAPH, "Show/Hide Overlay");
+ _addButton(view3d_group, Tools::CLIPPING, ENTYPO_ICON_SCISSORS, "Enable/Disable Clipping");
+
+ auto *b = new Button(this, "", ENTYPO_ICON_CHEVRON_THIN_UP);
+ b->setTooltip("Show/Hide Tools");
+ b->setCallback([this, b]() {
+ if (container_->visible()) {
+ container_->setVisible(false);
+ b->setIcon(ENTYPO_ICON_CHEVRON_THIN_UP);
+ screen()->performLayout();
+ } else {
+ container_->setVisible(true);
+ b->setIcon(ENTYPO_ICON_CHEVRON_THIN_DOWN);
+ screen()->performLayout();
+ }
+ });
+ container_->setVisible(false);
+}
+
+ToolPanel::~ToolPanel() {
+
+}
+
+bool ToolPanel::isActive(ftl::gui2::Tools tool) {
+ if (group_map_.count(tool)) {
+ auto &grp = group_data_[group_map_[tool]];
+ return grp.active.count(tool) > 0;
+ }
+ return false;
+}
+
+void ToolPanel::setTool(ftl::gui2::Tools tool) {
+ if (group_map_.count(tool)) {
+ auto &grp = group_data_[group_map_[tool]];
+
+ if (grp.type == nanogui::Button::Flags::RadioButton) {
+ for (auto t : grp.active) {
+ if (t != tool) {
+ if (buttons_.count(t)) {
+ auto *b = buttons_[t];
+ b->setTextColor(nanogui::Color(255,255,255,255));
+ b->setPushed(false);
+ }
+ }
+ }
+
+ grp.active.clear();
+ grp.active.insert(tool);
+
+ if (buttons_.count(tool)) {
+ auto *b = buttons_[tool];
+ b->setTextColor(dynamic_cast<Screen*>(screen())->getColor("highlight1"));
+ b->setPushed(true);
+ }
+ } else if (grp.type == nanogui::Button::Flags::ToggleButton) {
+ grp.active.insert(tool);
+
+ if (buttons_.count(tool)) {
+ auto *b = buttons_[tool];
+ b->setTextColor(dynamic_cast<Screen*>(screen())->getColor("highlight1"));
+ b->setPushed(true);
+ }
+ } else {
+
+ }
+
+ for (auto &f : callbacks_) {
+ if (f(tool)) break;
+ }
+ }
+}
+
+nanogui::Widget *ToolPanel::_addGroup(ftl::gui2::ToolGroup group, nanogui::Button::Flags type, const std::unordered_set<ftl::gui2::Tools> &tools) {
+ auto &grp = group_data_[group];
+ grp.tools = tools;
+ grp.type = type;
+ for (auto t : tools) group_map_[t] = group;
+
+ auto *w = new nanogui::Widget(container_);
+ w->setLayout(new nanogui::BoxLayout(nanogui::Orientation::Vertical, nanogui::Alignment::Middle, 0, 10));
+ return w;
+}
+
+void ToolPanel::_addButton(nanogui::Widget *g, ftl::gui2::Tools tool, int icon, const std::string &tooltip) {
+ auto *b = new nanogui::Button(g, "", icon);
+ b->setTooltip(tooltip);
+ b->setCallback([this, tool]() {
+ setTool(tool);
+ });
+ buttons_[tool] = b;
+}
+
+void ToolPanel::_addButton(ftl::gui2::PopupButton *parent, ftl::gui2::Tools tool, const std::string &label) {
+ auto *b = new nanogui::Button(parent->popup(), label);
+ b->setCallback([this, parent, tool]() {
+ parent->setPushed(false);
+ setTool(tool);
+ });
+ //buttons_[tool] = b;
+}
+
+ftl::gui2::PopupButton *ToolPanel::_addButton(nanogui::Widget *g, std::unordered_set<ftl::gui2::Tools> tools, int icon, const std::string &tooltip) {
+ auto *b = new ftl::gui2::PopupButton(g, "", icon);
+ b->setTooltip(tooltip);
+ b->setSide(nanogui::Popup::Side::Left);
+ b->setChevronIcon(0);
+
+ for (auto t : tools) {
+ buttons_[t] = b;
+ }
+
+ auto *popup = b->popup();
+ popup->setLayout(new nanogui::BoxLayout
+ (nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 10, 6));
+
+ auto theme = dynamic_cast<ftl::gui2::Screen*>(screen())->getTheme("media_small");
+ popup->setTheme(theme);
+
+ return b;
+}
+
+void ToolPanel::setAvailable(const std::unordered_set<ftl::gui2::Tools> &s) {
+ for (auto &b : buttons_) {
+ if (s.count(b.first)) {
+ b.second->setVisible(true);
+ } else {
+ b.second->setVisible(false);
+ }
+ }
+}
+
+void ToolPanel::setEnabled(const std::unordered_set<ftl::gui2::Tools> &s) {
+ for (auto &b : buttons_) {
+ if (s.count(b.first)) {
+ b.second->setVisible(true);
+ b.second->setEnabled(true);
+ } else {
+ b.second->setEnabled(false);
+ }
+ }
+}
+
+void ToolPanel::enable(const std::unordered_set<ftl::gui2::Tools> &s) {
+ for (auto &b : buttons_) {
+ if (s.count(b.first)) {
+ b.second->setVisible(true);
+ b.second->setEnabled(true);
+ }
+ }
+}
+
+void ToolPanel::disable(const std::unordered_set<ftl::gui2::Tools> &s) {
+ for (auto &b : buttons_) {
+ if (s.count(b.first)) {
+ b.second->setEnabled(false);
+ }
+ }
+}
+
+void ToolPanel::draw(NVGcontext *ctx) {
+ auto size = this->size();
+ setPosition(
+ nanogui::Vector2i( screen()->width() - 30 - size[0],
+ screen()->height() - 30 - size[1]));
+
+ FixedWindow::draw(ctx);
+}
+
// ==== CameraView =============================================================
CameraView::CameraView(ftl::gui2::Screen* parent, ftl::gui2::Camera* ctrl) :
@@ -376,6 +610,7 @@ CameraView::CameraView(ftl::gui2::Screen* parent, ftl::gui2::Camera* ctrl) :
imview_ = new ftl::gui2::FTLImageView(this);
panel_ = new ftl::gui2::MediaPanel(screen(), ctrl, this);
+ tools_ = new ftl::gui2::ToolPanel(screen(), ctrl, this);
imview_->setFlipped(ctrl->isVR());
@@ -410,6 +645,28 @@ CameraView::CameraView(ftl::gui2::Screen* parent, ftl::gui2::Camera* ctrl) :
context_menu_->setVisible(false);
ctrl_->screen->getModule<ftl::gui2::ConfigCtrl>()->show(ctrl_->getID());
});
+
+ tools_->setAvailable({
+ Tools::SELECT_POINT,
+ Tools::OVERLAY,
+ Tools::PAN,
+ Tools::ZOOM_FIT,
+ Tools::ZOOM_IN,
+ Tools::ZOOM_OUT,
+ Tools::CENTRE_VIEW,
+ Tools::INSPECT_POINT
+ });
+
+ tools_->addCallback([this](ftl::gui2::Tools tool) {
+ switch (tool) {
+ case Tools::OVERLAY : ctrl_->toggleOverlay(); return true;
+ case Tools::ZOOM_FIT : imview_->fit(); return true;
+ case Tools::CENTRE_VIEW : imview_->center(); return true;
+ //case CameraTools::ZOOM_OUT : imview_->zoom(-1, imview_->sizeF() / 2); return true;
+ //case CameraTools::ZOOM_IN : imview_->zoom(1, imview_->sizeF() / 2); return true;
+ default: return false;
+ }
+ });
}
CameraView::~CameraView() {
@@ -417,6 +674,7 @@ CameraView::~CameraView() {
// segfault without this check; nanogui already deleted windows?
// should be fixed in nanogui
panel_->dispose();
+ tools_->dispose();
}
if (context_menu_->parent()->getRefCount() > 0) {
@@ -473,11 +731,14 @@ void CameraView::setPan(bool v) {
bool CameraView::mouseMotionEvent(const Eigen::Vector2i &p, const Eigen::Vector2i &rel, int button, int modifiers) {
//if (button == 1) {
- auto pos = imview_->imageCoordinateAt((p - mPos + rel).cast<float>());
- if (pos.x() >= 0.0f && pos.y() >= 0.0f) {
- ctrl_->touch(0, ftl::codecs::TouchType::MOUSE_LEFT, pos.x(), pos.y(), 0.0f, (button > 0) ? 255 : 0);
- //LOG(INFO) << "Depth at " << pos.x() << "," << pos.y() << " = " << ctrl_->depthAt(pos.x(), pos.y());
+ if (tools_->isActive(Tools::SELECT_POINT)) {
+ auto pos = imview_->imageCoordinateAt((p - mPos + rel).cast<float>());
+ if (pos.x() >= 0.0f && pos.y() >= 0.0f) {
+ ctrl_->touch(0, ftl::codecs::TouchType::MOUSE_LEFT, pos.x(), pos.y(), 0.0f, (button > 0) ? 255 : 0);
+
+ //LOG(INFO) << "Depth at " << pos.x() << "," << pos.y() << " = " << ctrl_->depthAt(pos.x(), pos.y());
+ }
}
return true;
//}
@@ -487,10 +748,17 @@ bool CameraView::mouseMotionEvent(const Eigen::Vector2i &p, const Eigen::Vector2
bool CameraView::mouseButtonEvent(const Eigen::Vector2i &p, int button, bool down, int modifiers) {
//LOG(INFO) << "mouseButtonEvent: " << p << " - " << button;
if (button == 0) {
- auto pos = imview_->imageCoordinateAt((p - mPos).cast<float>());
- if (pos.x() >= 0.0f && pos.y() >= 0.0f) {
- ctrl_->touch(0, ftl::codecs::TouchType::MOUSE_LEFT, pos.x(), pos.y(), 0.0f, (down) ? 255 : 0);
+ if (tools_->isActive(Tools::SELECT_POINT)) {
+ auto pos = imview_->imageCoordinateAt((p - mPos).cast<float>());
+ if (pos.x() >= 0.0f && pos.y() >= 0.0f) {
+ ctrl_->touch(0, ftl::codecs::TouchType::MOUSE_LEFT, pos.x(), pos.y(), 0.0f, (down) ? 255 : 0);
+ }
+ } else if (tools_->isActive(Tools::ZOOM_IN)) {
+ imview_->zoom(1, p.cast<float>());
+ } else if (tools_->isActive(Tools::ZOOM_OUT)) {
+ imview_->zoom(-1, p.cast<float>());
}
+
context_menu_->setVisible(false);
return true;
} else if (button == 1) {
@@ -525,12 +793,14 @@ void CameraView::draw(NVGcontext*ctx) {
auto osize = imview_->scaledImageSizeF();
ctrl_->drawOverlay(ctx, screen()->size().cast<float>(), osize, imview_->offset());
- auto mouse = screen()->mousePos();
- auto pos = imview_->imageCoordinateAt((mouse - mPos).cast<float>());
- float d = ctrl_->depthAt(pos.x(), pos.y());
+ if (tools_->isActive(Tools::INSPECT_POINT)) {
+ auto mouse = screen()->mousePos();
+ auto pos = imview_->imageCoordinateAt((mouse - mPos).cast<float>());
+ float d = ctrl_->depthAt(pos.x(), pos.y());
- if (d > 0.0f) {
- nvgText(ctx, mouse.x()+25.0f, mouse.y()+20.0f, (to_string_with_precision(d,2) + std::string("m")).c_str(), nullptr);
+ if (d > 0.0f) {
+ nvgText(ctx, mouse.x()+25.0f, mouse.y()+20.0f, (to_string_with_precision(d,2) + std::string("m")).c_str(), nullptr);
+ }
}
}
diff --git a/applications/gui2/src/views/camera.hpp b/applications/gui2/src/views/camera.hpp
index 3c1c27a81..179f4eff2 100644
--- a/applications/gui2/src/views/camera.hpp
+++ b/applications/gui2/src/views/camera.hpp
@@ -7,12 +7,59 @@
#include "../widgets/window.hpp"
#include "../widgets/soundctrl.hpp"
#include "../widgets/imageview.hpp"
+#include "../widgets/popupbutton.hpp"
+#include "../modules/camera_tools.hpp"
namespace ftl {
namespace gui2 {
class Camera;
class MediaPanel;
+class CameraView;
+
+
+struct ToolGroupData {
+ nanogui::Button::Flags type;
+ std::unordered_set<ftl::gui2::Tools> active;
+ std::unordered_set<ftl::gui2::Tools> tools;
+};
+
+class ToolPanel : public FixedWindow {
+public:
+ ToolPanel(nanogui::Widget *parent, Camera* ctrl, CameraView* view);
+ virtual ~ToolPanel();
+
+ void setAvailable(const std::unordered_set<ftl::gui2::Tools> &);
+ void setEnabled(const std::unordered_set<ftl::gui2::Tools> &);
+ void enable(const std::unordered_set<ftl::gui2::Tools> &);
+ void disable(const std::unordered_set<ftl::gui2::Tools> &);
+
+ void draw(NVGcontext *ctx) override;
+
+ //inline ftl::gui2::Tools activeTool() const { return active_; }
+ bool isActive(ftl::gui2::Tools);
+ void setTool(ftl::gui2::Tools tool);
+
+ inline void addCallback(const std::function<bool(ftl::gui2::Tools)> &cb) { callbacks_.push_back(cb); }
+
+private:
+ Camera* ctrl_;
+ CameraView* view_;
+ nanogui::Widget *container_;
+ std::unordered_map<ftl::gui2::Tools, nanogui::Button*> buttons_;
+ std::unordered_map<ftl::gui2::ToolGroup, ftl::gui2::ToolGroupData> group_data_;
+ std::unordered_map<ftl::gui2::Tools, ftl::gui2::ToolGroup> group_map_;
+
+ std::list<std::function<bool(ftl::gui2::Tools)>> callbacks_;
+
+ nanogui::Widget *_addGroup(ftl::gui2::ToolGroup group, nanogui::Button::Flags type, const std::unordered_set<ftl::gui2::Tools> &tools);
+ void _addButton(nanogui::Widget *, ftl::gui2::Tools, int icon, const std::string &tooltip);
+ void _addButton(ftl::gui2::PopupButton *parent, ftl::gui2::Tools, const std::string &label);
+ ftl::gui2::PopupButton *_addButton(nanogui::Widget *, std::unordered_set<ftl::gui2::Tools> tools, int icon, const std::string &tooltip);
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
class CameraView : public View {
public:
@@ -35,6 +82,7 @@ protected:
bool enable_pan_;
Camera* ctrl_;
MediaPanel* panel_;
+ ToolPanel* tools_;
FTLImageView* imview_;
nanogui::Window *context_menu_;
diff --git a/applications/gui2/src/views/camera3d.cpp b/applications/gui2/src/views/camera3d.cpp
index 9d1c0fea2..29bb0ae4d 100644
--- a/applications/gui2/src/views/camera3d.cpp
+++ b/applications/gui2/src/views/camera3d.cpp
@@ -35,8 +35,35 @@ CameraView3D::CameraView3D(ftl::gui2::Screen *parent, ftl::gui2::Camera *ctrl) :
pose_up_to_date_.test_and_set();
+ tools_->setAvailable({
+ Tools::SELECT_POINT,
+ Tools::MOVEMENT,
+ Tools::OVERLAY,
+ Tools::INSPECT_POINT,
+ Tools::CLIPPING,
+ Tools::MOVE_CURSOR,
+ Tools::ORIGIN_TO_CURSOR,
+ Tools::RESET_ORIGIN,
+ Tools::SAVE_CURSOR
+ });
+
setZoom(false);
setPan(false);
+
+ tools_->setTool(Tools::MOVEMENT);
+
+ tools_->addCallback([this](ftl::gui2::Tools tool) {
+ if (tool == Tools::ORIGIN_TO_CURSOR) {
+ ctrl_->setOriginToCursor();
+ tools_->setTool(Tools::MOVEMENT);
+ return true;
+ } else if (tool == Tools::RESET_ORIGIN) {
+ ctrl_->resetOrigin();
+ tools_->setTool(Tools::MOVEMENT);
+ return true;
+ }
+ return false;
+ });
}
bool CameraView3D::keyboardEvent(int key, int scancode, int action, int modifiers) {
@@ -65,21 +92,45 @@ bool CameraView3D::keyboardEvent(int key, int scancode, int action, int modifier
}
bool CameraView3D::mouseButtonEvent(const Eigen::Vector2i &p, int button, bool down, int modifiers) {
+ if (button == 0 && !down) {
+ if (tools_->isActive(Tools::MOVE_CURSOR)) {
+ auto mouse = screen()->mousePos();
+ auto pos = imview_->imageCoordinateAt((mouse - mPos).cast<float>());
+ //Eigen::Vector3f world = ctrl_->worldAt(pos.x(), pos.y());
+
+ ctrl_->setCursor(pos.x(), pos.y());
+ tools_->setTool(Tools::ROTATE_CURSOR);
+ return true;
+ } else if (tools_->isActive(Tools::ROTATE_CURSOR)) {
+ tools_->setTool(Tools::MOVEMENT);
+ }
+ }
+
return CameraView::mouseButtonEvent(p, button, down, modifiers);
}
bool CameraView3D::mouseMotionEvent(const Eigen::Vector2i &p, const Eigen::Vector2i &rel, int button, int modifiers) {
- if (button != 1) {
+ //if (button != 1) {
+ // return true;
+ //}
+
+ if (button == 1 && tools_->isActive(Tools::MOVEMENT)) {
+ rx_ += rel[0];
+ ry_ += rel[1];
+ pose_up_to_date_.clear();
return true;
- }
+ } else if (tools_->isActive(Tools::ROTATE_CURSOR)) {
+ auto mouse = screen()->mousePos();
+ auto pos = imview_->imageCoordinateAt((mouse - mPos).cast<float>());
- rx_ += rel[0];
- ry_ += rel[1];
- pose_up_to_date_.clear();
+ Eigen::Vector3f world = ctrl_->worldAt(pos.x(), pos.y());
+ ctrl_->setCursorTarget(world);
+ return true;
+ }
//LOG(INFO) << "New pose: \n" << getUpdatedPose();
//ctrl_->sendPose(getUpdatedPose());
- return true;
+ return false;
}
bool CameraView3D::scrollEvent(const Eigen::Vector2i &p, const Eigen::Vector2f &rel) {
diff --git a/components/calibration/CMakeLists.txt b/components/calibration/CMakeLists.txt
index 68d658842..1f619a6e0 100644
--- a/components/calibration/CMakeLists.txt
+++ b/components/calibration/CMakeLists.txt
@@ -4,6 +4,7 @@ set(CALIBSRC
src/structures.cpp
src/visibility.cpp
src/object.cpp
+ src/stereorectify.cpp
)
if (WITH_CERES)
diff --git a/components/calibration/include/ftl/calibration/stereorectify.hpp b/components/calibration/include/ftl/calibration/stereorectify.hpp
new file mode 100644
index 000000000..47586b0ed
--- /dev/null
+++ b/components/calibration/include/ftl/calibration/stereorectify.hpp
@@ -0,0 +1,51 @@
+#pragma once
+
+#include <opencv2/core.hpp>
+#include <ftl/calibration/structures.hpp>
+
+namespace ftl {
+namespace calibration {
+
+/** Stereo rectification parameters. Wrapper for cv::stereoRectify() */
+struct StereoRectify {
+ /** Calculate rectification parameters. c1 and c2 contain valid calibration
+ * (extrinsic parameters for translation from world to camera) */
+ StereoRectify(const CalibrationData::Calibration &c1, const CalibrationData::Calibration& c2, const cv::Size size={0, 0}, double alpha=0.0, int flags=0);
+
+ /** stereo pair baseline (same unit as extrinsic paramters) */
+ double baseline() const;
+
+ /** calculate maps (cv::remap()) for camera 1 */
+ void map1(cv::Mat &m1, cv::Mat &m2, int format=CV_16SC2);
+ /** calculate maps (cv::remap()) for camera 2 */
+ void map2(cv::Mat &m1, cv::Mat &m2, int format=CV_16SC2);
+
+ cv::Size size;
+ /** unrectified params */
+ cv::Mat K1;
+ cv::Mat K2;
+ cv::Mat distCoeffs1;
+ cv::Mat distCoeffs2;
+
+ /** 3x4 projection matrix for first camera */
+ cv::Mat P1;
+ /** 3x4 projection matrix for second camera */
+ cv::Mat P2;
+ /** rotation matrix for first camera (unrectified to rectified) */
+ cv::Mat R1;
+ /** rotation matrix for second camera (unrectified to rectified) */
+ cv::Mat R2;
+ /** disparity to depth matrix */
+ cv::Mat Q;
+ /** rotation from first camera to second camera (unrectified) */
+ cv::Mat R;
+ /** translation from first camera to second camera (unrectified) */
+ cv::Mat t;
+ /** largest ROI containing only valid pixels in rectified image for first camera */
+ cv::Rect roi1;
+ /** largest ROI containing only valid pixels in rectified image for second camera */
+ cv::Rect roi2;
+};
+
+}
+}
\ No newline at end of file
diff --git a/components/calibration/include/ftl/calibration/structures.hpp b/components/calibration/include/ftl/calibration/structures.hpp
index 976614374..fffbe5035 100644
--- a/components/calibration/include/ftl/calibration/structures.hpp
+++ b/components/calibration/include/ftl/calibration/structures.hpp
@@ -115,6 +115,7 @@ struct CalibrationData {
struct Calibration {
Intrinsic intrinsic;
Extrinsic extrinsic;
+
MSGPACK_DEFINE(intrinsic, extrinsic);
};
@@ -129,12 +130,16 @@ struct CalibrationData {
Calibration& get(ftl::codecs::Channel channel);
bool hasCalibration(ftl::codecs::Channel channel) const;
-private:
// TODO: identify cameras with unique ID string instead of channel.
- std::map<ftl::codecs::Channel, Calibration> data_;
+ std::map<ftl::codecs::Channel, Calibration> data;
+
+ /** Correction to be applied (inverse) to extrinsic parameters
+ * (calibrated to new origin); Applied to rectified pose at the moment
+ */
+ cv::Mat origin = cv::Mat::eye(4, 4, CV_64FC1);
public:
- MSGPACK_DEFINE(enabled, data_);
+ MSGPACK_DEFINE(enabled, data, origin);
};
}
diff --git a/components/calibration/src/stereorectify.cpp b/components/calibration/src/stereorectify.cpp
new file mode 100644
index 000000000..7612a3554
--- /dev/null
+++ b/components/calibration/src/stereorectify.cpp
@@ -0,0 +1,43 @@
+#include <ftl/calibration/stereorectify.hpp>
+#include <ftl/calibration/parameters.hpp>
+#include <opencv2/calib3d.hpp>
+
+namespace ftl {
+namespace calibration {
+
+// ==== StereoRectify ==========================================================
+
+StereoRectify::StereoRectify(const CalibrationData::Calibration& c1, const CalibrationData::Calibration& c2, cv::Size sz, double alpha, int flags) {
+ size = sz;
+
+ if (size == cv::Size{0, 0}) {
+ size = c1.intrinsic.resolution;
+ }
+ K1 = c1.intrinsic.matrix(size);
+ K2 = c2.intrinsic.matrix(size);
+ c1.intrinsic.distCoeffs.Mat().copyTo(distCoeffs1);
+ c2.intrinsic.distCoeffs.Mat().copyTo(distCoeffs2);
+
+ cv::Mat T1 = c1.extrinsic.matrix();
+ cv::Mat T2 = c2.extrinsic.matrix();
+ cv::Mat T = T2 * transform::inverse(T1);
+
+ transform::getRotationAndTranslation(T, R, t);
+ cv::stereoRectify( K1, distCoeffs1, K2, distCoeffs2, size, R, t,
+ R1, R2, P1, P2, Q, flags, alpha, size, &roi1, &roi2);
+}
+
+double StereoRectify::baseline() const {
+ return cv::norm(t);
+}
+
+void StereoRectify::map1(cv::Mat &m1, cv::Mat &m2, int format) {
+ cv::initUndistortRectifyMap(K1, distCoeffs1, R1, P1, size, format, m1, m2);
+}
+
+void StereoRectify::map2(cv::Mat &m1, cv::Mat &m2, int format) {
+ cv::initUndistortRectifyMap(K2, distCoeffs2, R2, P2, size, format, m1, m2);
+}
+
+}
+}
\ No newline at end of file
diff --git a/components/calibration/src/structures.cpp b/components/calibration/src/structures.cpp
index 36d3ff4fe..8c2c9ec5f 100644
--- a/components/calibration/src/structures.cpp
+++ b/components/calibration/src/structures.cpp
@@ -187,6 +187,9 @@ CalibrationData CalibrationData::readFile(const std::string &path) {
}
CalibrationData calibration;
fs["enabled"] >> calibration.enabled;
+ if (!fs["origin"].isNone()) {
+ fs["origin"] >> calibration.origin;
+ }
for (auto it = fs["calibration"].begin(); it != fs["calibration"].end(); it++) {
Calibration calib;
@@ -203,7 +206,7 @@ CalibrationData CalibrationData::readFile(const std::string &path) {
(*it)["rvec"] >> calib.extrinsic.rvec;
(*it)["tvec"] >> calib.extrinsic.tvec;
- calibration.data_[channel] = calib;
+ calibration.data[channel] = calib;
}
return calibration;
@@ -216,8 +219,9 @@ void CalibrationData::writeFile(const std::string &path) const {
}
fs << "enabled" << enabled;
+ fs << "origin" << origin;
fs << "calibration" << "[";
- for (auto &[channel, data] : data_) {
+ for (auto &[channel, data] : data) {
fs << "{"
<< "channel" << int(channel)
<< "resolution" << data.intrinsic.resolution
@@ -237,9 +241,9 @@ void CalibrationData::writeFile(const std::string &path) const {
}
CalibrationData::Calibration& CalibrationData::get(ftl::codecs::Channel channel) {
- return data_[channel];
+ return data[channel];
}
bool CalibrationData::hasCalibration(ftl::codecs::Channel channel) const {
- return data_.count(channel) != 0;
+ return data.count(channel) != 0;
}
diff --git a/components/codecs/include/ftl/codecs/channels.hpp b/components/codecs/include/ftl/codecs/channels.hpp
index b5a03877b..90e3d3cec 100644
--- a/components/codecs/include/ftl/codecs/channels.hpp
+++ b/components/codecs/include/ftl/codecs/channels.hpp
@@ -64,7 +64,8 @@ enum struct Channel : int {
Transforms = 2050, // Transformation matrices for framesets
Shapes3D = 2051, // Labeled 3D shapes
Messages = 2052, // Vector of Strings
- Touch = 2053 // List of touch data type (each touch point)
+ Touch = 2053, // List of touch data type (each touch point)
+ Pipelines = 2054, // List of pipline URIs that have been applied
};
inline bool isVideo(Channel c) { return (int)c < 32; };
diff --git a/components/control/cpp/src/master.cpp b/components/control/cpp/src/master.cpp
index d6a2f1cac..9e8b85e4c 100644
--- a/components/control/cpp/src/master.cpp
+++ b/components/control/cpp/src/master.cpp
@@ -57,7 +57,8 @@ Master::Master(Configurable *root, Universe *net)
});
net->bind("get_configurable", [](const std::string &uri) -> std::string {
- return ftl::config::find(uri)->getConfig().dump();
+ auto *cfg = ftl::config::find(uri);
+ return (cfg) ? cfg->getConfig().dump() : "{}";
});
net->bind("list_configurables", []() {
diff --git a/components/operators/src/operator.cpp b/components/operators/src/operator.cpp
index 7eabcdc70..cdbeb0c00 100644
--- a/components/operators/src/operator.cpp
+++ b/components/operators/src/operator.cpp
@@ -8,6 +8,7 @@ using ftl::operators::Graph;
using ftl::rgbd::Frame;
using ftl::rgbd::FrameSet;
using ftl::rgbd::Source;
+using ftl::codecs::Channel;
Operator::Operator(ftl::Configurable *config) : config_(config) {
enabled_ = config_->value("enabled", true);
@@ -64,6 +65,12 @@ bool Graph::apply(FrameSet &in, FrameSet &out, cudaStream_t stream) {
return false;
}
+ for (auto &f : out.frames) {
+ if (!f.hasOwn(Channel::Pipelines)) f.create<std::list<std::string>>(Channel::Pipelines);
+ auto pls = f.set<std::list<std::string>>(Channel::Pipelines);
+ pls = getID();
+ }
+
for (auto &i : operators_) {
if (i.instances.size() < 1) {
i.instances.push_back(i.maker->make());
@@ -147,6 +154,10 @@ bool Graph::apply(Frame &in, Frame &out, cudaStream_t stream) {
return false;
}
+ if (!out.hasOwn(Channel::Pipelines)) out.create<std::list<std::string>>(Channel::Pipelines);
+ auto pls = out.set<std::list<std::string>>(Channel::Pipelines);
+ pls = getID();
+
for (auto &i : operators_) {
// Make sure there are enough instances
if (i.instances.size() < 1) {
diff --git a/components/renderers/cpp/include/ftl/render/overlay.hpp b/components/renderers/cpp/include/ftl/render/overlay.hpp
index 945af363f..4bb01d799 100644
--- a/components/renderers/cpp/include/ftl/render/overlay.hpp
+++ b/components/renderers/cpp/include/ftl/render/overlay.hpp
@@ -26,7 +26,7 @@ class Overlay : public ftl::Configurable {
//void apply(ftl::rgbd::FrameSet &fs, cv::Mat &out, ftl::rgbd::FrameState &state);
- void draw(NVGcontext *, ftl::data::FrameSet &fs, ftl::rgbd::Frame &frame, const Eigen::Vector2f &, const Eigen::Vector2f &, const Eigen::Vector2f &offset);
+ void draw(NVGcontext *, ftl::data::FrameSet &fs, ftl::rgbd::Frame &frame, const Eigen::Vector2f &, const Eigen::Vector2f &, const Eigen::Vector2f &offset, const Eigen::Matrix4d &cursor);
private:
nanogui::GLShader oShader;
diff --git a/components/renderers/cpp/src/overlay.cpp b/components/renderers/cpp/src/overlay.cpp
index 864fbddef..611e9737c 100644
--- a/components/renderers/cpp/src/overlay.cpp
+++ b/components/renderers/cpp/src/overlay.cpp
@@ -244,7 +244,7 @@ void Overlay::_drawAxis(const Eigen::Matrix4d &pose, const Eigen::Vector3f &scal
(const void *)(loffset * sizeof(uint32_t)));
}
-void Overlay::draw(NVGcontext *ctx, ftl::data::FrameSet &fs, ftl::rgbd::Frame &frame, const Eigen::Vector2f &screenSize, const Eigen::Vector2f &imageSize, const Eigen::Vector2f &offset) {
+void Overlay::draw(NVGcontext *ctx, ftl::data::FrameSet &fs, ftl::rgbd::Frame &frame, const Eigen::Vector2f &screenSize, const Eigen::Vector2f &imageSize, const Eigen::Vector2f &offset, const Eigen::Matrix4d &cursor) {
if (!value("enabled", false)) return;
double zfar = 8.0f;
@@ -333,6 +333,11 @@ void Overlay::draw(NVGcontext *ctx, ftl::data::FrameSet &fs, ftl::rgbd::Frame &f
_drawAxis(frame.getPose().inverse(), Eigen::Vector3f(0.5f, 0.5f, 0.5f));
}
+ if (value("show_cursor", true)) {
+ _drawAxis(frame.getPose().inverse() * cursor.inverse(), Eigen::Vector3f(0.2f, 0.2f, 0.2f));
+ _drawOutlinedShape(Shape::XZPLANE, frame.getPose().inverse() * cursor.inverse(), Eigen::Vector3f(0.05f, 0.05f, 0.05f), make_uchar4(200,200,200,50), make_uchar4(255,255,255,100));
+ }
+
if (value("show_shapes", true)) {
/*if (fs.hasChannel(Channel::Shapes3D)) {
std::vector<ftl::codecs::Shape3D> shapes;
diff --git a/components/rgbd-sources/include/ftl/rgbd/camera.hpp b/components/rgbd-sources/include/ftl/rgbd/camera.hpp
index f68de82d0..3698a32ca 100644
--- a/components/rgbd-sources/include/ftl/rgbd/camera.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/camera.hpp
@@ -66,6 +66,8 @@ struct __align__(16) Camera {
*/
__host__ __device__ float3 screenToCam(int ux, int uy, float depth) const;
+ //Eigen::Vector4f eigenScreenToCam(int ux, int uy, float depth) const;
+
/**
* Convert screen plus depth into camera coordinates.
*/
@@ -185,7 +187,7 @@ inline float3 ftl::rgbd::Camera::screenToCam(uint ux, uint uy, float depth) cons
return make_float3(depth*x, depth*y, depth);
}
-__device__
+__device__ __host__
inline float3 ftl::rgbd::Camera::screenToCam(int ux, int uy, float depth) const {
const float x = static_cast<float>(((float)ux+cx) / fx);
const float y = static_cast<float>(((float)uy+cy) / fy);
diff --git a/components/rgbd-sources/include/ftl/rgbd/frame.hpp b/components/rgbd-sources/include/ftl/rgbd/frame.hpp
index 509c4bec8..e3a77ce2f 100644
--- a/components/rgbd-sources/include/ftl/rgbd/frame.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/frame.hpp
@@ -26,6 +26,10 @@
#include <Eigen/Eigen>
namespace ftl {
+namespace calibration {
+struct CalibrationData;
+}
+
namespace rgbd {
//typedef ftl::data::Frame Frame;
@@ -91,6 +95,9 @@ class Frame : public ftl::data::Frame {
ftl::rgbd::Camera &setRight();
Eigen::Matrix4d &setPose();
+ ftl::calibration::CalibrationData& setCalibration();
+ const ftl::calibration::CalibrationData& getCalibration() const;
+
std::string serial() const;
std::string device() const;
diff --git a/components/rgbd-sources/src/camera.cpp b/components/rgbd-sources/src/camera.cpp
index 3876fa1ed..a4f540ffd 100644
--- a/components/rgbd-sources/src/camera.cpp
+++ b/components/rgbd-sources/src/camera.cpp
@@ -65,3 +65,14 @@ Camera Camera::scaled(int width, int height) const {
return newcam;
}
+
+/*Eigen::Vector4f ftl::rgbd::Camera::eigenScreenToCam(int ux, int uy, float depth) const {
+ const float x = static_cast<float>(((float)ux+cx) / fx);
+ const float y = static_cast<float>(((float)uy+cy) / fy);
+ Eigen::Vector4f v;
+ v[0] = depth*x;
+ v[1] = depth*y;
+ v[2] = depth;
+ v[3] = 1.0f;
+ return v;
+}*/
diff --git a/components/rgbd-sources/src/frame.cpp b/components/rgbd-sources/src/frame.cpp
index 10bacd8f0..a4ace9e46 100644
--- a/components/rgbd-sources/src/frame.cpp
+++ b/components/rgbd-sources/src/frame.cpp
@@ -1,5 +1,6 @@
#include <ftl/rgbd/frame.hpp>
+#include <ftl/calibration/structures.hpp>
#define LOGURU_REPLACE_GLOG 1
#include <loguru.hpp>
@@ -138,6 +139,14 @@ Eigen::Matrix4d &ftl::rgbd::Frame::setPose() {
return this->create<Eigen::Matrix4d>(ftl::codecs::Channel::Pose);
}
+const ftl::calibration::CalibrationData& ftl::rgbd::Frame::getCalibration() const {
+ return this->get<ftl::calibration::CalibrationData>(Channel::CalibrationData);
+}
+
+ftl::calibration::CalibrationData& ftl::rgbd::Frame::setCalibration() {
+ return this->create<ftl::calibration::CalibrationData>(Channel::CalibrationData);
+}
+
std::string ftl::rgbd::Frame::serial() const {
if (hasChannel(Channel::MetaData)) {
const auto &meta = get<std::map<std::string,std::string>>(Channel::MetaData);
@@ -199,4 +208,3 @@ cv::cuda::GpuMat &ftl::data::Frame::set<cv::cuda::GpuMat, 0>(ftl::codecs::Channe
return set<ftl::rgbd::VideoFrame>(c).setGPU();
}
-
diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.cpp b/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
index e495ef263..39e91193c 100644
--- a/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
@@ -69,14 +69,6 @@ void StereoRectification::updateCalibration_() {
valid_ = false;
- // create temporary buffers for rectification
- if (tmp_l_.size() != image_resolution_) {
- tmp_l_ = cv::Mat(image_resolution_, CV_8UC4);
- }
- if (tmp_l_.size() != image_resolution_) {
- tmp_r_ = cv::Mat(image_resolution_, CV_8UC4);
- }
-
// calculate rotation and translation from left to right using calibration
cv::Mat T_l = calib_left_.extrinsic.matrix();
cv::Mat T_r = calib_right_.extrinsic.matrix();
@@ -106,7 +98,6 @@ void StereoRectification::calculateParameters_() {
map_format_, map_l_.first, map_l_.second);
cv::initUndistortRectifyMap(K_r, dc_r, R_r_, P_r_, image_resolution_,
map_format_, map_r_.first, map_r_.second);
-
}
void StereoRectification::rectify(cv::InputArray im, cv::OutputArray im_out, Channel c) {
@@ -165,6 +156,7 @@ cv::Mat StereoRectification::getPose(Channel c) {
}
}
else {
+ // unrectified pose not used anywhere (and isn't necessarily valid).
if (c == Channel::Left) {
return inverse(calib_left_.extrinsic.matrix());
}
diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.hpp b/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
index 756747e04..edbb66fc4 100644
--- a/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
@@ -96,9 +96,6 @@ private:
std::pair<cv::Mat,cv::Mat> map_l_;
std::pair<cv::Mat,cv::Mat> map_r_;
- // temporary buffers for left/right
- cv::Mat tmp_l_;
- cv::Mat tmp_r_;
};
}
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index ecd902da1..b572e0e4e 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -25,6 +25,7 @@
#include <ftl/rgbd/capabilities.hpp>
#include <ftl/codecs/shapes.hpp>
#include <ftl/calibration/structures.hpp>
+#include <ftl/calibration/parameters.hpp>
#include "stereovideo.hpp"
#include "ftl/threads.hpp"
@@ -231,13 +232,15 @@ void StereoVideoSource::updateParameters(ftl::rgbd::Frame &frame) {
calibration_ = ftl::calibration::CalibrationData(change);
rectification_->setCalibration(calibration_);
rectification_->setEnabled(change.enabled);
+
+ do_update_params_ = true;
return true;
});
if (lsrc_->isStereo()) {
Eigen::Matrix4d pose;
- cv::cv2eigen(rectification_->getPose(Channel::Left), pose);
-
+ // NOTE: pose update (new origin/rotation)
+ cv::cv2eigen(calibration_.origin * rectification_->getPose(Channel::Left), pose);
frame.setPose() = pose;
cv::Mat K = rectification_->cameraMatrix(color_size_);
@@ -270,14 +273,6 @@ void StereoVideoSource::updateParameters(ftl::rgbd::Frame &frame) {
doff
};
- CHECK(params.fx > 0) << "focal length must be positive";
- CHECK(params.fy > 0) << "focal length must be positive";
- CHECK(params.cx < 0) << "bad principal point coordinate (negative value)";
- CHECK(-params.cx < params.width) << "principal point must be inside image";
- CHECK(params.cy < 0) << "bad principal point coordinate (negative value)";
- CHECK(-params.cy < params.height) << "principal point must be inside image";
- CHECK(params.baseline >= 0.0) << "baseline must be positive";
-
host_->getConfig()["focal"] = params.fx;
host_->getConfig()["centre_x"] = params.cx;
host_->getConfig()["centre_y"] = params.cy;
@@ -313,6 +308,15 @@ void StereoVideoSource::updateParameters(ftl::rgbd::Frame &frame) {
frame.setPose() = pose;
}
+
+ const auto& params = frame.getLeft();
+ CHECK(params.fx > 0) << "focal length must be positive";
+ CHECK(params.fy > 0) << "focal length must be positive";
+ CHECK(params.cx < 0) << "bad principal point coordinate (negative value)";
+ CHECK(-params.cx < params.width) << "principal point must be inside image";
+ CHECK(params.cy < 0) << "bad principal point coordinate (negative value)";
+ CHECK(-params.cy < params.height) << "principal point must be inside image";
+ CHECK(params.baseline >= 0.0) << "baseline must be positive";
}
bool StereoVideoSource::capture(int64_t ts) {
diff --git a/components/rgbd-sources/test/CMakeLists.txt b/components/rgbd-sources/test/CMakeLists.txt
index 4a4b0d2c5..16a83e404 100644
--- a/components/rgbd-sources/test/CMakeLists.txt
+++ b/components/rgbd-sources/test/CMakeLists.txt
@@ -6,7 +6,7 @@ $<TARGET_OBJECTS:CatchTest>
)
target_include_directories(source_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../include")
target_link_libraries(source_unit
- ftlcommon ftlcodecs ftlnet Eigen3::Eigen ftldata ${CUDA_LIBRARIES})
+ ftlcommon ftlcalibration ftlcodecs ftlnet Eigen3::Eigen ftldata ${CUDA_LIBRARIES})
add_test(SourceUnitTest source_unit)
diff --git a/components/streams/include/ftl/streams/feed.hpp b/components/streams/include/ftl/streams/feed.hpp
index 62234701b..ac141a045 100644
--- a/components/streams/include/ftl/streams/feed.hpp
+++ b/components/streams/include/ftl/streams/feed.hpp
@@ -110,6 +110,9 @@ public:
*/
ftl::rgbd::Source *getRGBD(ftl::data::FrameID id);
+ std::list<ftl::Configurable*> getPipelines(ftl::data::FrameID id);
+ std::list<ftl::Configurable*> getOperators(ftl::data::FrameID id, const std::string &name);
+
void remove(const std::string &str);
void remove(uint32_t id);
diff --git a/components/streams/include/ftl/streams/renderer.hpp b/components/streams/include/ftl/streams/renderer.hpp
index 441fd32ce..0ae3e7242 100644
--- a/components/streams/include/ftl/streams/renderer.hpp
+++ b/components/streams/include/ftl/streams/renderer.hpp
@@ -31,6 +31,8 @@ class Source : public ftl::Configurable, public ftl::data::DiscreteSource {
ftl::audio::StereoMixerF<100> &mixer();
+ ftl::stream::Feed::Filter *filter() const;
+
private:
ftl::stream::Feed *feed_;
ftl::render::BaseSourceImpl *impl_;
diff --git a/components/streams/include/ftl/streams/sender.hpp b/components/streams/include/ftl/streams/sender.hpp
index 8edbbf1d6..9e679167a 100644
--- a/components/streams/include/ftl/streams/sender.hpp
+++ b/components/streams/include/ftl/streams/sender.hpp
@@ -68,6 +68,7 @@ class Sender : public ftl::Configurable {
int64_t injection_timestamp_=0;
SHARED_MUTEX mutex_;
std::atomic_flag do_inject_;
+ std::atomic_flag do_reinject_;
//std::function<void(ftl::codecs::Channel, int, int)> state_cb_;
ftl::stream::StreamCallback reqcb_;
int add_iframes_;
@@ -105,7 +106,7 @@ class Sender : public ftl::Configurable {
float _selectFloatMax(ftl::codecs::Channel c);
ftl::audio::Encoder *_getAudioEncoder(int fsid, int sid, ftl::codecs::Channel c, ftl::codecs::Packet &pkt);
- void _sendPersistent(ftl::data::Frame &frame);
+ void _sendPersistent(ftl::data::FrameSet &fs);
void _send(ftl::rgbd::FrameSet &fs, ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt);
bool _checkNeedsIFrame(int64_t ts, bool injecting);
diff --git a/components/streams/src/baserender.hpp b/components/streams/src/baserender.hpp
index 51c6ce09c..55082964f 100644
--- a/components/streams/src/baserender.hpp
+++ b/components/streams/src/baserender.hpp
@@ -30,6 +30,8 @@ class BaseSourceImpl {
inline ftl::audio::StereoMixerF<100> &mixer() { return mixer_; }
+ virtual ftl::stream::Feed::Filter *filter()=0;
+
protected:
ftl::render::Source *host_;
ftl::audio::StereoMixerF<100> mixer_;
diff --git a/components/streams/src/renderer.cpp b/components/streams/src/renderer.cpp
index 439ea2f2e..66d3add2b 100644
--- a/components/streams/src/renderer.cpp
+++ b/components/streams/src/renderer.cpp
@@ -32,6 +32,11 @@ ftl::audio::StereoMixerF<100> &Source::mixer() {
return impl_->mixer();
}
+ftl::stream::Feed::Filter *Source::filter() const {
+ if (!impl_) return nullptr;
+ return impl_->filter();
+}
+
bool Source::supports(const std::string &puri) {
ftl::URI uri(puri);
if (!uri.isValid() || uri.getScheme() != ftl::URI::SCHEME_DEVICE) return false;
diff --git a/components/streams/src/renderers/openvr_render.hpp b/components/streams/src/renderers/openvr_render.hpp
index 3907b9c3e..d07383998 100644
--- a/components/streams/src/renderers/openvr_render.hpp
+++ b/components/streams/src/renderers/openvr_render.hpp
@@ -32,6 +32,8 @@ class OpenVRRender : public ftl::render::BaseSourceImpl {
static bool supported();
+ ftl::stream::Feed::Filter *filter() override { return filter_; }
+
private:
ftl::stream::Feed *feed_;
ftl::stream::Feed::Filter *filter_;
diff --git a/components/streams/src/renderers/screen_render.hpp b/components/streams/src/renderers/screen_render.hpp
index 91a867cf0..fedc65ad4 100644
--- a/components/streams/src/renderers/screen_render.hpp
+++ b/components/streams/src/renderers/screen_render.hpp
@@ -26,6 +26,8 @@ class ScreenRender : public ftl::render::BaseSourceImpl {
bool isReady() override;
+ ftl::stream::Feed::Filter *filter() override { return filter_; }
+
private:
ftl::stream::Feed *feed_;
ftl::stream::Feed::Filter *filter_;
diff --git a/components/streams/src/sender.cpp b/components/streams/src/sender.cpp
index fba9c922a..62e48bd0b 100644
--- a/components/streams/src/sender.cpp
+++ b/components/streams/src/sender.cpp
@@ -25,6 +25,7 @@ using ftl::stream::injectConfig;
Sender::Sender(nlohmann::json &config) : ftl::Configurable(config), stream_(nullptr) {
do_inject_.test_and_set();
+ do_reinject_.test_and_set();
iframe_ = 1;
add_iframes_ = value("iframes", 50);
timestamp_ = -1;
@@ -104,13 +105,20 @@ static void writeValue(std::vector<unsigned char> &data, T value) {
data.insert(data.end(), pvalue_start, pvalue_start+sizeof(T));
}
-void Sender::_sendPersistent(ftl::data::Frame &frame) {
- auto *session = frame.parent();
- if (session) {
- for (auto c : session->channels()) {
- Sender::post(frame, c);
+void Sender::_sendPersistent(ftl::rgbd::FrameSet &fs) {
+ std::unordered_set<ftl::codecs::Channel> persist_chan;
+
+ for (auto &frame : fs.frames) {
+ auto *session = frame.parent();
+ if (session) {
+ auto chans = session->channels();
+ persist_chan.insert(chans.begin(), chans.end());
}
}
+
+ for (auto c : persist_chan) {
+ post(fs, c);
+ }
}
void Sender::fakePost(ftl::data::FrameSet &fs, ftl::codecs::Channel c) {
@@ -174,6 +182,10 @@ void Sender::post(ftl::data::FrameSet &fs, ftl::codecs::Channel c, bool noencode
pkt.packet_count = static_cast<uint8_t>(fs.frames[i].packet_tx+1); // FIXME: 255 limit currently
_send(fs, spkt, pkt);
}
+
+ if (injection_timestamp_ >= spkt.timestamp) {
+ do_reinject_.clear();
+ }
return;
}
@@ -182,6 +194,9 @@ void Sender::post(ftl::data::FrameSet &fs, ftl::codecs::Channel c, bool noencode
bool do_inject = !do_inject_.test_and_set();
bool do_iframe = _checkNeedsIFrame(fs.timestamp(), do_inject);
+ if (!do_reinject_.test_and_set()) {
+ do_inject = true;
+ }
FTL_Profile("SenderPost", 0.02);
@@ -192,6 +207,10 @@ void Sender::post(ftl::data::FrameSet &fs, ftl::codecs::Channel c, bool noencode
//int ccount = 0;
int forward_count = 0;
+ if (do_inject) {
+ _sendPersistent(fs);
+ }
+
for (size_t i=0; i<fs.frames.size(); ++i) {
auto &frame = fs.frames[i];
if (!frame.has(c)) continue;
@@ -199,11 +218,6 @@ void Sender::post(ftl::data::FrameSet &fs, ftl::codecs::Channel c, bool noencode
++valid_frames;
//++fs.flush_count;
- // TODO: Send entire persistent session on inject
- if (do_inject) {
- _sendPersistent(frame);
- }
-
//ccount += frame.changed().size();
if (selected.find(c) != selected.end() || (int)c >= 32) {
diff --git a/components/structures/include/ftl/data/new_frame.hpp b/components/structures/include/ftl/data/new_frame.hpp
index 6bbd61bc2..94c539086 100644
--- a/components/structures/include/ftl/data/new_frame.hpp
+++ b/components/structures/include/ftl/data/new_frame.hpp
@@ -930,9 +930,25 @@ T &ftl::data::Frame::set(ftl::codecs::Channel c) {
auto i = data_.find(c);
if (i != data_.end()) {
if (i->second.status != ftl::data::ChannelStatus::FLUSHED) {
- i->second.encoded.clear();
+
+ auto &d = i->second;
+ if (d.status != ftl::data::ChannelStatus::INVALID && !d.data.has_value() && d.encoded.size() > 0) {
+ UNIQUE_LOCK(parent_->mutex(), lk);
+ if (!d.data.has_value()) {
+ // Do a decode now and change the status
+ //d.status = ftl::data::ChannelStatus::DISPATCHED;
+
+ try {
+ decode_type<T>(d.data, d.encoded.front().data);
+ } catch (...) {
+ throw FTL_Error("Decode failure for channel " << int(c));
+ }
+ }
+ }
+
+ d.encoded.clear();
touch(c);
- return *std::any_cast<T>(&i->second.data);
+ return *std::any_cast<T>(&d.data);
} else {
throw FTL_Error("Channel is flushed and read-only: " << static_cast<unsigned int>(c));
}
@@ -952,6 +968,22 @@ ftl::data::Aggregator<T> ftl::data::Frame::set(ftl::codecs::Channel c) {
auto i = data_.find(c);
if (i != data_.end()) {
if (i->second.status != ftl::data::ChannelStatus::FLUSHED) {
+
+ auto &d = i->second;
+ if (d.status != ftl::data::ChannelStatus::INVALID && !d.data.has_value() && d.encoded.size() > 0) {
+ UNIQUE_LOCK(parent_->mutex(), lk);
+ if (!d.data.has_value()) {
+ // Do a decode now and change the status
+ //d.status = ftl::data::ChannelStatus::DISPATCHED;
+
+ try {
+ decode_type<T>(d.data, d.encoded.front().data);
+ } catch (...) {
+ throw FTL_Error("Decode failure for channel " << int(c));
+ }
+ }
+ }
+
i->second.encoded.clear();
touch(c);
return ftl::data::Aggregator<T>{*std::any_cast<T>(&i->second.data), isAggregate(c)};
--
GitLab