diff --git a/applications/gui/CMakeLists.txt b/applications/gui/CMakeLists.txt
index b2dbd004f25c03332a40efe988c664af0d7d225a..cac36513e264f7176daf75eb75d5c1a8e9fb5839 100644
--- a/applications/gui/CMakeLists.txt
+++ b/applications/gui/CMakeLists.txt
@@ -14,6 +14,7 @@ set(GUISRC
src/media_panel.cpp
src/thumbview.cpp
src/record_window.cpp
+ src/frameset_mgr.cpp
)
if (HAVE_OPENVR)
diff --git a/applications/gui/src/frameset_mgr.cpp b/applications/gui/src/frameset_mgr.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..70a0b226fbe3ceb427b6303e30a523da84bc6661
--- /dev/null
+++ b/applications/gui/src/frameset_mgr.cpp
@@ -0,0 +1,9 @@
+#include "frameset_mgr.hpp"
+#include <ftl/uri.hpp>
+
+static int frameset_counter = 0;
+
+int ftl::gui::mapToFrameset(const std::string &uri) {
+ ftl::URI u(uri);
+ return frameset_counter++;
+}
diff --git a/applications/gui/src/frameset_mgr.hpp b/applications/gui/src/frameset_mgr.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5412fd1d4bdb65030177e5948d3861d5b1eeb0a
--- /dev/null
+++ b/applications/gui/src/frameset_mgr.hpp
@@ -0,0 +1,17 @@
+#ifndef _FTL_GUI_FRAMESET_MANAGER_HPP_
+#define _FTL_GUI_FRAMESET_MANAGER_HPP_
+
+#include <string>
+
+namespace ftl {
+namespace gui {
+
+/**
+ * Given a stream URI, allocate a frameset number to that stream.
+ */
+int mapToFrameset(const std::string &uri);
+
+}
+}
+
+#endif
diff --git a/applications/gui/src/src_window.cpp b/applications/gui/src/src_window.cpp
index 105a37d546153ec7b405f6bc5fcfd43cff9bd077..88700d146dd7a1dbd7486e83d1f5038f94d2f306 100644
--- a/applications/gui/src/src_window.cpp
+++ b/applications/gui/src/src_window.cpp
@@ -3,6 +3,7 @@
#include "screen.hpp"
#include "camera.hpp"
#include "scene.hpp"
+#include "frameset_mgr.hpp"
#include <ftl/profiler.hpp>
@@ -32,6 +33,7 @@
#include <ftl/operators/weighting.hpp>
#include <ftl/operators/mvmls.hpp>
#include <ftl/operators/clipping.hpp>
+#include <ftl/operators/poser.hpp>
#include <nlohmann/json.hpp>
@@ -87,8 +89,13 @@ SourceWindow::SourceWindow(ftl::gui::Screen *screen)
nanogui::Alignment::Middle, 0, 5));
screen->net()->onConnect([this](ftl::net::Peer *p) {
- UNIQUE_LOCK(mutex_, lk);
- _updateCameras(screen_->net()->findAll<string>("list_streams"));
+ ftl::pool.push([this](int id) {
+ // 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(mutex_, lk);
+ _updateCameras(screen_->net()->findAll<string>("list_streams"));
+ });
});
UNIQUE_LOCK(mutex_, lk);
@@ -140,7 +147,6 @@ SourceWindow::SourceWindow(ftl::gui::Screen *screen)
// Check paths for FTL files to load.
auto paths = (*screen->root()->get<nlohmann::json>("paths"));
- int ftl_count = available_.size();
for (auto &x : paths.items()) {
std::string path = x.value().get<std::string>();
auto eix = path.find_last_of('.');
@@ -149,9 +155,11 @@ SourceWindow::SourceWindow(ftl::gui::Screen *screen)
// Command line path is ftl file
if (ext == "ftl") {
LOG(INFO) << "Found FTL file: " << path;
- auto *fstream = ftl::create<ftl::stream::File>(screen->root(), std::string("ftlfile-")+std::to_string(ftl_count+1));
+ int fsid = ftl::gui::mapToFrameset(path);
+ auto *fstream = ftl::create<ftl::stream::File>(screen->root(), std::string("ftlfile-")+std::to_string(fsid));
fstream->set("filename", path);
- stream_->add(fstream, ftl_count++);
+ available_[path] = fstream;
+ stream_->add(fstream, fsid);
} else if (path.rfind("device:", 0) == 0) {
ftl::URI uri(path);
uri.to_json(screen->root()->getConfig()["sources"].emplace_back());
@@ -169,8 +177,10 @@ SourceWindow::SourceWindow(ftl::gui::Screen *screen)
if (screen->root()->getConfig()["sources"].size() > 0) {
devices = ftl::createArray<Source>(screen->root(), "sources", screen->control()->getNet());
auto *gen = createSourceGenerator(screen->root(), devices);
- gen->onFrameSet([this, ftl_count](ftl::rgbd::FrameSet &fs) {
- fs.id = ftl_count; // Set a frameset id to something unique.
+ int fsid = ftl::gui::mapToFrameset(screen->root()->getID());
+
+ gen->onFrameSet([this, fsid](ftl::rgbd::FrameSet &fs) {
+ fs.id = fsid; // Set a frameset id to something unique.
return _processFrameset(fs, false);
});
}
@@ -254,6 +264,7 @@ void SourceWindow::_checkFrameSets(int id) {
p->append<ftl::operators::BorderMask>("border_mask");
p->append<ftl::operators::CullDiscontinuity>("remove_discontinuity");
p->append<ftl::operators::MultiViewMLS>("mvmls")->value("enabled", false);
+ p->append<ftl::operators::Poser>("poser")->value("enabled", true);
pre_pipelines_.push_back(p);
framesets_.push_back(new ftl::rgbd::FrameSet);
@@ -329,17 +340,19 @@ std::vector<ftl::gui::Camera*> SourceWindow::getCameras() {
void SourceWindow::_updateCameras(const vector<string> &netcams) {
if (netcams.size() == 0) return;
+ int ncount = 0;
for (auto s : netcams) {
- LOG(INFO) << "ADDING CAMERA: " << s;
-
if (available_.count(s) == 0) {
- auto *stream = ftl::create<ftl::stream::Net>(screen_->root(), string("netstream")+std::to_string(available_.size()-1), screen_->net());
+ auto *stream = ftl::create<ftl::stream::Net>(screen_->root(), string("netstream")+std::to_string(available_.size()), screen_->net());
available_[s] = stream;
stream->set("uri", s);
- bool isspecial = (stream->get<std::string>("uri") == screen_->root()->value("data_stream",std::string("")));
- stream_->add(stream, (isspecial) ? 1 : 0);
+ int fsid = ftl::gui::mapToFrameset(s);
+ stream_->add(stream, fsid);
- LOG(INFO) << "Add Stream: " << stream->value("uri", std::string("NONE"));
+ LOG(INFO) << "Add Stream: " << stream->value("uri", std::string("NONE")) << " (" << fsid << ")";
+ ++ncount;
+ } else {
+ LOG(INFO) << "Stream exists: " << s;
}
// FIXME: Check for already existing...
@@ -354,7 +367,7 @@ void SourceWindow::_updateCameras(const vector<string> &netcams) {
}
//stream_->reset();
- stream_->begin();
+ if (ncount > 0) stream_->begin();
//std::vector<ftl::stream::Net*> strms = ftl::createArray<ftl::stream::Net>(screen_->root(), "streams", screen_->net());
diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index c713a56a8ca6e35896af803da53f9ecaf0a9887a..786596f798ece5282d721482fdea8ad9a3b4d05a 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -130,6 +130,13 @@ static void run(ftl::Configurable *root) {
sender->setStream(outstream);
ftl::audio::Source *audioSrc = nullptr;
+ // TODO: Temporary reconstruction local audio source for testing
+ audioSrc = ftl::create<ftl::audio::Source>(root, "audio_test");
+
+ audioSrc->onFrameSet([sender](ftl::audio::FrameSet &fs) {
+ sender->post(fs);
+ return true;
+ });
std::vector<Source*> sources;
// Create a vector of all input RGB-Depth sources
@@ -183,14 +190,6 @@ static void run(ftl::Configurable *root) {
});
groups.push_back(reconstr);
++i;
-
- // TODO: Temporary reconstruction local audio source for testing
- audioSrc = ftl::create<ftl::audio::Source>(root, "audio_test");
-
- audioSrc->onFrameSet([sender](ftl::audio::FrameSet &fs) {
- sender->post(fs);
- return true;
- });
} else {
ftl::URI uri(path);
if (uri.getScheme() == ftl::URI::SCHEME_TCP || uri.getScheme() == ftl::URI::SCHEME_WS) {
@@ -218,7 +217,7 @@ static void run(ftl::Configurable *root) {
int count = 0;
for (auto &s : stream_uris) {
LOG(INFO) << " --- found stream: " << s;
- auto *nstream = ftl::create<ftl::stream::Net>(stream, std::to_string(count), net);
+ auto *nstream = ftl::create<ftl::stream::Net>(stream, std::string("netstream")+std::to_string(count), net);
nstream->set("uri", s);
stream->add(nstream);
++count;
diff --git a/applications/vision/src/main.cpp b/applications/vision/src/main.cpp
index 07e6b4ae547bf7ec960d0d01ac74285f840d4595..e6086797379c8be213b65f4a9505b04d6f437681 100644
--- a/applications/vision/src/main.cpp
+++ b/applications/vision/src/main.cpp
@@ -63,7 +63,12 @@ static void run(ftl::Configurable *root) {
Source *source = nullptr;
source = ftl::create<Source>(root, "source", net);
- if (file != "") source->set("uri", file);
+ if (file != "") {
+ //source->set("uri", file);
+ ftl::URI uri(file);
+ uri.to_json(source->getConfig());
+ source->set("uri", uri.getBaseURI());
+ }
ftl::stream::Sender *sender = ftl::create<ftl::stream::Sender>(root, "sender");
ftl::stream::Net *outstream = ftl::create<ftl::stream::Net>(root, "stream", net);
diff --git a/components/codecs/include/ftl/codecs/shapes.hpp b/components/codecs/include/ftl/codecs/shapes.hpp
index ff5b189b31fc3c9d3f94d524a97e40a12606b37e..3fffaee3e209000733bba5f94f4cf84f01cce87a 100644
--- a/components/codecs/include/ftl/codecs/shapes.hpp
+++ b/components/codecs/include/ftl/codecs/shapes.hpp
@@ -16,7 +16,8 @@ enum class Shape3DType {
HEAD,
CLIPPING,
CAMERA,
- FEATURE
+ FEATURE,
+ ARUCO
};
struct Shape3D {
diff --git a/components/operators/CMakeLists.txt b/components/operators/CMakeLists.txt
index c2e7b791df51bdf57bedf01e9fdeb312461cdaee..ec80792076b0c4c4ff020e586d0f718b11926233 100644
--- a/components/operators/CMakeLists.txt
+++ b/components/operators/CMakeLists.txt
@@ -30,6 +30,7 @@ set(OPERSRC
src/aruco.cpp
src/weighting.cpp
src/weighting.cu
+ src/poser.cpp
)
if (LIBSGM_FOUND)
diff --git a/components/operators/include/ftl/operators/poser.hpp b/components/operators/include/ftl/operators/poser.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..2b11bf9a9d55f4d060f8fe0c48c6fc1a0f9fd070
--- /dev/null
+++ b/components/operators/include/ftl/operators/poser.hpp
@@ -0,0 +1,37 @@
+#ifndef _FTL_OPERATORS_POSER_HPP_
+#define _FTL_OPERATORS_POSER_HPP_
+
+#include <ftl/operators/operator.hpp>
+#include <ftl/cuda_common.hpp>
+
+#include <unordered_map>
+
+namespace ftl {
+namespace operators {
+
+/**
+ * Cache and apply poses from and to framesets.
+ */
+class Poser : public ftl::operators::Operator {
+ public:
+ explicit Poser(ftl::Configurable*);
+ ~Poser();
+
+ inline Operator::Type type() const override { return Operator::Type::ManyToMany; }
+
+ bool apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) override;
+
+ private:
+ struct PoseState {
+ Eigen::Matrix4d pose;
+ bool locked;
+ };
+
+ static std::unordered_map<std::string,PoseState> pose_db__;
+
+};
+
+}
+}
+
+#endif
diff --git a/components/operators/src/aruco.cpp b/components/operators/src/aruco.cpp
index b8f54eb4c254fcca166b2c2f48cc46f360a1adf5..2608ca1bbd6f881fc5c316c2332f36e80fed5a5a 100644
--- a/components/operators/src/aruco.cpp
+++ b/components/operators/src/aruco.cpp
@@ -1,5 +1,9 @@
#include "ftl/operators/detectandtrack.hpp"
-#include "ftl/codecs/transformation.hpp"
+#include "ftl/codecs/shapes.hpp"
+
+#include <Eigen/Eigen>
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/calib3d.hpp>
#define LOGURU_REPLACE_GLOG 1
#include <loguru.hpp>
@@ -8,7 +12,7 @@ using ftl::operators::ArUco;
using ftl::rgbd::Frame;
using ftl::codecs::Channel;
-using ftl::codecs::Transformation;
+using ftl::codecs::Shape3D;
using cv::Mat;
using cv::Point2f;
@@ -16,6 +20,23 @@ using cv::Vec3d;
using std::vector;
+static cv::Mat rmat(cv::Vec3d &rvec) {
+ cv::Mat R(cv::Size(3, 3), CV_64FC1);
+ cv::Rodrigues(rvec, R);
+ return R;
+}
+
+static Eigen::Matrix4d matrix(cv::Vec3d &rvec, cv::Vec3d &tvec) {
+ cv::Mat M = cv::Mat::eye(cv::Size(4, 4), CV_64FC1);
+ rmat(rvec).copyTo(M(cv::Rect(0, 0, 3, 3)));
+ M.at<double>(0, 3) = tvec[0];
+ M.at<double>(1, 3) = tvec[1];
+ M.at<double>(2, 3) = tvec[2];
+ Eigen::Matrix4d r;
+ cv::cv2eigen(M,r);
+ return r;
+}
+
ArUco::ArUco(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) {
dictionary_ = cv::aruco::getPredefinedDictionary(cfg->value("dictionary", 0));
params_ = cv::aruco::DetectorParameters::create();
@@ -32,7 +53,7 @@ ArUco::ArUco(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) {
//}
channel_in_ = Channel::Colour;
- channel_out_ = Channel::Transforms;
+ channel_out_ = Channel::Shapes3D;
cfg->on("dictionary", [this,cfg](const ftl::config::Event &e) {
dictionary_ = cv::aruco::getPredefinedDictionary(cfg->value("dictionary", 0));
@@ -77,10 +98,19 @@ bool ArUco::apply(Frame &in, Frame &out, cudaStream_t stream) {
cv::aruco::estimatePoseSingleMarkers(corners, marker_size_, K, dist, rvecs, tvecs);
}
- vector<Transformation> result;
+ vector<Shape3D> result;
+ if (out.hasChannel(channel_out_)) {
+ out.get(channel_out_, result);
+ }
+
for (size_t i = 0; i < rvecs.size(); i++) {
if (estimate_pose_) {
- result.push_back(ftl::codecs::Transformation(ids[i], rvecs[i], tvecs[i]));
+ auto &t = result.emplace_back();
+ t.id = ids[i];
+ t.type = ftl::codecs::Shape3DType::ARUCO;
+ t.pose = (in.getPose() * matrix(rvecs[i], tvecs[i])).cast<float>();
+ t.size = Eigen::Vector3f(0.1f,0.1f,0.1f);
+ t.label = "Aruco";
}
}
diff --git a/components/operators/src/clipping.cpp b/components/operators/src/clipping.cpp
index 97a28296ed06a0b520886d2a3c78fba627b6ffbf..9d392b4f93e8be872a9cfce578662f15e56fbb92 100644
--- a/components/operators/src/clipping.cpp
+++ b/components/operators/src/clipping.cpp
@@ -3,6 +3,9 @@
#include <ftl/utility/matrix_conversion.hpp>
#include <ftl/codecs/shapes.hpp>
+#define LOGURU_REPLACE_GLOG 1
+#include <loguru.hpp>
+
using ftl::operators::ClipScene;
using ftl::codecs::Channel;
using ftl::rgbd::Format;
@@ -56,7 +59,10 @@ bool ClipScene::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStr
bool no_clip = config()->value("no_clip", false);
std::vector<ftl::codecs::Shape3D> shapes;
- if (in.hasChannel(Channel::Shapes3D)) in.get(Channel::Shapes3D, shapes);
+ if (in.hasChannel(Channel::Shapes3D)) {
+ in.get(Channel::Shapes3D, shapes);
+ LOG(INFO) << "Already has shapes: " << shapes.size();
+ }
shapes.push_back(shape);
in.create(Channel::Shapes3D, shapes);
diff --git a/components/operators/src/colours.cpp b/components/operators/src/colours.cpp
index bdcbeedbdf0a0a9b72e901633bceb5c5ceb13504..2c102482ec3738a649a57a618b867388d209f0e4 100644
--- a/components/operators/src/colours.cpp
+++ b/components/operators/src/colours.cpp
@@ -65,7 +65,7 @@ bool ColourChannels::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, cudaStre
//cv::cuda::resize(right, rbuf_, depth.size(), 0.0, 0.0, cv::INTER_LINEAR, cvstream);
//cv::cuda::swap(right, rbuf_);
- throw FTL_Error("Depth and colour channels and different resolutions: " << depth.rows << " vs " << col.rows);
+ throw FTL_Error("Depth and colour channels and different resolutions: " << depth.size() << " vs " << right.size());
}
}
}
diff --git a/components/operators/src/poser.cpp b/components/operators/src/poser.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..588da753586fac035e0667156f1e93f76225081d
--- /dev/null
+++ b/components/operators/src/poser.cpp
@@ -0,0 +1,79 @@
+#include <ftl/operators/poser.hpp>
+#include <ftl/codecs/shapes.hpp>
+
+#define LOGURU_REPLACE_GLOG 1
+#include <loguru.hpp>
+
+using ftl::operators::Poser;
+using ftl::codecs::Channel;
+using ftl::codecs::Shape3DType;
+using std::string;
+
+std::unordered_map<std::string,ftl::operators::Poser::PoseState> Poser::pose_db__;
+
+Poser::Poser(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) {
+
+}
+
+Poser::~Poser() {
+
+}
+
+bool Poser::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) {
+ if (in.hasChannel(Channel::Shapes3D)) {
+ std::vector<ftl::codecs::Shape3D> transforms;
+ in.get(Channel::Shapes3D, transforms);
+
+ for (auto &t : transforms) {
+ //LOG(INFO) << "Have FS transform: " << t.id;
+ }
+ }
+
+ for (size_t i=0; i<in.frames.size(); ++i) {
+ if (in.hasFrame(i)) {
+ auto &f = in.frames[i];
+
+ if (f.hasChannel(Channel::Shapes3D)) {
+ std::vector<ftl::codecs::Shape3D> transforms;
+ f.get(Channel::Shapes3D, transforms);
+
+ for (auto &t : transforms) {
+ std::string idstr;
+ switch(t.type) {
+ case Shape3DType::ARUCO : idstr = "aruco-"; break;
+ default : idstr = "unk-"; break;
+ }
+
+ idstr += std::to_string(in.id) + string("-") + std::to_string(i) + string("-") + std::to_string(t.id);
+
+ LOG(INFO) << "POSE ID: " << idstr;
+ auto pose = t.pose.cast<double>(); // f.getPose() *
+
+ auto p = pose_db__.find(idstr);
+ if (p == pose_db__.end()) {
+ ftl::operators::Poser::PoseState ps;
+ ps.pose = pose;
+ ps.locked = false;
+ pose_db__.emplace(std::make_pair(idstr,ps));
+ } else {
+ // TODO: Merge poses
+ if (!(*p).second.locked) (*p).second.pose = pose;
+ }
+ }
+ }
+ }
+ }
+
+ string pose_ident = config()->value("pose_ident",string("default"));
+ if (pose_ident != "default") {
+ auto p = pose_db__.find(pose_ident);
+ if (p != pose_db__.end()) {
+ (*p).second.locked = config()->value("locked",false);
+ in.pose = (*p).second.pose;
+ } else {
+ LOG(WARNING) << "Pose not found: " << pose_ident;
+ }
+ }
+
+ return true;
+}
\ No newline at end of file
diff --git a/components/renderers/cpp/src/CUDARender.cpp b/components/renderers/cpp/src/CUDARender.cpp
index 73b078b140f3f0db140729017b66288fa77ab623..a3fb4cdd221d1852046011bea4edbec4848307b5 100644
--- a/components/renderers/cpp/src/CUDARender.cpp
+++ b/components/renderers/cpp/src/CUDARender.cpp
@@ -134,7 +134,7 @@ void CUDARender::_renderChannel(ftl::rgbd::Frame &output, ftl::codecs::Channel i
auto &texture = colouriser_->colourise(f, in, stream);
auto transform = MatrixConversion::toCUDA(f.getPose().cast<float>().inverse() * t.cast<float>().inverse()) * poseInverse_;
- auto transformR = MatrixConversion::toCUDA(f.getPose().cast<float>().inverse()).getFloat3x3();
+ auto transformR = MatrixConversion::toCUDA(f.getPose().cast<float>().inverse() * t.cast<float>().inverse()).getFloat3x3();
if (f.hasChannel(Channel::Depth)) {
ftl::cuda::reproject(
@@ -571,7 +571,7 @@ bool CUDARender::submit(ftl::rgbd::FrameSet *in, Channels<0> chans, const Eigen:
bool success = true;
try {
- _renderPass1(t);
+ _renderPass1(in->pose);
//cudaSafeCall(cudaStreamSynchronize(stream_));
} catch (std::exception &e) {
LOG(ERROR) << "Exception in render: " << e.what();
@@ -581,7 +581,7 @@ bool CUDARender::submit(ftl::rgbd::FrameSet *in, Channels<0> chans, const Eigen:
auto &s = sets_.emplace_back();
s.fs = in;
s.channels = chans;
- s.transform = t;
+ s.transform = in->pose;
last_frame_ = scene_->timestamp;
scene_ = nullptr;
diff --git a/components/renderers/cpp/src/overlay.cpp b/components/renderers/cpp/src/overlay.cpp
index 9b3328b336a410c29758f3cdec54e23323fde8ef..78e6645c7ce6c79ba1dd83156f502ba47b608a11 100644
--- a/components/renderers/cpp/src/overlay.cpp
+++ b/components/renderers/cpp/src/overlay.cpp
@@ -335,9 +335,17 @@ void Overlay::draw(ftl::rgbd::FrameSet &fs, ftl::rgbd::FrameState &state, const
fs.frames[i].get(Channel::Shapes3D, shapes);
for (auto &s : shapes) {
- auto pose = s.pose.cast<double>().inverse() * state.getPose();
- Eigen::Vector4d pos = pose.inverse() * Eigen::Vector4d(0,0,0,1);
- pos /= pos[3];
+ auto pose = s.pose.cast<double>(); //.inverse() * state.getPose();
+ //Eigen::Vector4d pos = pose.inverse() * Eigen::Vector4d(0,0,0,1);
+ //pos /= pos[3];
+
+ Eigen::Vector3f scale(s.size[0]/2.0f, s.size[1]/2.0f, s.size[2]/2.0f);
+
+ switch (s.type) {
+ case ftl::codecs::Shape3DType::CLIPPING: _drawOutlinedShape(Shape::BOX, state.getPose().inverse() * pose, scale, make_uchar4(255,0,255,80), make_uchar4(255,0,255,255)); break;
+ case ftl::codecs::Shape3DType::ARUCO: _drawAxis(state.getPose().inverse() * pose, Eigen::Vector3f(0.2f, 0.2f, 0.2f)); break;
+ default: break;
+ }
//ftl::overlay::drawBox(state.getLeft(), out, over_depth_, pose, cv::Scalar(0,0,255,100), s.size.cast<double>());
//ftl::overlay::drawText(state.getLeft(), out, over_depth_, s.label, pos, 0.5, cv::Scalar(0,0,255,100));
diff --git a/components/rgbd-sources/src/sources/screencapture/screencapture.cpp b/components/rgbd-sources/src/sources/screencapture/screencapture.cpp
index e96f675ba6a42bd83d221b9ff249cbfc7019bb52..2b56065fe767d51222b4b6d5a43320ef06006b30 100644
--- a/components/rgbd-sources/src/sources/screencapture/screencapture.cpp
+++ b/components/rgbd-sources/src/sources/screencapture/screencapture.cpp
@@ -4,6 +4,9 @@
#include <loguru.hpp>
#include <ftl/threads.hpp>
#include <ftl/rgbd/source.hpp>
+#include <opencv2/calib3d.hpp>
+#include <Eigen/Eigen>
+#include <opencv2/core/eigen.hpp>
using ftl::rgbd::detail::ScreenCapture;
using ftl::codecs::Channel;
@@ -33,6 +36,24 @@ struct X11State {
}
#endif
+static cv::Mat rmat(const cv::Vec3d &rvec) {
+ cv::Mat R(cv::Size(3, 3), CV_64FC1);
+ cv::Rodrigues(rvec, R);
+ return R;
+}
+
+static Eigen::Matrix4d matrix(const cv::Vec3d &rvec, const cv::Vec3d &tvec) {
+ cv::Mat M = cv::Mat::eye(cv::Size(4, 4), CV_64FC1);
+ rmat(rvec).copyTo(M(cv::Rect(0, 0, 3, 3)));
+ M.at<double>(0, 3) = tvec[0];
+ M.at<double>(1, 3) = tvec[1];
+ M.at<double>(2, 3) = tvec[2];
+ Eigen::Matrix4d r;
+ cv::cv2eigen(M,r);
+ return r;
+}
+
+
ScreenCapture::ScreenCapture(ftl::rgbd::Source *host)
: ftl::rgbd::detail::Source(host) {
capabilities_ = kCapVideo;
@@ -106,7 +127,7 @@ ScreenCapture::ScreenCapture(ftl::rgbd::Source *host)
params_.cy = -(params_.height / 2.0);
params_.fx = 700.0;
params_.fy = 700.0;
- params_.maxDepth = host_->value("depth", 1.0f);
+ params_.maxDepth = host_->value("size", 1.0f);
params_.minDepth = 0.0f;
params_.doffs = 0.0;
params_.baseline = 0.1f;
@@ -114,9 +135,14 @@ ScreenCapture::ScreenCapture(ftl::rgbd::Source *host)
state_.getLeft() = params_;
state_.set("name", std::string("[ScreenCapture] ") + host_->value("name", host_->getID()));
- host_->on("depth", [this](const ftl::config::Event &e) {
- params_.maxDepth = host_->value("depth", 1.0f);
+ float offsetz = host_->value("offset_z",0.0f);
+ state_.setPose(matrix(cv::Vec3d(0.0, 3.14159, 0.0), cv::Vec3d(0.0,0.0,params_.maxDepth+offsetz)));
+
+ host_->on("size", [this](const ftl::config::Event &e) {
+ float offsetz = host_->value("offset_z",0.0f);
+ params_.maxDepth = host_->value("size", 1.0f);
state_.getLeft() = params_;
+ state_.setPose(matrix(cv::Vec3d(0.0, 3.14159, 0.0), cv::Vec3d(0.0,0.0,params_.maxDepth+offsetz)));
});
host_->on("offset_x", [this](const ftl::config::Event &e) {
diff --git a/components/rgbd-sources/src/sources/stereovideo/local.cpp b/components/rgbd-sources/src/sources/stereovideo/local.cpp
index 89eeca114786f77c32bf863d92388a4fef7798eb..a6f4017485756498086b84638b1ea99d112c5e60 100644
--- a/components/rgbd-sources/src/sources/stereovideo/local.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/local.cpp
@@ -110,8 +110,8 @@ LocalSource::LocalSource(nlohmann::json &config)
LOG(WARNING) << "Not able to find second camera for stereo";
}
else {
- camera_b_->set(cv::CAP_PROP_FRAME_WIDTH, value("width", 640));
- camera_b_->set(cv::CAP_PROP_FRAME_HEIGHT, value("height", 480));
+ camera_b_->set(cv::CAP_PROP_FRAME_WIDTH, value("width", 1280));
+ camera_b_->set(cv::CAP_PROP_FRAME_HEIGHT, value("height", 720));
camera_b_->set(cv::CAP_PROP_FPS, 1000 / ftl::timer::getInterval());
//camera_b_->set(cv::CAP_PROP_BUFFERSIZE, 0); // Has no effect
@@ -121,8 +121,8 @@ LocalSource::LocalSource(nlohmann::json &config)
LOG(INFO) << "Video backend: " << camera_a_->getBackendName();
LOG(INFO) << "Video defaults: " << camera_a_->get(cv::CAP_PROP_FRAME_WIDTH) << "x" << camera_a_->get(cv::CAP_PROP_FRAME_HEIGHT) << " @ " << camera_a_->get(cv::CAP_PROP_FPS);
- camera_a_->set(cv::CAP_PROP_FRAME_WIDTH, value("width", 640));
- camera_a_->set(cv::CAP_PROP_FRAME_HEIGHT, value("height", 480));
+ camera_a_->set(cv::CAP_PROP_FRAME_WIDTH, value("width", 1280));
+ camera_a_->set(cv::CAP_PROP_FRAME_HEIGHT, value("height", 720));
camera_a_->set(cv::CAP_PROP_FPS, 1000 / ftl::timer::getInterval());
//camera_a_->set(cv::CAP_PROP_BUFFERSIZE, 0); // Has no effect
diff --git a/components/streams/src/netstream.cpp b/components/streams/src/netstream.cpp
index e50b494d24928991c430665d7ac09ce245d46ac2..c1248287878b9cbe32f0e02e14a3f3c309ee2041 100644
--- a/components/streams/src/netstream.cpp
+++ b/components/streams/src/netstream.cpp
@@ -228,12 +228,16 @@ bool Net::begin() {
// TODO: Register URI as available.
host_ = true;
return true;
+ } else {
+ //LOG(INFO) << "Net cfg: " << net_->call<std::string>(*p, "get_cfg", uri_);
}
host_ = false;
peer_ = *p;
tally_ = 30*kTallyScale;
for (size_t i=0; i<reqtally_.size(); ++i) reqtally_[i] = 0;
+
+ LOG(INFO) << "SEND REQUESTS FOR: " << uri_;
// Initially send a colour request just to create the connection
_sendRequest(Channel::Colour, kAllFramesets, kAllFrames, 30, 0);