diff --git a/applications/gui2/src/modules/camera.cpp b/applications/gui2/src/modules/camera.cpp index 1bf50c6a93b6388cb983a282f51f75056ad1ae43..e16f381a4c286c04bc7e216b3f774b4c59bde1be 100644 --- a/applications/gui2/src/modules/camera.cpp +++ b/applications/gui2/src/modules/camera.cpp @@ -235,6 +235,11 @@ void Camera::activate(ftl::data::FrameID id) { std::atomic_store(¤t_fs_, fs); std::atomic_store(&latest_, fs); + // Deal with audio + if (fs->frames[frame_idx].hasOwn(Channel::AudioStereo)) { + speaker->queue(fs->timestamp(), fs->frames[frame_idx]); + } + // Need to notify GUI thread when first data comes if (!has_seen_frame_) { //std::unique_lock<std::mutex> lk(m); @@ -270,10 +275,6 @@ void Camera::activate(ftl::data::FrameID id) { } } - // Deal with audio - if (fs->frames[frame_idx].hasOwn(Channel::AudioStereo)) { - speaker->queue(fs->timestamp(), fs->frames[frame_idx]); - } screen->redraw(); nframes_++; return true; @@ -331,6 +332,11 @@ void Camera::startRecording(const std::string &filename, const std::unordered_se io->feed()->startRecording(filter, filename); } +void Camera::startStreaming(const std::unordered_set<ftl::codecs::Channel> &channels) { + filter->select(channels); + io->feed()->startStreaming(filter); +} + ftl::cuda::TextureObject<uchar4>& Camera::getFrame() { if (std::atomic_load(¤t_fs_)) { auto& frame = current_fs_->frames[frame_idx].cast<ftl::rgbd::Frame>(); @@ -396,3 +402,21 @@ 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)) { + auto ptr = std::atomic_load(&latest_); + + 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); + if (x >= 0 && y >= 0 && x < depth.cols && y < depth.rows) { + return depth.at<float>(y, x); + } + } + } + } + return 0.0f; +} diff --git a/applications/gui2/src/modules/camera.hpp b/applications/gui2/src/modules/camera.hpp index a33a1950c3ca762874cdb6fb2a1002902d5bbdff..162500e367d6a741911c26e7549860ff24229062 100644 --- a/applications/gui2/src/modules/camera.hpp +++ b/applications/gui2/src/modules/camera.hpp @@ -52,9 +52,12 @@ public: std::string getActiveSourceURI(); + float depthAt(int x, int y); + bool isRecording(); void stopRecording(); void startRecording(const std::string &filename, const std::unordered_set<ftl::codecs::Channel> &channels); + void startStreaming(const std::unordered_set<ftl::codecs::Channel> &channels); private: int frame_idx = -1; diff --git a/applications/gui2/src/views/addsource.cpp b/applications/gui2/src/views/addsource.cpp index 699e9aef1b475768aa376f1287a8c07478e7ff3a..1f962b4970b0cea566bc1d557188175fd7cd5ae1 100644 --- a/applications/gui2/src/views/addsource.cpp +++ b/applications/gui2/src/views/addsource.cpp @@ -29,11 +29,9 @@ AddSourceWindow::AddSourceWindow(nanogui::Widget* parent, AddCtrl *ctrl) : rebuild(); new_source_handle_ = ctrl_->feed()->onNewSources([this](int a) { + LOG(INFO) << "NEW SOURCES"; UNIQUE_LOCK(mutex_, lk); - // Note the 1, first widget is the title bar buttons - while(childCount() > 1) { removeChild(childCount()-1); } - rebuild(); - screen()->performLayout(); + do_rebuild_ = true; return true; }); } @@ -140,6 +138,17 @@ void AddSourceWindow::close() { dispose(); } -//void AddSourceWindow::draw(NVGcontext *ctx) { +void AddSourceWindow::draw(NVGcontext *ctx) { + { + UNIQUE_LOCK(mutex_, lk); + if (do_rebuild_) { + do_rebuild_ = false; + // Note the 1, first widget is the title bar buttons + while(childCount() > 1) { removeChild(childCount()-1); } + rebuild(); + screen()->performLayout(); + } + } -//} + nanogui::Window::draw(ctx); +} diff --git a/applications/gui2/src/views/addsource.hpp b/applications/gui2/src/views/addsource.hpp index fcca02ecca0c260ddf1fd94f64551b30320ab481..4d340f856c832fd28450adf94c43907c423b1b87 100644 --- a/applications/gui2/src/views/addsource.hpp +++ b/applications/gui2/src/views/addsource.hpp @@ -18,7 +18,7 @@ class AddSourceWindow : public nanogui::Window { AddSourceWindow(nanogui::Widget *parent, AddCtrl *ctrl); virtual ~AddSourceWindow(); - //virtual void draw(NVGcontext *ctx); + virtual void draw(NVGcontext *ctx); private: AddCtrl *ctrl_; @@ -27,6 +27,7 @@ private: ftl::Handle new_source_handle_; MUTEX mutex_; + std::atomic_bool do_rebuild_=false; public: // EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/applications/gui2/src/views/camera.cpp b/applications/gui2/src/views/camera.cpp index 4b4dd782fcf83b6dcc10673f5968b057739069b0..5d1e21f83801c09eb962b2e5625844f1d8f0e7af 100644 --- a/applications/gui2/src/views/camera.cpp +++ b/applications/gui2/src/views/camera.cpp @@ -2,6 +2,7 @@ #include <nanogui/layout.h> #include <nanogui/button.h> #include <nanogui/vscrollpanel.h> +#include <ftl/utility/string.hpp> #include <ftl/codecs/touch.hpp> @@ -123,7 +124,21 @@ RecordOptions::RecordOptions(nanogui::Widget *parent, Camera* ctrl) }); auto stream = new Button(button_panel, "Stream"); - stream->setEnabled(false); + stream->setCallback([this]() { + std::unordered_set<ftl::codecs::Channel> selection; + for (auto &s : channels_) { + if (std::get<0>(s)->checked()) { + selection.emplace(std::get<1>(s)); + } + } + + if (selection.size() > 0) { + ctrl_->startStreaming(selection); + setVisible(false); + } + + if (callback_) callback_(true); + }); auto closebut = new Button(button_panel, "Cancel"); closebut->setCallback([this]() { @@ -352,6 +367,12 @@ CameraView::CameraView(ftl::gui2::Screen* parent, ftl::gui2::Camera* ctrl) : imview_ = new ftl::gui2::FTLImageView(this); panel_ = new ftl::gui2::MediaPanel(screen(), ctrl); + + if (ctrl_->isMovable()) { + imview_->setCursor(nanogui::Cursor::Hand); + } else { + imview_->setCursor(nanogui::Cursor::Crosshair); + } } CameraView::~CameraView() { @@ -394,6 +415,8 @@ bool CameraView::mouseMotionEvent(const Eigen::Vector2i &p, const Eigen::Vector2 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; //} @@ -423,6 +446,14 @@ void CameraView::draw(NVGcontext*ctx) { } } View::draw(ctx); + + 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); + } } void CameraView::performLayout(NVGcontext* ctx) { diff --git a/applications/gui2/src/views/statistics.cpp b/applications/gui2/src/views/statistics.cpp index 7a512c096ad2c7fe31b8656adfb6ad2b42645c90..a593ac942c9506ebba0b6b020ed54d2d36cbed34 100644 --- a/applications/gui2/src/views/statistics.cpp +++ b/applications/gui2/src/views/statistics.cpp @@ -4,6 +4,7 @@ #include <ftl/streams/builder.hpp> #include <ftl/streams/netstream.hpp> #include <ftl/render/colouriser.hpp> +#include <ftl/utility/string.hpp> #include <nanogui/screen.h> #include <nanogui/opengl.h> @@ -13,14 +14,6 @@ using ftl::gui2::StatisticsWidget; using std::string; -template <typename T> -std::string to_string_with_precision(const T a_value, const int n = 6) { - std::ostringstream out; - out.precision(n); - out << std::fixed << a_value; - return out.str(); -} - StatisticsWidget::StatisticsWidget(nanogui::Widget* parent, ftl::gui2::Statistics* ctrl) : nanogui::Widget(parent), ctrl_(ctrl), last_stats_count_(0) { diff --git a/applications/gui2/src/widgets/imageview.cpp b/applications/gui2/src/widgets/imageview.cpp index e34b3a3657b75f1156407b3930a1863ab9026f27..900bbf9580edf2f2d3786fcb5a8861ec9f4e5e49 100644 --- a/applications/gui2/src/widgets/imageview.cpp +++ b/applications/gui2/src/widgets/imageview.cpp @@ -111,6 +111,7 @@ void ftl::gui2::ImageView::bindImage(GLuint imageId) { mImageID = imageId; updateImageParameters(); + } Vector2f ftl::gui2::ImageView::imageCoordinateAt(const Vector2f& position) const { diff --git a/applications/vision/src/main.cpp b/applications/vision/src/main.cpp index 77c5a85866729fd7e9db6352f3e4dc06bf53616f..c2860bf5c276d8005e2b55e2210a3439d5cc61aa 100644 --- a/applications/vision/src/main.cpp +++ b/applications/vision/src/main.cpp @@ -298,7 +298,7 @@ int main(int argc, char **argv) { root->value("restart", 0); // Allow config controlled restart - root->on("restart", [root](const ftl::config::Event &e) { + root->on("restart", [root]() { auto val = root->get<int>("restart"); if (val) { ftl::exit_code = *val; diff --git a/components/audio/src/speaker.cpp b/components/audio/src/speaker.cpp index 99bcee9e5a7e601cfdecc7784da6cad103fc6f0d..9560bbed16d9def03c2015437d5d35590510c9ed 100644 --- a/components/audio/src/speaker.cpp +++ b/components/audio/src/speaker.cpp @@ -38,7 +38,7 @@ Speaker::Speaker(nlohmann::json &config) : ftl::Configurable(config), buffer_(nu volume_ = 1.0f; active_ = false; extra_delay_ = value("delay",0.0f); - on("delay", [this](const ftl::config::Event &e) { + on("delay", [this]() { extra_delay_ = value("delay",0.0f); }); } diff --git a/components/common/cpp/include/ftl/configurable.hpp b/components/common/cpp/include/ftl/configurable.hpp index c0b8ae802aeb722a547af95f0b884e780f0cd933..f08f5a72ac31e836972a327fbc1e47eb11a6b1f6 100644 --- a/components/common/cpp/include/ftl/configurable.hpp +++ b/components/common/cpp/include/ftl/configurable.hpp @@ -93,19 +93,22 @@ class Configurable { * @param prop Name of property to watch * @param callback A function object that will be called on change. */ - void on(const std::string &prop, std::function<void(const config::Event&)>); + void on(const std::string &prop, std::function<void()>); - //void on(const std::string &prop, const std::function<void()> &); + /** + * Same callback for all properties in set. + */ + void onAny(const std::unordered_set<std::string> &props, std::function<void()>); template <typename T> void on(const std::string &prop, T &v) { - on(prop, [&v,this,prop](const config::Event &e) { v = *this->get<T>(prop); }); + on(prop, [&v,this,prop]() { v = *this->get<T>(prop); }); } template <typename T> void on(const std::string &prop, T &v, const T &def) { v = this->value(prop, def); - on(prop, [&v,this,prop](const config::Event &e) { v = *this->get<T>(prop); }); + on(prop, [&v,this,prop]() { v = *this->get<T>(prop); }); } void patchPtr(nlohmann::json &newcfg) { config_ = &newcfg; } @@ -147,7 +150,7 @@ class Configurable { std::string restore_; std::string defaults_; std::unordered_set<std::string> save_allowed_; - std::map<std::string, std::list<std::function<void(const config::Event&)>>> observers_; + std::map<std::string, std::list<std::function<void()>>> observers_; void _trigger(const std::string &name); }; diff --git a/components/common/cpp/include/ftl/utility/string.hpp b/components/common/cpp/include/ftl/utility/string.hpp new file mode 100644 index 0000000000000000000000000000000000000000..d88aa11532a649702fa4325970e60c2fac16ca7c --- /dev/null +++ b/components/common/cpp/include/ftl/utility/string.hpp @@ -0,0 +1,12 @@ +#ifndef _FTL_UTILITY_STRING_HPP_ +#define _FTL_UTILITY_STRING_HPP_ + +template <typename T> +std::string to_string_with_precision(const T a_value, const int n = 6) { + std::ostringstream out; + out.precision(n); + out << std::fixed << a_value; + return out.str(); +} + +#endif \ No newline at end of file diff --git a/components/common/cpp/src/configurable.cpp b/components/common/cpp/src/configurable.cpp index c947eadada1e6f353b34341a6a3c440d1ccbfbe1..fd8db20e3d5f0a76518847f5e97c6876f8387b8b 100644 --- a/components/common/cpp/src/configurable.cpp +++ b/components/common/cpp/src/configurable.cpp @@ -136,7 +136,7 @@ void Configurable::_trigger(const string &name) { if (ix != observers_.end()) { for (auto &f : (*ix).second) { try { - f({this, name}); + f(); } catch(...) { LOG(ERROR) << "Exception in event handler for '" << name << "'"; } @@ -144,7 +144,13 @@ void Configurable::_trigger(const string &name) { } } -void Configurable::on(const string &prop, function<void(const ftl::config::Event&)> f) { +void Configurable::onAny(const std::unordered_set<string> &props, function<void()> f) { + for (const auto &p : props) { + on(p, f); + } +} + +void Configurable::on(const string &prop, function<void()> f) { auto ix = observers_.find(prop); if (ix == observers_.end()) { observers_[prop] = {f}; diff --git a/components/common/cpp/src/configuration.cpp b/components/common/cpp/src/configuration.cpp index e987b6afac423cc0d35f11d4501bc5f81e4737ac..327bfd03ba7585b7b2efd6120f9b79eb15422422 100644 --- a/components/common/cpp/src/configuration.cpp +++ b/components/common/cpp/src/configuration.cpp @@ -886,9 +886,9 @@ Configurable *ftl::config::configure(int argc, char **argv, const std::string &r if (rootcfg->get<std::string>("branch")) { ftl::branch_name = *rootcfg->get<std::string>("branch"); } - rootcfg->on("branch", [](const ftl::config::Event &e) { - if (e.entity->get<std::string>("branch")) { - ftl::branch_name = *e.entity->get<std::string>("branch"); + rootcfg->on("branch", [rootcfg]() { + if (rootcfg->get<std::string>("branch")) { + ftl::branch_name = *rootcfg->get<std::string>("branch"); } }); diff --git a/components/common/cpp/test/configurable_unit.cpp b/components/common/cpp/test/configurable_unit.cpp index ed5a1a73c5870b1aabebd417c4f05af8c5bca7ae..ec5668c43521d5838c510a515febcc924e0a79c8 100644 --- a/components/common/cpp/test/configurable_unit.cpp +++ b/components/common/cpp/test/configurable_unit.cpp @@ -47,7 +47,7 @@ SCENARIO( "Configurable::on()" ) { Configurable cfg(json); bool trig = false; - cfg.on("test", [&trig](const ftl::config::Event &e) { + cfg.on("test", [&trig]() { trig = true; }); @@ -63,10 +63,10 @@ SCENARIO( "Configurable::on()" ) { bool trig1 = false; bool trig2 = false; - cfg.on("test", [&trig1](const ftl::config::Event &e) { + cfg.on("test", [&trig1]() { trig1 = true; }); - cfg.on("test", [&trig2](const ftl::config::Event &e) { + cfg.on("test", [&trig2]() { trig2 = true; }); diff --git a/components/control/cpp/src/master.cpp b/components/control/cpp/src/master.cpp index a2913024aedd24c37b905b3eaa8f41239cfd4616..c0677c03d5ab597464b5fb412944abb24183e379 100644 --- a/components/control/cpp/src/master.cpp +++ b/components/control/cpp/src/master.cpp @@ -158,9 +158,9 @@ vector<string> Master::getConfigurables() { vector<string> Master::getConfigurables(const ftl::UUID &peer) { try { - LOG(INFO) << "LISTING CONFIGS"; return net_->call<vector<string>>(peer, "list_configurables"); - } catch (...) { + } catch (const ftl::exception &e) { + e.ignore(); return {}; } } diff --git a/components/net/cpp/include/ftl/net/universe.hpp b/components/net/cpp/include/ftl/net/universe.hpp index 37bdd1362199cede98464535027b25e158134ef9..6cc631a06ac509b96f615aa046c51bf5b55ded81 100644 --- a/components/net/cpp/include/ftl/net/universe.hpp +++ b/components/net/cpp/include/ftl/net/universe.hpp @@ -384,7 +384,7 @@ std::vector<R> Universe::findAll(const std::string &name, ARGS... args) { { // Block thread until async callback notifies us //UNIQUE_LOCK(m,llk); std::unique_lock<std::mutex> llk(m); - cv.wait_for(llk, std::chrono::seconds(1), [&returncount,&sentcount]{return returncount == sentcount;}); + cv.wait_for(llk, std::chrono::seconds(1), [&returncount,sentcount]{return returncount == sentcount;}); // Cancel any further results lk.lock(); diff --git a/components/net/cpp/src/peer.cpp b/components/net/cpp/src/peer.cpp index 339e2f6670b48c0b1823cb6c6ebf1de6fe280158..7263281e8203139fb23da7211ab7a554cae6d1d6 100644 --- a/components/net/cpp/src/peer.cpp +++ b/components/net/cpp/src/peer.cpp @@ -130,22 +130,30 @@ static SOCKET tcpConnect(URI &uri, size_t ssize, size_t rsize) { if (rc < 0) { if (errno == EINPROGRESS) { // FIXME:(Nick) Move to main select thread to prevent blocking - fd_set myset; + fd_set myset; + fd_set errset; struct timeval tv; tv.tv_sec = 1; tv.tv_usec = 0; FD_ZERO(&myset); - FD_SET(csocket, &myset); - rc = select(csocket+1u, NULL, &myset, NULL, &tv); - if (rc <= 0) { //} && errno != EINTR) { + FD_SET(csocket, &myset); + FD_ZERO(&errset); + FD_SET(csocket, &errset); + + rc = select(csocket+1u, NULL, &myset, &errset, &tv); + if (rc <= 0 || FD_ISSET(csocket, &errset)) { //} && errno != EINTR) { + if (rc <= 0) { + LOG(ERROR) << "Could not connect to " << uri.getBaseURI(); + } else { + LOG(ERROR) << "Could not connect (" << errno << ") " << uri.getBaseURI(); + } + #ifndef WIN32 close(csocket); #else closesocket(csocket); #endif - LOG(ERROR) << "Could not connect to " << uri.getBaseURI(); - return INVALID_SOCKET; } } else { @@ -252,7 +260,7 @@ Peer::Peer(const char *pUri, Universe *u, Dispatcher *d) : can_reconnect_(true), _badClose(false); } else { status_ = kConnecting; - LOG(INFO) << "WEB SOCK CONNECTED"; + LOG(INFO) << "Websocket connected: " << pUri; } } else { LOG(ERROR) << "Connection refused to " << uri.getHost() << ":" << uri.getPort(); diff --git a/components/operators/src/aruco.cpp b/components/operators/src/aruco.cpp index 7c7259ef2011ce9dea0a25807603068c8f3b5064..871e5812adf2927d864ff5640f50def31983444b 100644 --- a/components/operators/src/aruco.cpp +++ b/components/operators/src/aruco.cpp @@ -55,7 +55,7 @@ ArUco::ArUco(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) { channel_in_ = Channel::Colour; channel_out_ = Channel::Shapes3D; - cfg->on("dictionary", [this,cfg](const ftl::config::Event &e) { + cfg->on("dictionary", [this,cfg]() { dictionary_ = cv::aruco::getPredefinedDictionary(cfg->value("dictionary", 0)); }); } diff --git a/components/operators/src/depth.cpp b/components/operators/src/depth.cpp index 107d92564406ca27846d95e7bccb56917a872f79..33af310458d24d110a8e721e46022376d02fcb3d 100644 --- a/components/operators/src/depth.cpp +++ b/components/operators/src/depth.cpp @@ -59,10 +59,10 @@ DepthBilateralFilter::DepthBilateralFilter(ftl::Configurable* cfg) : max_disc_ = cfg->value("max_discontinuity", 0.1f); channel_ = Channel::Depth; - cfg->on("edge_discontinuity", [this](const ftl::config::Event &e) { + cfg->on("edge_discontinuity", [this]() { edge_disc_ = config()->value("edge_discontinuity", 0.04f); }); - cfg->on("max_discontinuity", [this](const ftl::config::Event &e) { + cfg->on("max_discontinuity", [this]() { max_disc_ = config()->value("max_discontinuity", 0.1f); }); @@ -82,10 +82,10 @@ DepthBilateralFilter::DepthBilateralFilter(ftl::Configurable* cfg, const std::tu max_disc_ = cfg->value("max_discontinuity", 0.1f); channel_ = std::get<0>(p); - cfg->on("edge_discontinuity", [this](const ftl::config::Event &e) { + cfg->on("edge_discontinuity", [this]() { edge_disc_ = config()->value("edge_discontinuity", 0.04f); }); - cfg->on("max_discontinuity", [this](const ftl::config::Event &e) { + cfg->on("max_discontinuity", [this]() { max_disc_ = config()->value("max_discontinuity", 0.1f); }); diff --git a/components/operators/src/detectandtrack.cpp b/components/operators/src/detectandtrack.cpp index a690e1ade6c60dc0c0a1aa9037dce414889381c2..4e9b1b7294bc168957bc63206d4601ef424f45e6 100644 --- a/components/operators/src/detectandtrack.cpp +++ b/components/operators/src/detectandtrack.cpp @@ -28,7 +28,7 @@ bool DetectAndTrack::init() { fname_ = config()->value<string>("filename", FTL_LOCAL_DATA_ROOT "/haarcascades/haarcascade_frontalface_default.xml"); debug_ = config()->value<bool>("debug", false); - config()->on("debug", [this](const ftl::config::Event &e) { + config()->on("debug", [this]() { debug_ = config()->value<bool>("debug", false); }); diff --git a/components/operators/src/disparity/fixstars_sgm.cpp b/components/operators/src/disparity/fixstars_sgm.cpp index 0e48629fc2f76d259ca5411d24136d278a5b2f6b..0274022737726a8d6a94c5bcc8124c82a8d4dd17 100644 --- a/components/operators/src/disparity/fixstars_sgm.cpp +++ b/components/operators/src/disparity/fixstars_sgm.cpp @@ -87,7 +87,7 @@ FixstarsSGM::FixstarsSGM(ftl::Configurable* cfg) : LOG(WARNING) << "Invalid value for max_disp, using default value (256)"; } - cfg->on("P1", [this, cfg](const ftl::config::Event&) { + cfg->on("P1", [this, cfg]() { int P1 = cfg->value("P1", 0); if (P1 <= 0) { LOG(WARNING) << "Invalid value for P1 (" << P1 << ")"; @@ -98,7 +98,7 @@ FixstarsSGM::FixstarsSGM(ftl::Configurable* cfg) : } }); - cfg->on("P2", [this, cfg](const ftl::config::Event&) { + cfg->on("P2", [this, cfg]() { int P2 = cfg->value("P2", 0); if (P2 < P1_) { LOG(WARNING) << "Invalid value for P2 (" << P2 << ")"; @@ -109,7 +109,7 @@ FixstarsSGM::FixstarsSGM(ftl::Configurable* cfg) : } }); - cfg->on("uniqueness", [this, cfg](const ftl::config::Event&) { + cfg->on("uniqueness", [this, cfg]() { double uniqueness = cfg->value("uniqueness", 0.0); if (uniqueness < 0.0 || uniqueness > 1.0) { LOG(WARNING) << "Invalid value for uniqueness (" << uniqueness << ")"; @@ -122,11 +122,11 @@ FixstarsSGM::FixstarsSGM(ftl::Configurable* cfg) : updateP2Parameters(); - cfg->on("canny_low", [this, cfg](const ftl::config::Event&) { + cfg->on("canny_low", [this, cfg]() { updateP2Parameters(); }); - cfg->on("canny_high", [this, cfg](const ftl::config::Event&) { + cfg->on("canny_high", [this, cfg]() { updateP2Parameters(); }); } diff --git a/components/operators/src/disparity/optflow_smoothing.cpp b/components/operators/src/disparity/optflow_smoothing.cpp index 897b67a7fcbac032d9fe8ea888cc13a165d31bb7..c69b0727beda8ceb22580609ed8f7181a22d40f9 100644 --- a/components/operators/src/disparity/optflow_smoothing.cpp +++ b/components/operators/src/disparity/optflow_smoothing.cpp @@ -47,7 +47,7 @@ void OpticalFlowTemporalSmoothing::_init(ftl::Configurable* cfg) { threshold_ = cfg->value("threshold", 5.0f); - cfg->on("threshold", [this](const ftl::config::Event&) { + cfg->on("threshold", [this]() { float threshold = config()->value("threshold", 5.0f); if (threshold < 0.0) { LOG(WARNING) << "invalid threshold " << threshold << ", value must be positive"; @@ -58,7 +58,7 @@ void OpticalFlowTemporalSmoothing::_init(ftl::Configurable* cfg) { } }); - cfg->on("history_size", [this, &cfg](const ftl::config::Event&) { + cfg->on("history_size", [this, &cfg]() { int n_max = cfg->value("history_size", 7); if (n_max < 1) { diff --git a/components/operators/src/operator.cpp b/components/operators/src/operator.cpp index 5c729697c1ebbc6a34d44a2350330d1f1acddc7e..339ba27f81bb32f05c54fe66df0ded78792ca566 100644 --- a/components/operators/src/operator.cpp +++ b/components/operators/src/operator.cpp @@ -12,7 +12,7 @@ using ftl::rgbd::Source; Operator::Operator(ftl::Configurable *config) : config_(config) { enabled_ = config_->value("enabled", true); - config_->on("enabled", [this](const ftl::config::Event &e) { + config_->on("enabled", [this]() { enabled_ = config_->value("enabled", true); }); } diff --git a/components/renderers/cpp/src/CUDARender.cpp b/components/renderers/cpp/src/CUDARender.cpp index 55487c6dc01660627e93bacb397801fab1bc83a2..c72d3dacea9684adb9abd26f458665b6c0079dbe 100644 --- a/components/renderers/cpp/src/CUDARender.cpp +++ b/components/renderers/cpp/src/CUDARender.cpp @@ -62,27 +62,27 @@ CUDARender::CUDARender(nlohmann::json &config) : ftl::render::FSRenderer(config) on("touch_sensitivity", touch_dist_, 0.04f); - on("clipping_enabled", [this](const ftl::config::Event &e) { + on("clipping_enabled", [this]() { clipping_ = value("clipping_enabled", true); }); norm_filter_ = value("normal_filter", -1.0f); - on("normal_filter", [this](const ftl::config::Event &e) { + on("normal_filter", [this]() { norm_filter_ = value("normal_filter", -1.0f); }); backcull_ = value("back_cull", true); - on("back_cull", [this](const ftl::config::Event &e) { + on("back_cull", [this]() { backcull_ = value("back_cull", true); }); mesh_ = value("meshing", true); - on("meshing", [this](const ftl::config::Event &e) { + on("meshing", [this]() { mesh_ = value("meshing", true); }); background_ = parseCVColour(value("background", std::string("#4c4c4c"))); - on("background", [this](const ftl::config::Event &e) { + on("background", [this]() { background_ = parseCVColour(value("background", std::string("#4c4c4c"))); }); @@ -184,7 +184,7 @@ void CUDARender::_dibr(ftl::rgbd::Frame &out, const Eigen::Matrix4d &t, cudaStre auto &f = scene_->frames[i].cast<ftl::rgbd::Frame>(); //auto *s = scene_->sources[i]; - if (f.has(Channel::Colour)) { + if (!f.has(Channel::Colour)) { LOG(ERROR) << "Missing required channel"; continue; } @@ -462,7 +462,7 @@ void CUDARender::_postprocessColours(ftl::rgbd::Frame &out) { params_.camera, stream_ ); - } else if (out.hasChannel(_getDepthChannel()) && out.hasChannel(out_chan_)) { + } else if (mesh_ && out.hasChannel(_getDepthChannel()) && out.hasChannel(out_chan_)) { ftl::cuda::fix_bad_colour( out.getTexture<float>(_getDepthChannel()), out.getTexture<uchar4>(out_chan_), diff --git a/components/renderers/cpp/src/dibr.cu b/components/renderers/cpp/src/dibr.cu index e57ffe615fb3ccf806899a1d6e12588a6048906c..a00f4be523294ddaea6f31e3ac847f01415e59fb 100644 --- a/components/renderers/cpp/src/dibr.cu +++ b/components/renderers/cpp/src/dibr.cu @@ -14,6 +14,7 @@ using ftl::rgbd::Projection; /* * DIBR point cloud with a depth check */ + template <Projection PROJECT> __global__ void dibr_merge_kernel(TextureObject<float> depth, TextureObject<int> depth_out, float4x4 transform, @@ -30,7 +31,7 @@ using ftl::rgbd::Projection; //const float d = camPos.z; //const uint2 screenPos = params.camera.camToScreen<uint2>(camPos); - const float3 screenPos = params.camera.project<Projection::PERSPECTIVE>(camPos); + const float3 screenPos = params.camera.project<PROJECT>(camPos); const unsigned int cx = (unsigned int)(screenPos.x+0.5f); const unsigned int cy = (unsigned int)(screenPos.y+0.5f); const float d = screenPos.z; @@ -70,7 +71,11 @@ void ftl::cuda::dibr_merge(TextureObject<float> &depth, TextureObject<int> &dept const dim3 gridSize((depth.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (depth.height() + T_PER_BLOCK - 1)/T_PER_BLOCK); const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK); - dibr_merge_kernel<<<gridSize, blockSize, 0, stream>>>(depth, depth_out, transform, cam, params); + if (params.projection == Projection::PERSPECTIVE) { + dibr_merge_kernel<Projection::PERSPECTIVE><<<gridSize, blockSize, 0, stream>>>(depth, depth_out, transform, cam, params); + } else { + dibr_merge_kernel<Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(depth, depth_out, transform, cam, params); + } cudaSafeCall( cudaGetLastError() ); } diff --git a/components/renderers/cpp/src/reprojection.cu b/components/renderers/cpp/src/reprojection.cu index 358e465c1ac199bceeb2fabac954729526b2dfa5..e58ba6fa77984ba6ed2926cc3e15c8bd053ce27a 100644 --- a/components/renderers/cpp/src/reprojection.cu +++ b/components/renderers/cpp/src/reprojection.cu @@ -239,7 +239,7 @@ __global__ void reprojection_kernel( const float d = depth_in.tex2D((int)x, (int)y); if (d > params.camera.minDepth && d < params.camera.maxDepth) { //const float2 rpt = convertScreen<VPMODE>(params, x, y); - const float3 camPos = transform * params.camera.project<PROJECT>(make_float3(x, y, d)); + const float3 camPos = transform * params.camera.unproject<PROJECT>(make_float3(x, y, d)); if (camPos.z > camera.minDepth && camPos.z < camera.maxDepth) { const float3 screenPos = camera.project<Projection::PERSPECTIVE>(camPos); @@ -250,7 +250,8 @@ __global__ void reprojection_kernel( // Boolean match (0 or 1 weight). 1.0 if depths are sufficiently close float weight = depthMatching(params, camPos.z, d2); - weight *= float(weights.tex2D(int(screenPos.x+0.5f), int(screenPos.y+0.5f))) / 32767.0f; + if (params.m_flags & ftl::render::kUseWeightsChannel) + weight *= float(weights.tex2D(int(screenPos.x+0.5f), int(screenPos.y+0.5f))) / 32767.0f; const B output = make<B>(input); // * weight; //weightInput(input, weight); @@ -282,30 +283,35 @@ void ftl::cuda::reproject( if (params.accumulationMode == AccumulationFunction::CloseWeights) { switch (params.projection) { case Projection::PERSPECTIVE: reprojection_kernel<A,B,AccumulationFunction::CloseWeights, Projection::PERSPECTIVE><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; + case Projection::ORTHOGRAPHIC: reprojection_kernel<A,B,AccumulationFunction::CloseWeights, Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; case Projection::EQUIRECTANGULAR: reprojection_kernel<A,B,AccumulationFunction::CloseWeights, Projection::EQUIRECTANGULAR><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; //case ViewPortMode::Stretch: reprojection_kernel<A,B,ViewPortMode::Stretch,AccumulationFunction::CloseWeights><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; } } else if (params.accumulationMode == AccumulationFunction::BestWeight) { switch (params.projection) { case Projection::PERSPECTIVE: reprojection_kernel<A,B,AccumulationFunction::BestWeight, Projection::PERSPECTIVE><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; + case Projection::ORTHOGRAPHIC: reprojection_kernel<A,B,AccumulationFunction::BestWeight, Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; case Projection::EQUIRECTANGULAR: reprojection_kernel<A,B,AccumulationFunction::BestWeight, Projection::EQUIRECTANGULAR><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; //case ViewPortMode::Stretch: reprojection_kernel<A,B,ViewPortMode::Stretch,AccumulationFunction::BestWeight><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; } } else if (params.accumulationMode == AccumulationFunction::Simple) { switch (params.projection) { case Projection::PERSPECTIVE: reprojection_kernel<A,B,AccumulationFunction::Simple, Projection::PERSPECTIVE><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; + case Projection::ORTHOGRAPHIC: reprojection_kernel<A,B,AccumulationFunction::Simple, Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; case Projection::EQUIRECTANGULAR: reprojection_kernel<A,B,AccumulationFunction::Simple, Projection::EQUIRECTANGULAR><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; //case ViewPortMode::Stretch: reprojection_kernel<A,B,ViewPortMode::Stretch,AccumulationFunction::Simple><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; } } else if (params.accumulationMode == AccumulationFunction::ColourDiscard) { switch (params.projection) { case Projection::PERSPECTIVE: reprojection_kernel<A,B,AccumulationFunction::ColourDiscard, Projection::PERSPECTIVE><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; + case Projection::ORTHOGRAPHIC: reprojection_kernel<A,B,AccumulationFunction::ColourDiscard, Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; case Projection::EQUIRECTANGULAR: reprojection_kernel<A,B,AccumulationFunction::ColourDiscard, Projection::EQUIRECTANGULAR><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; //case ViewPortMode::Stretch: reprojection_kernel<A,B,ViewPortMode::Stretch,AccumulationFunction::ColourDiscard><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; } } else if (params.accumulationMode == AccumulationFunction::ColourDiscardSmooth) { switch (params.projection) { case Projection::PERSPECTIVE: reprojection_kernel<A,B,AccumulationFunction::ColourDiscardSmooth, Projection::PERSPECTIVE><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; + case Projection::ORTHOGRAPHIC: reprojection_kernel<A,B,AccumulationFunction::ColourDiscardSmooth, Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; case Projection::EQUIRECTANGULAR: reprojection_kernel<A,B,AccumulationFunction::ColourDiscardSmooth, Projection::EQUIRECTANGULAR><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; //case ViewPortMode::Stretch: reprojection_kernel<A,B,ViewPortMode::Stretch,AccumulationFunction::ColourDiscardSmooth><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, *normals, out, contrib, params, camera, transform, transformR); break; } @@ -314,30 +320,35 @@ void ftl::cuda::reproject( if (params.accumulationMode == AccumulationFunction::CloseWeights) { switch (params.projection) { case Projection::PERSPECTIVE: reprojection_kernel<A,B,AccumulationFunction::CloseWeights, Projection::PERSPECTIVE><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; + case Projection::ORTHOGRAPHIC: reprojection_kernel<A,B,AccumulationFunction::CloseWeights, Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; case Projection::EQUIRECTANGULAR: reprojection_kernel<A,B,AccumulationFunction::CloseWeights, Projection::EQUIRECTANGULAR><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; //case ViewPortMode::Stretch: reprojection_kernel<A,B,ViewPortMode::Stretch,AccumulationFunction::CloseWeights><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; } } else if (params.accumulationMode == AccumulationFunction::BestWeight) { switch (params.projection) { case Projection::PERSPECTIVE: reprojection_kernel<A,B,AccumulationFunction::BestWeight, Projection::PERSPECTIVE><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; + case Projection::ORTHOGRAPHIC: reprojection_kernel<A,B,AccumulationFunction::BestWeight, Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; case Projection::EQUIRECTANGULAR: reprojection_kernel<A,B,AccumulationFunction::BestWeight, Projection::EQUIRECTANGULAR><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; //case ViewPortMode::Stretch: reprojection_kernel<A,B,ViewPortMode::Stretch,AccumulationFunction::BestWeight><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; } } else if (params.accumulationMode == AccumulationFunction::Simple) { switch (params.projection) { case Projection::PERSPECTIVE: reprojection_kernel<A,B,AccumulationFunction::Simple, Projection::PERSPECTIVE><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; + case Projection::ORTHOGRAPHIC: reprojection_kernel<A,B,AccumulationFunction::Simple, Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; case Projection::EQUIRECTANGULAR: reprojection_kernel<A,B,AccumulationFunction::Simple, Projection::EQUIRECTANGULAR><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; //case ViewPortMode::Stretch: reprojection_kernel<A,B,ViewPortMode::Stretch,AccumulationFunction::Simple><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; } } else if (params.accumulationMode == AccumulationFunction::ColourDiscard) { switch (params.projection) { case Projection::PERSPECTIVE: reprojection_kernel<A,B,AccumulationFunction::ColourDiscard, Projection::PERSPECTIVE><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; + case Projection::ORTHOGRAPHIC: reprojection_kernel<A,B,AccumulationFunction::ColourDiscard, Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; case Projection::EQUIRECTANGULAR: reprojection_kernel<A,B,AccumulationFunction::ColourDiscard, Projection::EQUIRECTANGULAR><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; //case ViewPortMode::Stretch: reprojection_kernel<A,B,ViewPortMode::Stretch,AccumulationFunction::ColourDiscard><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; } } else if (params.accumulationMode == AccumulationFunction::ColourDiscardSmooth) { switch (params.projection) { case Projection::PERSPECTIVE: reprojection_kernel<A,B,AccumulationFunction::ColourDiscardSmooth, Projection::PERSPECTIVE><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; + case Projection::ORTHOGRAPHIC: reprojection_kernel<A,B,AccumulationFunction::ColourDiscardSmooth, Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; case Projection::EQUIRECTANGULAR: reprojection_kernel<A,B,AccumulationFunction::ColourDiscardSmooth, Projection::EQUIRECTANGULAR><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; //case ViewPortMode::Stretch: reprojection_kernel<A,B,ViewPortMode::Stretch,AccumulationFunction::ColourDiscardSmooth><<<gridSize, blockSize, 0, stream>>>(in, depth_src, depth_in, weights, out, contrib, params, camera, transform, transformR); break; } diff --git a/components/renderers/cpp/src/screen.cu b/components/renderers/cpp/src/screen.cu index f2dd97e43b469ac373c9db4a63eb60f69ca5c7ac..5ebbdd223c539d5c7f404a0c5e9c0d990675abfe 100644 --- a/components/renderers/cpp/src/screen.cu +++ b/components/renderers/cpp/src/screen.cu @@ -91,6 +91,12 @@ void ftl::cuda::screen_coord(TextureObject<float> &depth, TextureObject<float> & case ViewPortMode::Clipping: screen_coord_kernel<ViewPortMode::Clipping, Projection::EQUIRECTANGULAR><<<gridSize, blockSize, 0, stream>>>(depth, depth_out, screen_out, params, pose, camera); break; case ViewPortMode::Stretch: screen_coord_kernel<ViewPortMode::Stretch, Projection::EQUIRECTANGULAR><<<gridSize, blockSize, 0, stream>>>(depth, depth_out, screen_out, params, pose, camera); break; } + } else if (params.projection == Projection::ORTHOGRAPHIC) { + switch (params.viewPortMode) { + case ViewPortMode::Disabled: screen_coord_kernel<ViewPortMode::Disabled, Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(depth, depth_out, screen_out, params, pose, camera); break; + case ViewPortMode::Clipping: screen_coord_kernel<ViewPortMode::Clipping, Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(depth, depth_out, screen_out, params, pose, camera); break; + case ViewPortMode::Stretch: screen_coord_kernel<ViewPortMode::Stretch, Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(depth, depth_out, screen_out, params, pose, camera); break; + } } cudaSafeCall( cudaGetLastError() ); } diff --git a/components/rgbd-sources/include/ftl/rgbd/camera.hpp b/components/rgbd-sources/include/ftl/rgbd/camera.hpp index 804a0c2d3ea769e28a89d1663de64ba7a1390d57..f68de82d0c031ae7d5ea2901ed3a02f6b93aa841 100644 --- a/components/rgbd-sources/include/ftl/rgbd/camera.hpp +++ b/components/rgbd-sources/include/ftl/rgbd/camera.hpp @@ -142,6 +142,22 @@ inline float3 ftl::rgbd::Camera::unproject<ftl::rgbd::Projection::PERSPECTIVE>(c return make_float3(pos.z*x, pos.z*y, pos.z); } +template <> __device__ __host__ +inline float3 ftl::rgbd::Camera::project<ftl::rgbd::Projection::ORTHOGRAPHIC>(const float3 &pos) const { + return make_float3( + static_cast<float>(pos.x*fx - cx), + static_cast<float>(pos.y*fy - cy), + pos.z + ); +} + +template <> __device__ __host__ +inline float3 ftl::rgbd::Camera::unproject<ftl::rgbd::Projection::ORTHOGRAPHIC>(const float3 &pos) const { + const float x = static_cast<float>((pos.x+cx) / fx); + const float y = static_cast<float>((pos.y+cy) / fy); + return make_float3(x, y, pos.z); +} + template <> __device__ __host__ inline float2 ftl::rgbd::Camera::camToScreen<float2>(const float3 &pos) const { return make_float2( diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp index 45018ee89619feeafb410a5518330cb350cc7f91..4a4d5e92549042e606f9241e687b817205387e20 100644 --- a/components/rgbd-sources/src/source.cpp +++ b/components/rgbd-sources/src/source.cpp @@ -35,7 +35,7 @@ Source::Source(ftl::config::json_t &cfg) : Configurable(cfg) { is_retrieving = false; reset(); - on("uri", [this](const ftl::config::Event &e) { + on("uri", [this]() { LOG(INFO) << "URI change for source: " << getURI(); reset(); }); diff --git a/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp b/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp index 656310c26a96730de4dec8cd37b72ae592bc9a9d..eee884d2f9e62c6c8a42b98d681b19b2463ac14b 100644 --- a/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp +++ b/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp @@ -91,20 +91,20 @@ MiddleburySource::MiddleburySource(ftl::rgbd::Source *host, const string &dir) host_->getConfig()["doffs"] = params_.doffs; // Add event handlers to allow calibration changes... - host_->on("baseline", [this](const ftl::config::Event &e) { + host_->on("baseline", [this]() { params_.baseline = host_->value("baseline", params_.baseline); }); - host_->on("focal", [this](const ftl::config::Event &e) { + host_->on("focal", [this]() { params_.fx = host_->value("focal", params_.fx); params_.fy = params_.fx; }); - host_->on("doffs", [this](const ftl::config::Event &e) { + host_->on("doffs", [this]() { params_.doffs = host_->value("doffs", params_.doffs); }); - host_->on("centre_x", [this](const ftl::config::Event &e) { + host_->on("centre_x", [this]() { params_.cx = host_->value("centre_x", params_.cx); }); diff --git a/components/rgbd-sources/src/sources/realsense/realsense_source.cpp b/components/rgbd-sources/src/sources/realsense/realsense_source.cpp index 0aa4a1776b22f7c927dd5bd218400bdeb8a9c8fe..5b3ee5b6d34e22b50108f45c2d722a34ee6f887d 100644 --- a/components/rgbd-sources/src/sources/realsense/realsense_source.cpp +++ b/components/rgbd-sources/src/sources/realsense/realsense_source.cpp @@ -69,10 +69,17 @@ RealsenseSource::~RealsenseSource() { } +static bool rs_supported = false; +static bool rs_init = false; + bool RealsenseSource::supported() { + if (rs_init) return rs_supported; + rs_init = true; + rs2::context ctx; auto devs = ctx.query_devices(); - return devs.size() > 0; + rs_supported = devs.size() > 0; + return rs_supported; } bool RealsenseSource::capture(int64_t ts) { diff --git a/components/rgbd-sources/src/sources/screencapture/screencapture.cpp b/components/rgbd-sources/src/sources/screencapture/screencapture.cpp index 5c9ca286b84726e7e5673d554207b822bc06b28d..87b34497b397f30860d72b29ad2f255e0541c173 100644 --- a/components/rgbd-sources/src/sources/screencapture/screencapture.cpp +++ b/components/rgbd-sources/src/sources/screencapture/screencapture.cpp @@ -180,7 +180,7 @@ ScreenCapture::ScreenCapture(ftl::rgbd::Source *host) //state_.setPose(matrix(cv::Vec3d(0.0, 3.14159, 0.0), cv::Vec3d(0.0,0.0,params_.maxDepth+offsetz))); pose_ = matrix(cv::Vec3d(0.0, 3.14159, 0.0), cv::Vec3d(0.0,0.0,params_.maxDepth+offsetz)); - host_->on("size", [this](const ftl::config::Event &e) { + host_->on("size", [this]() { float offsetz = host_->value("offset_z",0.0f); params_.maxDepth = host_->value("size", 1.0f); //state_.getLeft() = params_; @@ -188,7 +188,7 @@ ScreenCapture::ScreenCapture(ftl::rgbd::Source *host) do_update_params_ = true; }); - host_->on("offset_z", [this](const ftl::config::Event &e) { + host_->on("offset_z", [this]() { float offsetz = host_->value("offset_z",0.0f); params_.maxDepth = host_->value("size", 1.0f); //state_.getLeft() = params_; @@ -196,15 +196,15 @@ ScreenCapture::ScreenCapture(ftl::rgbd::Source *host) do_update_params_ = true; }); - host_->on("offset_x", [this](const ftl::config::Event &e) { + host_->on("offset_x", [this]() { offset_x_ = host_->value("offset_x", 0); }); - host_->on("offset_y", [this](const ftl::config::Event &e) { + host_->on("offset_y", [this]() { offset_y_ = host_->value("offset_y", 0); }); - host_->on("enable_touch", [this](const ftl::config::Event &e) { + host_->on("enable_touch", [this]() { do_update_params_ = true; }); } diff --git a/components/rgbd-sources/src/sources/stereovideo/opencv.cpp b/components/rgbd-sources/src/sources/stereovideo/opencv.cpp index cd9502709f9da35f39eea07a6b5e4bb3470ccb84..df3a9999d8113bdd45dacd203b9ffa1d290290fa 100644 --- a/components/rgbd-sources/src/sources/stereovideo/opencv.cpp +++ b/components/rgbd-sources/src/sources/stereovideo/opencv.cpp @@ -151,9 +151,11 @@ OpenCVDevice::~OpenCVDevice() { } static std::vector<ftl::rgbd::detail::DeviceDetails> opencv_devices; +static bool opencv_dev_init = false; std::vector<ftl::rgbd::detail::DeviceDetails> OpenCVDevice::getDevices() { - if (opencv_devices.size() > 0) return opencv_devices; + if (opencv_dev_init) return opencv_devices; + opencv_dev_init = true; std::vector<ftl::rgbd::detail::DeviceDetails> devices; diff --git a/components/rgbd-sources/src/sources/stereovideo/pylon.cpp b/components/rgbd-sources/src/sources/stereovideo/pylon.cpp index 591ed6b6bffeff48a696d9a9cac29a0af2e8a29a..cc3ac34ed6143724af34d4c95050473d59da9d59 100644 --- a/components/rgbd-sources/src/sources/stereovideo/pylon.cpp +++ b/components/rgbd-sources/src/sources/stereovideo/pylon.cpp @@ -100,7 +100,7 @@ PylonDevice::PylonDevice(nlohmann::json &config) right_hm_ = cv::cuda::HostMem(height_, width_, CV_8UC4); hres_hm_ = cv::cuda::HostMem(fullheight_, fullwidth_, CV_8UC4); - on("exposure", [this](const ftl::config::Event &e) { + on("exposure", [this]() { if (lcam_->GetDeviceInfo().GetModelName() != "Emulation") { lcam_->ExposureTime.SetValue(value("exposure", 24000.0f)); // Exposure time in microseconds } @@ -114,7 +114,13 @@ PylonDevice::~PylonDevice() { } +static std::vector<ftl::rgbd::detail::DeviceDetails> pylon_devices; +static bool pylon_dev_init = false; + std::vector<ftl::rgbd::detail::DeviceDetails> PylonDevice::listDevices() { + if (pylon_dev_init) return pylon_devices; + pylon_dev_init = true; + auto &inst = CTlFactory::GetInstance(); Pylon::DeviceInfoList_t devices; @@ -132,6 +138,7 @@ std::vector<ftl::rgbd::detail::DeviceDetails> PylonDevice::listDevices() { r.maxwidth = 0; } + pylon_devices = results; return results; } diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp index 9b67820a9467b1dec1a013122088bfe863626371..e0385eb5cc646b700d85225f56943ebc39f1a87e 100644 --- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp +++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp @@ -175,16 +175,16 @@ void StereoVideoSource::init(const string &file) { LOG(INFO) << "StereoVideo source ready..."; ready_ = true; - host_->on("size", [this](const ftl::config::Event &e) { + host_->on("size", [this]() { do_update_params_ = true; }); - host_->on("rectify", [this](const ftl::config::Event &e) { + host_->on("rectify", [this]() { calibration_.enabled = host_->value("rectify", true); do_update_params_ = true; }); - host_->on("offset_z", [this](const ftl::config::Event &e) { + host_->on("offset_z", [this]() { do_update_params_ = true; }); } diff --git a/components/streams/include/ftl/streams/builder.hpp b/components/streams/include/ftl/streams/builder.hpp index 844e7123bd21e092aeb6a6219a5633f69d34601a..c13e979014b57d48a288077cf27af658afe00840 100644 --- a/components/streams/include/ftl/streams/builder.hpp +++ b/components/streams/include/ftl/streams/builder.hpp @@ -119,8 +119,12 @@ class ManualSourceBuilder : public LocalBuilder { void tick(); + inline void setFrameRate(int fps) { mspf_ = 1000/fps; }; + private: ftl::data::DiscreteSource *src_; + int mspf_ = 30; + int64_t last_timestamp_=0; }; class ForeignBuilder : public BaseBuilder { diff --git a/components/streams/src/builder.cpp b/components/streams/src/builder.cpp index ab0991a5367973c33cf0bcf9a491a5b6ccb25297..7dc8400c239c672c2eb287c090026a9887aa6aee 100644 --- a/components/streams/src/builder.cpp +++ b/components/streams/src/builder.cpp @@ -171,6 +171,9 @@ void ManualSourceBuilder::tick() { if (!src_) return; int64_t ts = ftl::timer::get_time(); + if (ts < last_timestamp_ + mspf_) return; + last_timestamp_ = ts; + src_->capture(ts); auto fs = getNextFrameSet(ts); diff --git a/components/streams/src/feed.cpp b/components/streams/src/feed.cpp index 7dad368a14ee4828fe96ebbaf03f5cb5c50c355c..cd168145965193a63abda763ef2a4ae7b917b492 100644 --- a/components/streams/src/feed.cpp +++ b/components/streams/src/feed.cpp @@ -144,13 +144,13 @@ Feed::Feed(nlohmann::json &config, ftl::net::Universe*net) : // FIXME: Find better option that waiting here. // Wait to make sure streams have started properly. std::this_thread::sleep_for(std::chrono::milliseconds(100)); - UNIQUE_LOCK(mtx_, lk); + //UNIQUE_LOCK(mtx_, lk); updateNetSources(); }); }); net_->bind("add_stream", [this](std::string uri){ - UNIQUE_LOCK(mtx_, lk); + //UNIQUE_LOCK(mtx_, lk); updateNetSources(); }); @@ -425,9 +425,12 @@ void Feed::removeFilter(Feed::Filter* filter) { } void Feed::updateNetSources() { - netcams_ = + auto netcams = net_->findAll<std::string>("list_streams"); + UNIQUE_LOCK(mtx_, lk); + netcams_ = std::move(netcams); + if (value("auto_host_sources", false)) { for (auto s : netcams_) { ftl::URI uri(s); @@ -447,7 +450,7 @@ void Feed::updateNetSources() { << stream->value("uri", std::string("NONE")) << " (" << fsid << ")"; - cv_net_connect_.notify_one(); + //cv_net_connect_.notify_one(); } else { LOG(INFO) << "Stream exists: " << s; @@ -523,9 +526,11 @@ std::string Feed::getName(const std::string &puri) { auto *cfgble = ftl::config::find(puri); if (cfgble) { auto &j = cfgble->getConfig(); - return (j.is_structured()) ? j.value("name", j.value("uri", uri.getPathSegment(-1))) : uri.getPathSegment(-1); + std::string name = (j.is_structured()) ? j.value("name", j.value("uri", uri.getPathSegment(-1))) : uri.getPathSegment(-1); + return (name.size() == 0) ? uri.getHost() : name; } else { - return uri.getPathSegment(-1); + std::string name = uri.getPathSegment(-1); + return (name.size() == 0) ? uri.getHost() : name; } /*auto n = net_->findOne<std::string>("get_cfg", puri); if (n) { @@ -638,6 +643,9 @@ uint32_t Feed::add(const std::string &path) { // Create local builder instance auto *creator = new ftl::streams::ManualSourceBuilder(pool_.get(), fsid, source); + if (uri.getBaseURI() == "device::openvr") creator->setFrameRate(1000); + else creator->setFrameRate(30); + std::shared_ptr<ftl::streams::BaseBuilder> creatorptr(creator); lk.lock(); receiver_->registerBuilder(creatorptr); @@ -674,7 +682,7 @@ uint32_t Feed::add(const std::string &path) { auto &host_details = known_hosts[uri.getBaseURIWithUser()]; host_details["last_open"] = ftl::timer::get_time(); - net_->connect(path)->waitConnection(); + ftl::pool.push([this,path](int id) { net_->connect(path); }); } else if (scheme == ftl::URI::SCHEME_FTL) { @@ -791,9 +799,19 @@ void Feed::startStreaming(Filter *f, const std::string &filename) { } void Feed::startStreaming(Filter *f) { - if (_isRecording()) throw FTL_Error("Already recording, cannot live stream"); + { + UNIQUE_LOCK(mtx_, lk); + if (_isRecording()) throw FTL_Error("Already recording, cannot live stream"); - // TODO: Allow net streaming + record_filter_ = f; + + auto *nstream = ftl::create<ftl::stream::Net>(this, "live_stream", net_); + nstream->set("uri", value("uri", std::string("ftl://vision.utu.fi/live"))); + record_stream_->add(nstream); + record_stream_->begin(); + recorder_->resetSender(); + } + _beginRecord(f); } void Feed::_beginRecord(Filter *f) { diff --git a/components/streams/src/filestream.cpp b/components/streams/src/filestream.cpp index 90cf549ab31bfe989df6c9b3653527a0ec31b8ad..b887e028a0a1895d351ebb7052c979f1dfd7f131 100644 --- a/components/streams/src/filestream.cpp +++ b/components/streams/src/filestream.cpp @@ -17,7 +17,7 @@ File::File(nlohmann::json &config) : Stream(config), ostream_(nullptr), istream_ checked_ = false; save_data_ = value("save_data", false); - on("save_data", [this](const ftl::config::Event &e) { + on("save_data", [this]() { save_data_ = value("save_data", false); }); } @@ -35,7 +35,7 @@ File::File(nlohmann::json &config, std::ofstream *os) : Stream(config), ostream_ checked_ = false; save_data_ = value("save_data", false); - on("save_data", [this](const ftl::config::Event &e) { + on("save_data", [this]() { save_data_ = value("save_data", false); }); } diff --git a/components/streams/src/receiver.cpp b/components/streams/src/receiver.cpp index 20067b95525c7dbea313eff3653933de9e2fe72e..b1bd910450b26094b271af903859857689634a5c 100644 --- a/components/streams/src/receiver.cpp +++ b/components/streams/src/receiver.cpp @@ -44,7 +44,7 @@ Receiver::Receiver(nlohmann::json &config, ftl::data::Pool *p) : ftl::Configurab } });*/ - on("frame_mask", [this](const ftl::config::Event &e) { + on("frame_mask", [this]() { frame_mask_ = value("frame_mask", 0xFFFFFFFFu); }); } diff --git a/components/streams/src/renderer.cpp b/components/streams/src/renderer.cpp index b7716965a53d2add52d71a65b02ddda03770aac6..5da8355ed26927dbf3b9839e5359ed2dea4198fb 100644 --- a/components/streams/src/renderer.cpp +++ b/components/streams/src/renderer.cpp @@ -17,7 +17,7 @@ Source::Source(nlohmann::json &config, ftl::stream::Feed *feed) : ftl::Configurable(config), feed_(feed), impl_(nullptr) { reset(); - on("uri", [this](const ftl::config::Event &e) { + on("uri", [this]() { LOG(INFO) << "URI change for renderer: " << getURI(); reset(); }); diff --git a/components/streams/src/renderers/openvr_render.cpp b/components/streams/src/renderers/openvr_render.cpp index 5c515e7c15068ff4acaf16069670bcae8b4cc0b7..fade788777e9f5f43cee4c35c10cfc35212c5141 100644 --- a/components/streams/src/renderers/openvr_render.cpp +++ b/components/streams/src/renderers/openvr_render.cpp @@ -48,7 +48,7 @@ OpenVRRender::OpenVRRender(ftl::render::Source *host, ftl::stream::Feed *feed) filter_ = feed_->filter({Channel::Colour, Channel::Depth}); } - host_->on("source", [this](const ftl::config::Event &e) { + host_->on("source", [this]() { std::string source = host_->value("source", std::string("")); if (source.size() > 0) { diff --git a/components/streams/src/renderers/screen_render.cpp b/components/streams/src/renderers/screen_render.cpp index 9a1728c139621868912bb233f8073e5d8007382f..b59860b003fd21cac69d5e495abf9a4cc4154a80 100644 --- a/components/streams/src/renderers/screen_render.cpp +++ b/components/streams/src/renderers/screen_render.cpp @@ -31,6 +31,11 @@ ScreenRender::ScreenRender(ftl::render::Source *host, ftl::stream::Feed *feed) ); intrinsics_ = ftl::create<ftl::Configurable>(host_, "intrinsics"); + refresh_calibration_ = false; + + intrinsics_->onAny({"focal","width","height"}, [this]() { + refresh_calibration_ = true; + }); filter_ = nullptr; std::string source = host_->value("source", std::string("")); @@ -41,7 +46,7 @@ ScreenRender::ScreenRender(ftl::render::Source *host, ftl::stream::Feed *feed) filter_ = feed_->filter({Channel::Colour, Channel::Depth}); } - host_->on("source", [this](const ftl::config::Event &e) { + host_->on("source", [this]() { std::string source = host_->value("source", std::string("")); if (source.size() > 0) { @@ -78,7 +83,8 @@ bool ScreenRender::retrieve(ftl::data::Frame &frame_out) { if (sets.size() > 0) { ftl::rgbd::Frame &rgbdframe = frame_out.cast<ftl::rgbd::Frame>(); - if (!frame_out.has(Channel::Calibration)) { + if (!frame_out.has(Channel::Calibration) || refresh_calibration_) { + refresh_calibration_ = false; rgbdframe.setLeft() = ftl::rgbd::Camera::from(intrinsics_); if (!frame_out.has(Channel::Capabilities)) { diff --git a/components/streams/src/renderers/screen_render.hpp b/components/streams/src/renderers/screen_render.hpp index 084b096fe5875c2a297b7a0458ad19e5fc0c9f53..4de8e9609f54847dd9c41285f1e3dc12b7f2a355 100644 --- a/components/streams/src/renderers/screen_render.hpp +++ b/components/streams/src/renderers/screen_render.hpp @@ -34,6 +34,7 @@ class ScreenRender : public ftl::render::BaseSourceImpl { ftl::Configurable *intrinsics_; uint32_t my_id_; ftl::operators::Graph *post_pipe_; + bool refresh_calibration_; }; } diff --git a/components/streams/src/sender.cpp b/components/streams/src/sender.cpp index 7811101be3ddc663fb9e8c23c0e5ed1713122e29..b6b807080180cc046a4900c2309f7a47fd891624 100644 --- a/components/streams/src/sender.cpp +++ b/components/streams/src/sender.cpp @@ -29,7 +29,7 @@ Sender::Sender(nlohmann::json &config) : ftl::Configurable(config), stream_(null add_iframes_ = value("iframes", 50); timestamp_ = -1; - on("iframes", [this](const ftl::config::Event &e) { + on("iframes", [this]() { add_iframes_ = value("iframes", 50); }); }