diff --git a/applications/gui2/README.md b/applications/gui2/README.md
index def2ab0c5781049ea9dab1d5e45d1c5abde5daf4..054581884b6512ffe40bd52fd628df49b67ed34c 100644
--- a/applications/gui2/README.md
+++ b/applications/gui2/README.md
@@ -7,7 +7,7 @@ General:
GUI callbacks or draw().
* Expensive processing should be moved out of gui thread (draw() and callbacks)
* Module is only required to implement Module. Each module is expected to be
- loaded only once (this design decision could be modified).
+ loaded only once.
Classes
@@ -19,33 +19,32 @@ Screen
no other references are remaining.
* Note: toolbar could be a module, but other modules likely assume it is
always available anyways.
+ * Centralized access to Nanogui::Themes and custom non-theme colors.
Module (controller)
* GUI module class wraps pointers for io, config and net. Initialization should
add necessary buttons to Screen
* Build necessary callbacks to process data from InputOutput to view.
Note: If callback passes data to view, callback handle should be owned by
- the view or Module has to keep a nanogui reference to the View.
+ the view or Module has to keep a nanogui reference to the View. Also note
+ that View destructor is called when active view is replaced.
View
- * active view will be the main window; only one view can be active at time
- * button callbacks (eg. those registered by module init) may change active view
+ * Active view will be the main window; only one view can be active at time
+ * Button callbacks (eg. those registered by module init) may change active view
* Destroyed when view is changed. Object lifetime can be used to remove
callbacks from InputOutput (TODO: only one active callback supported at the
moment)
* Implementations do not have to inherit from View. Popup/Window/Widget... can
be used to implement UI components available from any mode (config, record).
+ * Receives all unprocessed keyboard events.
InputOutput
- * Contains all relevant datastructures and objects for FTL system.
- * Network
- * Pipelines (incl. rendering) TODO
- * Audio player TODO
- * Recording TODO
- * TODO: Framset buffering/caching while in use
+ * Contains pointers to all required FTL objects (network/rendering/feed/...).
+ * Speaker
NanoGUI notes:
- * Window instances can only be deleted with dispose().
- * If sub-windows are created, they have to hold to a reference to parent
- object if they share resources (eg. in lambda callbacks
- [ref = std::move(ref)] could be used).
+ * If disposing Window in widget destructor, window->parent() reference count
+ must be checked and dispose() only called if refCount > 0. (segfault at exit)
+ * Nanogui does not dispose popup windows automatically. See above point if
+ using destructor for clean up.
diff --git a/applications/gui2/src/frameview.cpp b/applications/gui2/src/frameview.cpp
index 5de6193df9c51cd2fb3bb7b37c521bb360638a8e..4a5d161ade07dd815400917b2330465335a22c86 100644
--- a/applications/gui2/src/frameview.cpp
+++ b/applications/gui2/src/frameview.cpp
@@ -110,6 +110,21 @@ void main() {
color = colormap(texture(image, uv).x);
}
)";
+
+ const std::string GLFrameViewFragmentShaderBlend =
+R"(#version 330
+uniform sampler2D image1;
+uniform sampler2D image2;
+uniform sampler2D depthImage;
+uniform float blendAmount;
+out vec4 color;
+in vec2 uv;
+void main() {
+ // TODO: use same colorization as above? also requires gl_FragDepth
+ color = blendAmount * texture(image1, uv) + (1.0 - blendAmount) * texture(image2, uv);
+ color.w = 1.0f;
+ gl_FragDepth = texture(depthImage, uv).r;
+})";
}
void buildFrameViewShader(nanogui::GLShader &shader, std::string name, std::string fragmentShader) {
@@ -163,7 +178,9 @@ void FrameView::draw(NVGcontext *ctx) {
}
if (flush_) {
- nvgEndFrame(ctx); // Flush the NanoVG draw stack, not necessary to call nvgBeginFrame afterwards.
+ // Flush the NanoVG draw stack, not necessary to call
+ // nvgBeginFrame afterwards.
+ nvgEndFrame(ctx);
}
if (!texture.isValid()) {
diff --git a/applications/gui2/src/frameview.hpp b/applications/gui2/src/frameview.hpp
index 1b53d1511c52f86082f756d9c2a693e26178bf9c..23dda56b6d7c6bd506e1c968d4234185428507f7 100644
--- a/applications/gui2/src/frameview.hpp
+++ b/applications/gui2/src/frameview.hpp
@@ -5,9 +5,13 @@
#include <ftl/rgbd/frame.hpp>
#include <ftl/data/new_frame.hpp>
#include <ftl/data/new_frameset.hpp>
+#include <ftl/render/colouriser.hpp>
#include <opencv2/core/opengl.hpp>
+#include <ftl/render/colouriser.hpp>
+#include <ftl/render/overlay.hpp>
+
#include <nanogui/widget.h>
#include <nanogui/glutil.h>
@@ -31,7 +35,6 @@ private:
/** loads a new shader if necessary */
void buildShader(ftl::codecs::Channel c);
-
public:
FrameView(nanogui::Widget* parent);
virtual void draw(NVGcontext *ctx) override;
diff --git a/applications/gui2/src/modules/camera.cpp b/applications/gui2/src/modules/camera.cpp
index 1c7c656be0ee1e9d8cb6bd2450f23b0925d1ce88..43c83b8ec12499c93d9108070bf3fe5a0605f462 100644
--- a/applications/gui2/src/modules/camera.cpp
+++ b/applications/gui2/src/modules/camera.cpp
@@ -5,23 +5,16 @@
using ftl::gui2::Camera;
using ftl::codecs::Channel;
-void Camera::activate() {
+void Camera::init() {
- panel = new ftl::gui2::MediaPanel(screen, this);
- filter = io->feed()->filter({Channel::Left, Channel::Right, Channel::Depth});
+ colouriser_ = std::unique_ptr<ftl::render::Colouriser>(
+ ftl::create<ftl::render::Colouriser>(this, "colouriser"));
- filter->on(
- [this](const ftl::data::FrameSetPtr& fs){
- if (paused || !view) { return true; }
- std::atomic_store(&active_fs, fs);
- view->update(fs, source_idx);
- screen->redraw();
- return true;
- }
- );
+ overlay_ = std::unique_ptr<ftl::overlay::Overlay>
+ (ftl::create<ftl::overlay::Overlay>(this, "overlay"));
}
-void Camera::setSource(int idx) {
+void Camera::activate(int idx) {
source_idx = idx;
if (idx == 0) {
@@ -31,8 +24,23 @@ void Camera::setSource(int idx) {
view = new ftl::gui2::CameraView3D(screen);
}
- view->onClose([filter = this->filter, panel=this->panel](){
+ panel = new ftl::gui2::MediaPanel(screen, this);
+
+ filter = io->feed()->filter({Channel::Left, Channel::Right, Channel::Depth});
+ filter->on(
+ [this](ftl::data::FrameSetPtr fs){
+ if (paused || !view) { return true; }
+ std::atomic_store(&active_fs, fs);
+ view->fview->set(fs, source_idx, channel);
+ screen->redraw();
+ return true;
+ }
+ );
+
+ view->onClose([&filter = this->filter, panel=this->panel](){
filter->remove();
+ filter = nullptr;
+
if (panel->parent()->getRefCount() > 0) {
// segfault without this check; nanogui already deleted windows?
// should be fixed in nanogui
@@ -52,24 +60,23 @@ void Camera::setSource(int idx) {
screen->setView(view);
if (active_fs && !(*active_fs)[source_idx].hasChannel(channel)) {
- view->reset();
+ view->fview->reset();
}
if (paused && active_fs) {
- view->update(active_fs, source_idx);
+ view->fview->set(active_fs, source_idx, channel);
}
}
void Camera::setChannel(Channel c) {
channel = c;
- view->setChannel(c);
panel->setActiveChannel(channel);
if (active_fs && !(*active_fs)[source_idx].hasChannel(channel)) {
- view->reset();
+ view->fview->reset();
}
if (paused && active_fs) {
- view->update(active_fs, source_idx);
+ view->fview->set(active_fs, source_idx, channel);
}
}
diff --git a/applications/gui2/src/modules/camera.hpp b/applications/gui2/src/modules/camera.hpp
index 203478ef49262d6e234a880ac02a8e7ebca24740..c18bb6cc05b7ccadac38b281c5ab4818cf98d355 100644
--- a/applications/gui2/src/modules/camera.hpp
+++ b/applications/gui2/src/modules/camera.hpp
@@ -4,6 +4,9 @@
#include "../screen.hpp"
#include "../views/camera.hpp"
+#include <ftl/render/colouriser.hpp>
+#include <ftl/render/overlay.hpp>
+
namespace ftl {
namespace gui2 {
@@ -11,9 +14,9 @@ class Camera : public Module {
public:
using Module::Module;
- virtual void activate();
+ virtual void init() override;
- void setSource(int);
+ virtual void activate(int source_idx);
void setChannel(ftl::codecs::Channel c);
void setPaused(bool set) { paused = set; };
bool isPaused() { return paused; }
@@ -26,8 +29,12 @@ private:
ftl::stream::Feed::Filter *filter = nullptr;
ftl::data::FrameSetPtr active_fs;
+ std::unique_ptr<ftl::render::Colouriser> colouriser_;
+ std::unique_ptr<ftl::overlay::Overlay> overlay_;
+
CameraView* view = nullptr;
MediaPanel* panel = nullptr;
+
};
}
diff --git a/applications/gui2/src/modules/thumbnails.cpp b/applications/gui2/src/modules/thumbnails.cpp
index c8ed2e6c4ff7260e8a02c3ee36d38470a7144fb8..d4579f8ed9b11875918f2908778c401908ecfe53 100644
--- a/applications/gui2/src/modules/thumbnails.cpp
+++ b/applications/gui2/src/modules/thumbnails.cpp
@@ -47,6 +47,5 @@ void ThumbnailsController::show_thumbnails() {
void ThumbnailsController::show_camera(int frame_idx) {
auto* camera = screen->getModule<ftl::gui2::Camera>();
- camera->activate();
- camera->setSource(frame_idx);
+ camera->activate(frame_idx);
}
diff --git a/applications/gui2/src/screen.cpp b/applications/gui2/src/screen.cpp
index a971b3149d7e1c5d8045eb7473e5d3d3acb06936..097e1cce8023f7bacc35680dfd1c7210415fc1ee 100644
--- a/applications/gui2/src/screen.cpp
+++ b/applications/gui2/src/screen.cpp
@@ -26,7 +26,7 @@ static const int toolbar_w = 50;
static const Vector2i wsize(1280+toolbar_w,720);
Screen::Screen() :
- nanogui::Screen(Eigen::Vector2i(1024, 768), "FT-Lab Remote Presence"),
+ nanogui::Screen(wsize, "FT-Lab Remote Presence"),
toolbar_(nullptr),
active_view_(nullptr) {
@@ -44,7 +44,7 @@ Screen::Screen() :
toolbar_->setPosition({0, 0});
if (active_view_) {
active_view_->setFixedSize({max(s[0] - toolbar_->width(), 0), s[1]});
- active_view_->setPosition({toolbar_->width(), 0});
+ // active_view_->setFixedSize({s[0], s[1]});
}
performLayout();
});
@@ -92,8 +92,12 @@ void Screen::redraw() {
void Screen::setView(ftl::gui2::View *view) {
- view->setPosition(Vector2i(toolbar_->width(), 0));
- view->setFixedSize(Vector2i(width() - toolbar_->width(), height()));
+ view->setPosition({toolbar_->width(), 0});
+ view->setFixedSize({width() - toolbar_->width(), height()});
+
+ //view->setPosition({0, 0});
+ //view->setFixedSize({width(), height()});
+
view->setTheme(themes_["windowtheme"]);
view->setVisible(true);
@@ -121,6 +125,14 @@ void Screen::setView(ftl::gui2::View *view) {
active_view_ = view;
LOG(INFO) << "number of children (Screen): "<< mChildren.size();
+
+ // hide all popups (TODO: only works on toolbar at the moment)
+ for (nanogui::Widget* widget : tools_->children()) {
+ if (auto button = dynamic_cast<nanogui::PopupButton*>(widget)) {
+ button->setPushed(false);
+ }
+ }
+
performLayout();
}
diff --git a/applications/gui2/src/views/camera.cpp b/applications/gui2/src/views/camera.cpp
index 519431e8696dc34f5d50d496b5d4e51958ea0053..3b023714dcbb33623ea8f639a3d8f7cc065d7a82 100644
--- a/applications/gui2/src/views/camera.cpp
+++ b/applications/gui2/src/views/camera.cpp
@@ -16,6 +16,7 @@ using ftl::codecs::Channel;
MediaPanel::MediaPanel(nanogui::Widget *parent, ftl::gui2::Camera* ctrl) :
ftl::gui2::FixedWindow(parent, ""), ctrl_(ctrl) {
+ LOG(INFO) << __func__ << " (" << this << ")";
using namespace nanogui;
setLayout(new BoxLayout(Orientation::Horizontal,
@@ -49,10 +50,8 @@ MediaPanel::MediaPanel(nanogui::Widget *parent, ftl::gui2::Camera* ctrl) :
button_channels->setSide(Popup::Side::Right);
button_channels->setChevronIcon(ENTYPO_ICON_CHEVRON_SMALL_RIGHT);
- auto theme_popup = dynamic_cast<ftl::gui2::Screen*>(screen())->getTheme("toolbutton");
auto popup = button_channels->popup();
popup->setLayout(new GroupLayout());
- popup->setTheme(theme_popup);
for (int i=0; i < 32; ++i) {
ftl::codecs::Channel c = static_cast<ftl::codecs::Channel>(i);
@@ -66,11 +65,14 @@ MediaPanel::MediaPanel(nanogui::Widget *parent, ftl::gui2::Camera* ctrl) :
}
MediaPanel::~MediaPanel() {
+ LOG(INFO) << __func__ << " (" << this << ")";
+
auto popup = button_channels->popup();
popup->setVisible(false);
if (parent()->getRefCount() > 0 ) { // this and dispose should be patched
popup->dispose(); // in nanogui; nanogui doesn't dispose
}
+
}
void MediaPanel::draw(NVGcontext *ctx) {
@@ -123,6 +125,7 @@ void MediaPanel::setActiveChannel(Channel c) {
// ==== CameraView =============================================================
CameraView::CameraView(nanogui::Widget* parent) : View(parent) {
+ LOG(INFO) << __func__ << " (" << this << ")";
fview = new ftl::gui2::FrameView(this);
fview->setFlush(true); // buffer will be rendered as "background"
@@ -130,6 +133,7 @@ CameraView::CameraView(nanogui::Widget* parent) : View(parent) {
CameraView::~CameraView() {
// nanogui segfault at exit (screen not found, refcount already 0; bug?)
+ LOG(INFO) << __func__ << " (" << this << ")";
}
void CameraView::draw(NVGcontext *ctx) {
@@ -138,15 +142,3 @@ void CameraView::draw(NVGcontext *ctx) {
performLayout(ctx);
View::draw(ctx);
}
-
-void CameraView::update(const ftl::data::FrameSetPtr& fs, int fid) {
- fview->set(fs, fid, channel, 0, true);
-}
-
-void CameraView::reset() {
- fview->reset();
-}
-
-void CameraView::setChannel(ftl::codecs::Channel c) {
- channel = c;
-}
diff --git a/applications/gui2/src/views/camera.hpp b/applications/gui2/src/views/camera.hpp
index 46101be02560249a11e8a879288856c88468246b..e52cdeb3068f5877ae08bac1e3227f00e75a9c50 100644
--- a/applications/gui2/src/views/camera.hpp
+++ b/applications/gui2/src/views/camera.hpp
@@ -33,15 +33,8 @@ public:
~CameraView();
virtual void draw(NVGcontext *ctx) override;
- void update(const ftl::data::FrameSetPtr& fs, int fid);
- void setChannel(ftl::codecs::Channel c);
- /** reset buffer */
- void reset();
-
-protected:
ftl::gui2::FrameView *fview = nullptr;
- ftl::codecs::Channel channel = ftl::codecs::Channel::Colour;
};
}
diff --git a/applications/gui2/src/views/camera3d.cpp b/applications/gui2/src/views/camera3d.cpp
index 5a89f32a92655fd788118a9fc3a8f0d68f5c34fa..657df29b887bafb70669ee0f751e9d65d6087b4b 100644
--- a/applications/gui2/src/views/camera3d.cpp
+++ b/applications/gui2/src/views/camera3d.cpp
@@ -44,12 +44,13 @@ bool CameraView3D::keyboardEvent(int key, int scancode, int action, int modifier
//transform_ix_ = ix-1;
}
+ LOG(INFO) << "New pose: \n" << getUpdatedPose();
return true;
}
bool CameraView3D::mouseButtonEvent(const Eigen::Vector2i &p, int button, bool down, int modifiers) {
LOG(INFO) << "mouseButtonEvent: " << p;
- return true;
+ return false;
}
bool CameraView3D::mouseMotionEvent(const Eigen::Vector2i &p, const Eigen::Vector2i &rel, int button, int modifiers) {
@@ -60,13 +61,13 @@ bool CameraView3D::mouseMotionEvent(const Eigen::Vector2i &p, const Eigen::Vecto
rx_ += rel[0];
ry_ += rel[1];
+ LOG(INFO) << "New pose: \n" << getUpdatedPose();
return true;
}
bool CameraView3D::keyboardCharacterEvent(unsigned int codepoint) {
LOG(INFO) << "keyboardCharacterEvent: " << codepoint;
- LOG(INFO) << getPose();
- return true;
+ return false;
}
Eigen::Matrix4d CameraView3D::getUpdatedPose() {
diff --git a/applications/gui2/src/views/config.cpp b/applications/gui2/src/views/config.cpp
index c9222733cc556cc63b4a81f70b47792910818529..e02fdfd8237aa3e7a5c7262ef3a8fc44a29d84d2 100644
--- a/applications/gui2/src/views/config.cpp
+++ b/applications/gui2/src/views/config.cpp
@@ -76,9 +76,12 @@ static std::string titleForURI(const ftl::URI &uri) {
ConfigWindow::ConfigWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl)
: nanogui::Window(parent, "Settings"), ctrl_(ctrl) {
+
+ LOG(INFO) << __func__ << " (" << this << ")";
+
using namespace nanogui;
- setLayout(new GroupLayout(15, 6, 14, 2));
+ setLayout(new GroupLayout(15, 6, 14, 10));
setPosition(Vector2i(parent->width()/2.0f - 100.0f, parent->height()/2.0f - 100.0f));
auto close = new nanogui::Button(buttonPanel(), "", ENTYPO_ICON_CROSS);
@@ -142,7 +145,7 @@ ConfigWindow::ConfigWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl)
}
ConfigWindow::~ConfigWindow() {
- LOG(INFO) << "ConfigWindow::~ConfigWindow()";
+ LOG(INFO) << __func__ << " (" << this << ")";
}
bool ConfigWindow::_isEmpty(const std::string &uri) {
diff --git a/components/common/cpp/src/cuda_common.cpp b/components/common/cpp/src/cuda_common.cpp
index ede4a998a4a104539d1b13ce32f6f278c2f3a8b2..273b540c4e2a640c86255b8ed040edde9d8cc40a 100644
--- a/components/common/cpp/src/cuda_common.cpp
+++ b/components/common/cpp/src/cuda_common.cpp
@@ -11,7 +11,7 @@ static std::vector<cudaDeviceProp> properties;
bool ftl::cuda::initialise() {
if (dev_count > 0) return true;
-
+
// Do an initial CUDA check
cudaSafeCall(cudaGetDeviceCount(&dev_count));
CHECK_GE(dev_count, 1) << "No CUDA devices found";
@@ -174,4 +174,4 @@ void BufferBase::upload(const cv::Mat &m, cudaStream_t stream) {
void BufferBase::download(cv::Mat &m, cudaStream_t stream) const {
m.create(height(), width(), cvType_);
cudaSafeCall(cudaMemcpy2DAsync(m.data, m.step, devicePtr(), pitch(), m.cols * m.elemSize(), m.rows, cudaMemcpyDeviceToHost, stream));
-}
\ No newline at end of file
+}
diff --git a/components/renderers/cpp/src/overlay.cpp b/components/renderers/cpp/src/overlay.cpp
index 1513bc36f91db1fb4b3523772edb55af37b9d8db..2472ade58f32eb832677c265dece1cb5424fdf74 100644
--- a/components/renderers/cpp/src/overlay.cpp
+++ b/components/renderers/cpp/src/overlay.cpp
@@ -21,12 +21,12 @@ namespace {
uniform float height;
uniform float far;
uniform float near;
- uniform mat4 pose;
- uniform vec3 scale;
+ uniform mat4 pose;
+ uniform vec3 scale;
void main() {
- vec4 vert = pose*(vec4(scale*vertex,1.0));
- vert = vert / vert.w;
+ vec4 vert = pose*(vec4(scale*vertex,1.0));
+ vert = vert / vert.w;
//vec4 pos = vec4(-vert.x*focal / -vert.z / (width/2.0),
// vert.y*focal / -vert.z / (height/2.0),
// (vert.z-near) / (far-near) * 2.0 - 1.0, 1.0);
@@ -46,7 +46,7 @@ namespace {
R"(#version 330
uniform vec4 blockColour;
out vec4 color;
-
+
void main() {
color = blockColour;
})";
@@ -61,201 +61,201 @@ Overlay::~Overlay() {
}
void Overlay::_createShapes() {
- shape_verts_ = {
- // Box
- {-1.0, -1.0, -1.0},
- {1.0, -1.0, -1.0},
- {1.0, 1.0, -1.0},
- {-1.0, 1.0, -1.0},
- {-1.0, -1.0, 1.0},
- {1.0, -1.0, 1.0},
- {1.0, 1.0, 1.0},
- {-1.0, 1.0, 1.0},
-
- // Camera
- {0.0, 0.0, 0.0}, // 8
- {0.5, 0.28, 0.5},
- {0.5, -0.28, 0.5},
- {-0.5, 0.28, 0.5},
- {-0.5, -0.28, 0.5},
-
- // Axis lines
- {1.0, 0.0, 0.0}, // 13
- {0.0, -1.0, 0.0},
- {0.0, 0.0, 1.0},
-
- // Plane XZ big
- {-10.0, 0.0, -10.0}, // 16
- {10.0, 0.0, -10.0},
- {10.0, 0.0, 10.0},
- {-10.0, 0.0, 10.0}
- };
-
- // Generate a big plane
- for (int x=-9; x<=9; ++x) {
- shape_verts_.push_back({float(x), 0.0, -10.0});
- shape_verts_.push_back({float(x), 0.0, 10.0});
- shape_verts_.push_back({-10.0, 0.0, float(x)});
- shape_verts_.push_back({10.0, 0.0, float(x)});
- }
-
- shape_tri_indices_ = {
- // Box
- 0, 1, 2,
- 0, 2, 3,
- 1, 5, 6,
- 1, 6, 2,
- 0, 4, 7,
- 0, 7, 3,
- 3, 2, 6,
- 3, 6, 7,
- 0, 1, 5,
- 0, 5, 4,
-
- // Box Lines
- 0, 1, // 30
- 1, 5,
- 5, 6,
- 6, 2,
- 2, 1,
- 2, 3,
- 3, 0,
- 3, 7,
- 7, 4,
- 4, 5,
- 6, 7,
- 0, 4,
-
- // Camera
- 8, 9, 10, // 54
- 8, 11, 12,
- 8, 9, 11,
- 8, 10, 12,
-
- // Camera Lines
- 8, 9, // 66
- 8, 10,
- 8, 11,
- 8, 12,
- 9, 10,
- 11, 12,
- 9, 11,
- 10, 12,
-
- // Axis lines
- 8, 13, // 82
- 8, 14,
- 8, 15,
-
- // Big XZ Plane
- 16, 17, 18, // 88
- 18, 19, 16
- };
-
- int i = 20;
- for (int x=-10; x<=10; ++x) {
- shape_tri_indices_.push_back(i++);
- shape_tri_indices_.push_back(i++);
- shape_tri_indices_.push_back(i++);
- shape_tri_indices_.push_back(i++);
- }
-
- shapes_[Shape::BOX] = {0,30, 30, 12*2};
- shapes_[Shape::CAMERA] = {54, 4*3, 66, 8*2};
- shapes_[Shape::XZPLANE] = {88, 2*3, 94, 40*2};
- shapes_[Shape::AXIS] = {0, 0, 82, 2*3};
-
- oShader.uploadAttrib("vertex", sizeof(float3)*shape_verts_.size(), 3, sizeof(float), GL_FLOAT, false, shape_verts_.data());
- oShader.uploadAttrib ("indices", sizeof(int)*shape_tri_indices_.size(), 1, sizeof(int), GL_UNSIGNED_INT, true, shape_tri_indices_.data());
+ shape_verts_ = {
+ // Box
+ {-1.0, -1.0, -1.0},
+ {1.0, -1.0, -1.0},
+ {1.0, 1.0, -1.0},
+ {-1.0, 1.0, -1.0},
+ {-1.0, -1.0, 1.0},
+ {1.0, -1.0, 1.0},
+ {1.0, 1.0, 1.0},
+ {-1.0, 1.0, 1.0},
+
+ // Camera
+ {0.0, 0.0, 0.0}, // 8
+ {0.5, 0.28, 0.5},
+ {0.5, -0.28, 0.5},
+ {-0.5, 0.28, 0.5},
+ {-0.5, -0.28, 0.5},
+
+ // Axis lines
+ {1.0, 0.0, 0.0}, // 13
+ {0.0, -1.0, 0.0},
+ {0.0, 0.0, 1.0},
+
+ // Plane XZ big
+ {-10.0, 0.0, -10.0}, // 16
+ {10.0, 0.0, -10.0},
+ {10.0, 0.0, 10.0},
+ {-10.0, 0.0, 10.0}
+ };
+
+ // Generate a big plane
+ for (int x=-9; x<=9; ++x) {
+ shape_verts_.push_back({float(x), 0.0, -10.0});
+ shape_verts_.push_back({float(x), 0.0, 10.0});
+ shape_verts_.push_back({-10.0, 0.0, float(x)});
+ shape_verts_.push_back({10.0, 0.0, float(x)});
+ }
+
+ shape_tri_indices_ = {
+ // Box
+ 0, 1, 2,
+ 0, 2, 3,
+ 1, 5, 6,
+ 1, 6, 2,
+ 0, 4, 7,
+ 0, 7, 3,
+ 3, 2, 6,
+ 3, 6, 7,
+ 0, 1, 5,
+ 0, 5, 4,
+
+ // Box Lines
+ 0, 1, // 30
+ 1, 5,
+ 5, 6,
+ 6, 2,
+ 2, 1,
+ 2, 3,
+ 3, 0,
+ 3, 7,
+ 7, 4,
+ 4, 5,
+ 6, 7,
+ 0, 4,
+
+ // Camera
+ 8, 9, 10, // 54
+ 8, 11, 12,
+ 8, 9, 11,
+ 8, 10, 12,
+
+ // Camera Lines
+ 8, 9, // 66
+ 8, 10,
+ 8, 11,
+ 8, 12,
+ 9, 10,
+ 11, 12,
+ 9, 11,
+ 10, 12,
+
+ // Axis lines
+ 8, 13, // 82
+ 8, 14,
+ 8, 15,
+
+ // Big XZ Plane
+ 16, 17, 18, // 88
+ 18, 19, 16
+ };
+
+ int i = 20;
+ for (int x=-10; x<=10; ++x) {
+ shape_tri_indices_.push_back(i++);
+ shape_tri_indices_.push_back(i++);
+ shape_tri_indices_.push_back(i++);
+ shape_tri_indices_.push_back(i++);
+ }
+
+ shapes_[Shape::BOX] = {0,30, 30, 12*2};
+ shapes_[Shape::CAMERA] = {54, 4*3, 66, 8*2};
+ shapes_[Shape::XZPLANE] = {88, 2*3, 94, 40*2};
+ shapes_[Shape::AXIS] = {0, 0, 82, 2*3};
+
+ oShader.uploadAttrib("vertex", sizeof(float3)*shape_verts_.size(), 3, sizeof(float), GL_FLOAT, false, shape_verts_.data());
+ oShader.uploadAttrib ("indices", sizeof(int)*shape_tri_indices_.size(), 1, sizeof(int), GL_UNSIGNED_INT, true, shape_tri_indices_.data());
}
void Overlay::_drawFilledShape(Shape shape, const Eigen::Matrix4d &pose, float scale, uchar4 c) {
- if (shapes_.find(shape) ==shapes_.end()) {
- return;
- }
-
- Eigen::Matrix4f mv = pose.cast<float>();
-
- auto [offset,count, loffset, lcount] = shapes_[shape];
- UNUSED(loffset);
- UNUSED(lcount);
- oShader.setUniform("scale", scale);
- oShader.setUniform("pose", mv);
- oShader.setUniform("blockColour", Eigen::Vector4f(float(c.x)/255.0f,float(c.y)/255.0f,float(c.z)/255.0f,float(c.w)/255.0f));
+ if (shapes_.find(shape) ==shapes_.end()) {
+ return;
+ }
+
+ Eigen::Matrix4f mv = pose.cast<float>();
+
+ auto [offset,count, loffset, lcount] = shapes_[shape];
+ UNUSED(loffset);
+ UNUSED(lcount);
+ oShader.setUniform("scale", scale);
+ oShader.setUniform("pose", mv);
+ oShader.setUniform("blockColour", Eigen::Vector4f(float(c.x)/255.0f,float(c.y)/255.0f,float(c.z)/255.0f,float(c.w)/255.0f));
//oShader.drawIndexed(GL_TRIANGLES, offset, count);
- glDrawElements(GL_TRIANGLES, (GLsizei) count, GL_UNSIGNED_INT,
- (const void *)(offset * sizeof(uint32_t)));
+ glDrawElements(GL_TRIANGLES, (GLsizei) count, GL_UNSIGNED_INT,
+ (const void *)(offset * sizeof(uint32_t)));
}
void Overlay::_drawOutlinedShape(Shape shape, const Eigen::Matrix4d &pose, const Eigen::Vector3f &scale, uchar4 fill, uchar4 outline) {
- if (shapes_.find(shape) ==shapes_.end()) {
- return;
- }
-
- Eigen::Matrix4f mv = pose.cast<float>();
-
- auto [offset,count,loffset,lcount] = shapes_[shape];
- oShader.setUniform("scale", scale);
- oShader.setUniform("pose", mv);
-
- if (count > 0) {
- oShader.setUniform("blockColour", Eigen::Vector4f(float(fill.x)/255.0f,float(fill.y)/255.0f,float(fill.z)/255.0f,float(fill.w)/255.0f));
- //oShader.drawIndexed(GL_TRIANGLES, offset, count);
- glDrawElements(GL_TRIANGLES, (GLsizei) count, GL_UNSIGNED_INT,
- (const void *)(offset * sizeof(uint32_t)));
- }
-
- if (lcount != 0) {
- oShader.setUniform("blockColour", Eigen::Vector4f(float(outline.x)/255.0f,float(outline.y)/255.0f,float(outline.z)/255.0f,float(outline.w)/255.0f));
- //oShader.drawIndexed(GL_LINE_LOOP, offset, count);
- glDrawElements(GL_LINES, (GLsizei) lcount, GL_UNSIGNED_INT,
- (const void *)(loffset * sizeof(uint32_t)));
- }
+ if (shapes_.find(shape) ==shapes_.end()) {
+ return;
+ }
+
+ Eigen::Matrix4f mv = pose.cast<float>();
+
+ auto [offset,count,loffset,lcount] = shapes_[shape];
+ oShader.setUniform("scale", scale);
+ oShader.setUniform("pose", mv);
+
+ if (count > 0) {
+ oShader.setUniform("blockColour", Eigen::Vector4f(float(fill.x)/255.0f,float(fill.y)/255.0f,float(fill.z)/255.0f,float(fill.w)/255.0f));
+ //oShader.drawIndexed(GL_TRIANGLES, offset, count);
+ glDrawElements(GL_TRIANGLES, (GLsizei) count, GL_UNSIGNED_INT,
+ (const void *)(offset * sizeof(uint32_t)));
+ }
+
+ if (lcount != 0) {
+ oShader.setUniform("blockColour", Eigen::Vector4f(float(outline.x)/255.0f,float(outline.y)/255.0f,float(outline.z)/255.0f,float(outline.w)/255.0f));
+ //oShader.drawIndexed(GL_LINE_LOOP, offset, count);
+ glDrawElements(GL_LINES, (GLsizei) lcount, GL_UNSIGNED_INT,
+ (const void *)(loffset * sizeof(uint32_t)));
+ }
}
void Overlay::_drawAxis(const Eigen::Matrix4d &pose, const Eigen::Vector3f &scale) {
- Eigen::Matrix4f mv = pose.cast<float>();
-
- auto [offset,count,loffset,lcount] = shapes_[Shape::AXIS];
- UNUSED(offset);
- UNUSED(count);
- UNUSED(lcount);
- oShader.setUniform("scale", scale);
- oShader.setUniform("pose", mv);
-
- oShader.setUniform("blockColour", Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f));
- //oShader.drawIndexed(GL_LINE_LOOP, offset, count);
- glDrawElements(GL_LINES, (GLsizei) 2, GL_UNSIGNED_INT,
- (const void *)(loffset * sizeof(uint32_t)));
-
- loffset += 2;
- oShader.setUniform("blockColour", Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f));
- //oShader.drawIndexed(GL_LINE_LOOP, offset, count);
- glDrawElements(GL_LINES, (GLsizei) 2, GL_UNSIGNED_INT,
- (const void *)(loffset * sizeof(uint32_t)));
-
- loffset += 2;
- oShader.setUniform("blockColour", Eigen::Vector4f(0.0f, 0.0f, 1.0f, 1.0f));
- //oShader.drawIndexed(GL_LINE_LOOP, offset, count);
- glDrawElements(GL_LINES, (GLsizei) 2, GL_UNSIGNED_INT,
- (const void *)(loffset * sizeof(uint32_t)));
+ Eigen::Matrix4f mv = pose.cast<float>();
+
+ auto [offset,count,loffset,lcount] = shapes_[Shape::AXIS];
+ UNUSED(offset);
+ UNUSED(count);
+ UNUSED(lcount);
+ oShader.setUniform("scale", scale);
+ oShader.setUniform("pose", mv);
+
+ oShader.setUniform("blockColour", Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f));
+ //oShader.drawIndexed(GL_LINE_LOOP, offset, count);
+ glDrawElements(GL_LINES, (GLsizei) 2, GL_UNSIGNED_INT,
+ (const void *)(loffset * sizeof(uint32_t)));
+
+ loffset += 2;
+ oShader.setUniform("blockColour", Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f));
+ //oShader.drawIndexed(GL_LINE_LOOP, offset, count);
+ glDrawElements(GL_LINES, (GLsizei) 2, GL_UNSIGNED_INT,
+ (const void *)(loffset * sizeof(uint32_t)));
+
+ loffset += 2;
+ oShader.setUniform("blockColour", Eigen::Vector4f(0.0f, 0.0f, 1.0f, 1.0f));
+ //oShader.drawIndexed(GL_LINE_LOOP, offset, count);
+ glDrawElements(GL_LINES, (GLsizei) 2, GL_UNSIGNED_INT,
+ (const void *)(loffset * sizeof(uint32_t)));
}
void Overlay::draw(ftl::data::FrameSet &fs, ftl::rgbd::Frame &frame, const Eigen::Vector2f &screenSize) {
if (!value("enabled", false)) return;
-
+
double zfar = 8.0f;
auto intrin = frame.getLeft();
intrin = intrin.scaled(screenSize[0], screenSize[1]);
if (!init_) {
oShader.init("OverlayShader", overlayVertexShader, overlayFragmentShader);
- oShader.bind();
- _createShapes();
+ oShader.bind();
+ _createShapes();
init_ = true;
} else {
- oShader.bind();
- }
+ oShader.bind();
+ }
float3 tris[] = {
{0.5f, -0.7f, 2.0f},
@@ -300,22 +300,22 @@ void Overlay::draw(ftl::data::FrameSet &fs, ftl::rgbd::Frame &frame, const Eigen
auto pose = f.getPose(); //.inverse() * state.getPose();
auto name = fs.frames[i].get<std::string>(Channel::Name);
- _drawOutlinedShape(Shape::CAMERA, frame.getPose().inverse() * pose, Eigen::Vector3f(0.2f,0.2f,0.2f), make_uchar4(255,0,0,80), make_uchar4(255,0,0,255));
- _drawAxis(frame.getPose().inverse() * pose, Eigen::Vector3f(0.2f, 0.2f, 0.2f));
+ _drawOutlinedShape(Shape::CAMERA, frame.getPose().inverse() * pose, Eigen::Vector3f(0.2f,0.2f,0.2f), make_uchar4(255,0,0,80), make_uchar4(255,0,0,255));
+ _drawAxis(frame.getPose().inverse() * pose, Eigen::Vector3f(0.2f, 0.2f, 0.2f));
//ftl::overlay::drawCamera(state.getLeft(), out, over_depth_, fs.frames[i].getLeftCamera(), pose, cv::Scalar(0,0,255,255), 0.2,value("show_frustrum", false));
//if (name) ftl::overlay::drawText(state.getLeft(), out, over_depth_, *name, pos, 0.5, cv::Scalar(0,0,255,255));
}
}
- if (value("show_xz_plane", false)) {
- float gscale = value("grid_scale",0.5f);
- _drawOutlinedShape(Shape::XZPLANE, frame.getPose().inverse(), Eigen::Vector3f(gscale,gscale,gscale), make_uchar4(200,200,200,50), make_uchar4(255,255,255,100));
- }
+ if (value("show_xz_plane", false)) {
+ float gscale = value("grid_scale",0.5f);
+ _drawOutlinedShape(Shape::XZPLANE, frame.getPose().inverse(), Eigen::Vector3f(gscale,gscale,gscale), make_uchar4(200,200,200,50), make_uchar4(255,255,255,100));
+ }
- if (value("show_axis", true)) {
- _drawAxis(frame.getPose().inverse(), Eigen::Vector3f(0.5f, 0.5f, 0.5f));
- }
+ if (value("show_axis", true)) {
+ _drawAxis(frame.getPose().inverse(), Eigen::Vector3f(0.5f, 0.5f, 0.5f));
+ }
if (value("show_shapes", false)) {
/*if (fs.hasChannel(Channel::Shapes3D)) {
@@ -327,19 +327,19 @@ void Overlay::draw(ftl::data::FrameSet &fs, ftl::rgbd::Frame &frame, const Eigen
//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);
+ Eigen::Vector3f scale(s.size[0]/2.0f, s.size[1]/2.0f, s.size[2]/2.0f);
- if (s.type == ftl::codecs::Shape3DType::CAMERA) {
- //auto pose = s.pose;
- auto name = s.label;
- _drawOutlinedShape(Shape::CAMERA, state.getPose().inverse() * pose, Eigen::Vector3f(0.2f,0.2f,0.2f), make_uchar4(255,0,0,80), make_uchar4(255,0,0,255));
- _drawAxis(state.getPose().inverse() * pose, Eigen::Vector3f(0.2f, 0.2f, 0.2f));
- } else {
- _drawOutlinedShape(Shape::BOX, state.getPose().inverse() * pose, scale, make_uchar4(255,0,255,80), make_uchar4(255,0,255,255));
- }
+ if (s.type == ftl::codecs::Shape3DType::CAMERA) {
+ //auto pose = s.pose;
+ auto name = s.label;
+ _drawOutlinedShape(Shape::CAMERA, state.getPose().inverse() * pose, Eigen::Vector3f(0.2f,0.2f,0.2f), make_uchar4(255,0,0,80), make_uchar4(255,0,0,255));
+ _drawAxis(state.getPose().inverse() * pose, Eigen::Vector3f(0.2f, 0.2f, 0.2f));
+ } else {
+ _drawOutlinedShape(Shape::BOX, state.getPose().inverse() * pose, scale, make_uchar4(255,0,255,80), make_uchar4(255,0,255,255));
+ }
//ftl::overlay::drawFilledBox(state.getLeft(), out, over_depth_, pose, cv::Scalar(0,0,255,50), s.size.cast<double>());
- //ftl::overlay::drawBox(state.getLeft(), out, over_depth_, pose, cv::Scalar(0,0,255,255), s.size.cast<double>());
+ //ftl::overlay::drawBox(state.getLeft(), out, over_depth_, pose, cv::Scalar(0,0,255,255), s.size.cast<double>());
//ftl::overlay::drawText(state.getLeft(), out, over_depth_, s.label, pos, 0.5, cv::Scalar(0,0,255,100));
}
}*/
@@ -353,13 +353,13 @@ void Overlay::draw(ftl::data::FrameSet &fs, ftl::rgbd::Frame &frame, const Eigen
//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);
+ 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, frame.getPose().inverse() * pose, scale, make_uchar4(255,0,255,80), make_uchar4(255,0,255,255)); break;
- case ftl::codecs::Shape3DType::ARUCO: _drawAxis(frame.getPose().inverse() * pose, Eigen::Vector3f(0.2f, 0.2f, 0.2f)); break;
- default: break;
- }
+ switch (s.type) {
+ case ftl::codecs::Shape3DType::CLIPPING: _drawOutlinedShape(Shape::BOX, frame.getPose().inverse() * pose, scale, make_uchar4(255,0,255,80), make_uchar4(255,0,255,255)); break;
+ case ftl::codecs::Shape3DType::ARUCO: _drawAxis(frame.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));
@@ -368,384 +368,384 @@ void Overlay::draw(ftl::data::FrameSet &fs, ftl::rgbd::Frame &frame, const Eigen
}
}
- glDisable(GL_LINE_SMOOTH);
+ glDisable(GL_LINE_SMOOTH);
glDisable(GL_BLEND);
//cv::flip(out, out, 0);
}
/*void ftl::overlay::draw3DLine(
- const ftl::rgbd::Camera &cam,
- cv::Mat &colour,
- cv::Mat &depth,
- const Eigen::Vector4d &begin,
- const Eigen::Vector4d &end,
- const cv::Scalar &linecolour) {
-
-
- auto begin_pos = cam.camToScreen<int2>(make_float3(begin[0], begin[1], begin[2]));
- auto end_pos = cam.camToScreen<int2>(make_float3(end[0], end[1], end[2]));
-
- cv::LineIterator lineit(colour, cv::Point(begin_pos.x, begin_pos.y), cv::Point(end_pos.x, end_pos.y));
- double z_grad = (end[2] - begin[2]) / lineit.count;
- double current_z = begin[2];
-
- for(int i = 0; i < lineit.count; i++, ++lineit) {
- colour.at<cv::Vec4b>(lineit.pos()) = linecolour;
- depth.at<float>(lineit.pos()) = current_z;
- current_z += z_grad;
- }
+ const ftl::rgbd::Camera &cam,
+ cv::Mat &colour,
+ cv::Mat &depth,
+ const Eigen::Vector4d &begin,
+ const Eigen::Vector4d &end,
+ const cv::Scalar &linecolour) {
+
+
+ auto begin_pos = cam.camToScreen<int2>(make_float3(begin[0], begin[1], begin[2]));
+ auto end_pos = cam.camToScreen<int2>(make_float3(end[0], end[1], end[2]));
+
+ cv::LineIterator lineit(colour, cv::Point(begin_pos.x, begin_pos.y), cv::Point(end_pos.x, end_pos.y));
+ double z_grad = (end[2] - begin[2]) / lineit.count;
+ double current_z = begin[2];
+
+ for(int i = 0; i < lineit.count; i++, ++lineit) {
+ colour.at<cv::Vec4b>(lineit.pos()) = linecolour;
+ depth.at<float>(lineit.pos()) = current_z;
+ current_z += z_grad;
+ }
}
void ftl::overlay::drawPoseBox(
- const ftl::rgbd::Camera &cam,
- cv::Mat &colour,
- cv::Mat &depth,
- const Eigen::Matrix4d &pose,
- const cv::Scalar &linecolour,
- double size) {
-
- double size2 = size/2.0;
-
- Eigen::Vector4d p001 = pose.inverse() * Eigen::Vector4d(size2,size2,-size2,1);
- Eigen::Vector4d p011 = pose.inverse() * Eigen::Vector4d(size2,-size2,-size2,1);
- Eigen::Vector4d p111 = pose.inverse() * Eigen::Vector4d(-size2,-size2,-size2,1);
- Eigen::Vector4d p101 = pose.inverse() * Eigen::Vector4d(-size2,size2,-size2,1);
- Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-size2,-size2,size2,1);
- Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-size2,size2,size2,1);
- Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(size2,-size2,size2,1);
- Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(size2,size2,size2,1);
-
- p001 /= p001[3];
- p011 /= p011[3];
- p111 /= p111[3];
- p101 /= p101[3];
- p110 /= p110[3];
- p100 /= p100[3];
- p010 /= p010[3];
- p000 /= p000[3];
-
- if (p001[2] < 0.1 || p011[2] < 0.1 || p111[2] < 0.1 || p101[2] < 0.1 || p110[2] < 0.1 || p100[2] < 0.1 || p010[2] < 0.1 || p000[2] < 0.1) return;
-
- draw3DLine(cam, colour, depth, p000, p001, linecolour);
- draw3DLine(cam, colour, depth, p000, p010, linecolour);
- draw3DLine(cam, colour, depth, p000, p100, linecolour);
-
- draw3DLine(cam, colour, depth, p001, p011, linecolour);
- draw3DLine(cam, colour, depth, p001, p101, linecolour);
-
- draw3DLine(cam, colour, depth, p010, p011, linecolour);
- draw3DLine(cam, colour, depth, p010, p110, linecolour);
-
- draw3DLine(cam, colour, depth, p100, p101, linecolour);
- draw3DLine(cam, colour, depth, p100, p110, linecolour);
-
- draw3DLine(cam, colour, depth, p101, p111, linecolour);
- draw3DLine(cam, colour, depth, p110, p111, linecolour);
- draw3DLine(cam, colour, depth, p011, p111, linecolour);
+ const ftl::rgbd::Camera &cam,
+ cv::Mat &colour,
+ cv::Mat &depth,
+ const Eigen::Matrix4d &pose,
+ const cv::Scalar &linecolour,
+ double size) {
+
+ double size2 = size/2.0;
+
+ Eigen::Vector4d p001 = pose.inverse() * Eigen::Vector4d(size2,size2,-size2,1);
+ Eigen::Vector4d p011 = pose.inverse() * Eigen::Vector4d(size2,-size2,-size2,1);
+ Eigen::Vector4d p111 = pose.inverse() * Eigen::Vector4d(-size2,-size2,-size2,1);
+ Eigen::Vector4d p101 = pose.inverse() * Eigen::Vector4d(-size2,size2,-size2,1);
+ Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-size2,-size2,size2,1);
+ Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-size2,size2,size2,1);
+ Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(size2,-size2,size2,1);
+ Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(size2,size2,size2,1);
+
+ p001 /= p001[3];
+ p011 /= p011[3];
+ p111 /= p111[3];
+ p101 /= p101[3];
+ p110 /= p110[3];
+ p100 /= p100[3];
+ p010 /= p010[3];
+ p000 /= p000[3];
+
+ if (p001[2] < 0.1 || p011[2] < 0.1 || p111[2] < 0.1 || p101[2] < 0.1 || p110[2] < 0.1 || p100[2] < 0.1 || p010[2] < 0.1 || p000[2] < 0.1) return;
+
+ draw3DLine(cam, colour, depth, p000, p001, linecolour);
+ draw3DLine(cam, colour, depth, p000, p010, linecolour);
+ draw3DLine(cam, colour, depth, p000, p100, linecolour);
+
+ draw3DLine(cam, colour, depth, p001, p011, linecolour);
+ draw3DLine(cam, colour, depth, p001, p101, linecolour);
+
+ draw3DLine(cam, colour, depth, p010, p011, linecolour);
+ draw3DLine(cam, colour, depth, p010, p110, linecolour);
+
+ draw3DLine(cam, colour, depth, p100, p101, linecolour);
+ draw3DLine(cam, colour, depth, p100, p110, linecolour);
+
+ draw3DLine(cam, colour, depth, p101, p111, linecolour);
+ draw3DLine(cam, colour, depth, p110, p111, linecolour);
+ draw3DLine(cam, colour, depth, p011, p111, linecolour);
}
void ftl::overlay::drawBox(
- const ftl::rgbd::Camera &cam,
- cv::Mat &colour,
- cv::Mat &depth,
- const Eigen::Matrix4d &pose,
- const cv::Scalar &linecolour,
- const Eigen::Vector3d &size) {
-
- double size2x = size[0]/2.0;
+ const ftl::rgbd::Camera &cam,
+ cv::Mat &colour,
+ cv::Mat &depth,
+ const Eigen::Matrix4d &pose,
+ const cv::Scalar &linecolour,
+ const Eigen::Vector3d &size) {
+
+ double size2x = size[0]/2.0;
double size2y = size[1]/2.0;
double size2z = size[2]/2.0;
- Eigen::Vector4d p001 = pose.inverse() * Eigen::Vector4d(size2x,size2y,-size2z,1);
- Eigen::Vector4d p011 = pose.inverse() * Eigen::Vector4d(size2x,-size2y,-size2z,1);
- Eigen::Vector4d p111 = pose.inverse() * Eigen::Vector4d(-size2x,-size2y,-size2z,1);
- Eigen::Vector4d p101 = pose.inverse() * Eigen::Vector4d(-size2x,size2y,-size2z,1);
- Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-size2x,-size2y,size2z,1);
- Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-size2x,size2y,size2z,1);
- Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(size2x,-size2y,size2z,1);
- Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(size2x,size2y,size2z,1);
-
- p001 /= p001[3];
- p011 /= p011[3];
- p111 /= p111[3];
- p101 /= p101[3];
- p110 /= p110[3];
- p100 /= p100[3];
- p010 /= p010[3];
- p000 /= p000[3];
-
- if (p001[2] < 0.1 || p011[2] < 0.1 || p111[2] < 0.1 || p101[2] < 0.1 || p110[2] < 0.1 || p100[2] < 0.1 || p010[2] < 0.1 || p000[2] < 0.1) return;
-
- draw3DLine(cam, colour, depth, p000, p001, linecolour);
- draw3DLine(cam, colour, depth, p000, p010, linecolour);
- draw3DLine(cam, colour, depth, p000, p100, linecolour);
-
- draw3DLine(cam, colour, depth, p001, p011, linecolour);
- draw3DLine(cam, colour, depth, p001, p101, linecolour);
-
- draw3DLine(cam, colour, depth, p010, p011, linecolour);
- draw3DLine(cam, colour, depth, p010, p110, linecolour);
-
- draw3DLine(cam, colour, depth, p100, p101, linecolour);
- draw3DLine(cam, colour, depth, p100, p110, linecolour);
-
- draw3DLine(cam, colour, depth, p101, p111, linecolour);
- draw3DLine(cam, colour, depth, p110, p111, linecolour);
- draw3DLine(cam, colour, depth, p011, p111, linecolour);
+ Eigen::Vector4d p001 = pose.inverse() * Eigen::Vector4d(size2x,size2y,-size2z,1);
+ Eigen::Vector4d p011 = pose.inverse() * Eigen::Vector4d(size2x,-size2y,-size2z,1);
+ Eigen::Vector4d p111 = pose.inverse() * Eigen::Vector4d(-size2x,-size2y,-size2z,1);
+ Eigen::Vector4d p101 = pose.inverse() * Eigen::Vector4d(-size2x,size2y,-size2z,1);
+ Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-size2x,-size2y,size2z,1);
+ Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-size2x,size2y,size2z,1);
+ Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(size2x,-size2y,size2z,1);
+ Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(size2x,size2y,size2z,1);
+
+ p001 /= p001[3];
+ p011 /= p011[3];
+ p111 /= p111[3];
+ p101 /= p101[3];
+ p110 /= p110[3];
+ p100 /= p100[3];
+ p010 /= p010[3];
+ p000 /= p000[3];
+
+ if (p001[2] < 0.1 || p011[2] < 0.1 || p111[2] < 0.1 || p101[2] < 0.1 || p110[2] < 0.1 || p100[2] < 0.1 || p010[2] < 0.1 || p000[2] < 0.1) return;
+
+ draw3DLine(cam, colour, depth, p000, p001, linecolour);
+ draw3DLine(cam, colour, depth, p000, p010, linecolour);
+ draw3DLine(cam, colour, depth, p000, p100, linecolour);
+
+ draw3DLine(cam, colour, depth, p001, p011, linecolour);
+ draw3DLine(cam, colour, depth, p001, p101, linecolour);
+
+ draw3DLine(cam, colour, depth, p010, p011, linecolour);
+ draw3DLine(cam, colour, depth, p010, p110, linecolour);
+
+ draw3DLine(cam, colour, depth, p100, p101, linecolour);
+ draw3DLine(cam, colour, depth, p100, p110, linecolour);
+
+ draw3DLine(cam, colour, depth, p101, p111, linecolour);
+ draw3DLine(cam, colour, depth, p110, p111, linecolour);
+ draw3DLine(cam, colour, depth, p011, p111, linecolour);
}
void ftl::overlay::drawFilledBox(
- const ftl::rgbd::Camera &cam,
- cv::Mat &colour,
- cv::Mat &depth,
- const Eigen::Matrix4d &pose,
- const cv::Scalar &linecolour,
- const Eigen::Vector3d &size) {
-
- double size2x = size[0]/2.0;
+ const ftl::rgbd::Camera &cam,
+ cv::Mat &colour,
+ cv::Mat &depth,
+ const Eigen::Matrix4d &pose,
+ const cv::Scalar &linecolour,
+ const Eigen::Vector3d &size) {
+
+ double size2x = size[0]/2.0;
double size2y = size[1]/2.0;
double size2z = size[2]/2.0;
- Eigen::Vector4d p001 = pose.inverse() * Eigen::Vector4d(size2x,size2y,-size2z,1);
- Eigen::Vector4d p011 = pose.inverse() * Eigen::Vector4d(size2x,-size2y,-size2z,1);
- Eigen::Vector4d p111 = pose.inverse() * Eigen::Vector4d(-size2x,-size2y,-size2z,1);
- Eigen::Vector4d p101 = pose.inverse() * Eigen::Vector4d(-size2x,size2y,-size2z,1);
- Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-size2x,-size2y,size2z,1);
- Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-size2x,size2y,size2z,1);
- Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(size2x,-size2y,size2z,1);
- Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(size2x,size2y,size2z,1);
-
- p001 /= p001[3];
- p011 /= p011[3];
- p111 /= p111[3];
- p101 /= p101[3];
- p110 /= p110[3];
- p100 /= p100[3];
- p010 /= p010[3];
- p000 /= p000[3];
-
- if (p001[2] < 0.1 || p011[2] < 0.1 || p111[2] < 0.1 || p101[2] < 0.1 || p110[2] < 0.1 || p100[2] < 0.1 || p010[2] < 0.1 || p000[2] < 0.1) return;
-
- std::array<cv::Point, 4> pts;
-
- auto p = cam.camToScreen<int2>(make_float3(p000[0], p000[1], p000[2]));
- pts[0] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p001[0], p001[1], p001[2]));
- pts[1] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p011[0], p011[1], p011[2]));
- pts[2] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p010[0], p010[1], p010[2]));
- pts[3] = cv::Point(p.x, p.y);
- cv::fillConvexPoly(colour, pts, linecolour);
-
- p = cam.camToScreen<int2>(make_float3(p100[0], p100[1], p100[2]));
- pts[0] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p101[0], p101[1], p101[2]));
- pts[1] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p111[0], p111[1], p111[2]));
- pts[2] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p110[0], p110[1], p110[2]));
- pts[3] = cv::Point(p.x, p.y);
- cv::fillConvexPoly(colour, pts, linecolour);
-
- p = cam.camToScreen<int2>(make_float3(p000[0], p000[1], p000[2]));
- pts[0] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p010[0], p010[1], p010[2]));
- pts[1] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p110[0], p110[1], p110[2]));
- pts[2] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p100[0], p100[1], p100[2]));
- pts[3] = cv::Point(p.x, p.y);
- cv::fillConvexPoly(colour, pts, linecolour);
-
- p = cam.camToScreen<int2>(make_float3(p001[0], p001[1], p001[2]));
- pts[0] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p011[0], p011[1], p011[2]));
- pts[1] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p111[0], p111[1], p111[2]));
- pts[2] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p101[0], p101[1], p101[2]));
- pts[3] = cv::Point(p.x, p.y);
- cv::fillConvexPoly(colour, pts, linecolour);
-
- p = cam.camToScreen<int2>(make_float3(p000[0], p000[1], p000[2]));
- pts[0] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p001[0], p001[1], p001[2]));
- pts[1] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p101[0], p101[1], p101[2]));
- pts[2] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p100[0], p100[1], p100[2]));
- pts[3] = cv::Point(p.x, p.y);
- cv::fillConvexPoly(colour, pts, linecolour);
-
- p = cam.camToScreen<int2>(make_float3(p010[0], p010[1], p010[2]));
- pts[0] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p011[0], p011[1], p011[2]));
- pts[1] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p111[0], p111[1], p111[2]));
- pts[2] = cv::Point(p.x, p.y);
- p = cam.camToScreen<int2>(make_float3(p110[0], p110[1], p110[2]));
- pts[3] = cv::Point(p.x, p.y);
- cv::fillConvexPoly(colour, pts, linecolour);
+ Eigen::Vector4d p001 = pose.inverse() * Eigen::Vector4d(size2x,size2y,-size2z,1);
+ Eigen::Vector4d p011 = pose.inverse() * Eigen::Vector4d(size2x,-size2y,-size2z,1);
+ Eigen::Vector4d p111 = pose.inverse() * Eigen::Vector4d(-size2x,-size2y,-size2z,1);
+ Eigen::Vector4d p101 = pose.inverse() * Eigen::Vector4d(-size2x,size2y,-size2z,1);
+ Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-size2x,-size2y,size2z,1);
+ Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-size2x,size2y,size2z,1);
+ Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(size2x,-size2y,size2z,1);
+ Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(size2x,size2y,size2z,1);
+
+ p001 /= p001[3];
+ p011 /= p011[3];
+ p111 /= p111[3];
+ p101 /= p101[3];
+ p110 /= p110[3];
+ p100 /= p100[3];
+ p010 /= p010[3];
+ p000 /= p000[3];
+
+ if (p001[2] < 0.1 || p011[2] < 0.1 || p111[2] < 0.1 || p101[2] < 0.1 || p110[2] < 0.1 || p100[2] < 0.1 || p010[2] < 0.1 || p000[2] < 0.1) return;
+
+ std::array<cv::Point, 4> pts;
+
+ auto p = cam.camToScreen<int2>(make_float3(p000[0], p000[1], p000[2]));
+ pts[0] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p001[0], p001[1], p001[2]));
+ pts[1] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p011[0], p011[1], p011[2]));
+ pts[2] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p010[0], p010[1], p010[2]));
+ pts[3] = cv::Point(p.x, p.y);
+ cv::fillConvexPoly(colour, pts, linecolour);
+
+ p = cam.camToScreen<int2>(make_float3(p100[0], p100[1], p100[2]));
+ pts[0] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p101[0], p101[1], p101[2]));
+ pts[1] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p111[0], p111[1], p111[2]));
+ pts[2] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p110[0], p110[1], p110[2]));
+ pts[3] = cv::Point(p.x, p.y);
+ cv::fillConvexPoly(colour, pts, linecolour);
+
+ p = cam.camToScreen<int2>(make_float3(p000[0], p000[1], p000[2]));
+ pts[0] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p010[0], p010[1], p010[2]));
+ pts[1] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p110[0], p110[1], p110[2]));
+ pts[2] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p100[0], p100[1], p100[2]));
+ pts[3] = cv::Point(p.x, p.y);
+ cv::fillConvexPoly(colour, pts, linecolour);
+
+ p = cam.camToScreen<int2>(make_float3(p001[0], p001[1], p001[2]));
+ pts[0] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p011[0], p011[1], p011[2]));
+ pts[1] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p111[0], p111[1], p111[2]));
+ pts[2] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p101[0], p101[1], p101[2]));
+ pts[3] = cv::Point(p.x, p.y);
+ cv::fillConvexPoly(colour, pts, linecolour);
+
+ p = cam.camToScreen<int2>(make_float3(p000[0], p000[1], p000[2]));
+ pts[0] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p001[0], p001[1], p001[2]));
+ pts[1] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p101[0], p101[1], p101[2]));
+ pts[2] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p100[0], p100[1], p100[2]));
+ pts[3] = cv::Point(p.x, p.y);
+ cv::fillConvexPoly(colour, pts, linecolour);
+
+ p = cam.camToScreen<int2>(make_float3(p010[0], p010[1], p010[2]));
+ pts[0] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p011[0], p011[1], p011[2]));
+ pts[1] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p111[0], p111[1], p111[2]));
+ pts[2] = cv::Point(p.x, p.y);
+ p = cam.camToScreen<int2>(make_float3(p110[0], p110[1], p110[2]));
+ pts[3] = cv::Point(p.x, p.y);
+ cv::fillConvexPoly(colour, pts, linecolour);
}
void ftl::overlay::drawRectangle(
- const ftl::rgbd::Camera &cam,
- cv::Mat &colour,
- cv::Mat &depth,
- const Eigen::Matrix4d &pose,
- const cv::Scalar &linecolour,
- double width, double height) {
-
- double width2 = width/2.0;
- double height2 = height/2.0;
-
- Eigen::Vector4d p001 = pose.inverse() * Eigen::Vector4d(width2,height2,0,1);
- Eigen::Vector4d p011 = pose.inverse() * Eigen::Vector4d(width2,-height2,0,1);
- Eigen::Vector4d p111 = pose.inverse() * Eigen::Vector4d(-width2,-height2,0,1);
- Eigen::Vector4d p101 = pose.inverse() * Eigen::Vector4d(-width2,height2,0,1);
-
- p001 /= p001[3];
- p011 /= p011[3];
- p111 /= p111[3];
- p101 /= p101[3];
-
- if (p001[2] < 0.1 || p011[2] < 0.1 || p111[2] < 0.1 || p101[2] < 0.1) return;
-
- draw3DLine(cam, colour, depth, p001, p011, linecolour);
- draw3DLine(cam, colour, depth, p001, p101, linecolour);
- draw3DLine(cam, colour, depth, p101, p111, linecolour);
- draw3DLine(cam, colour, depth, p011, p111, linecolour);
+ const ftl::rgbd::Camera &cam,
+ cv::Mat &colour,
+ cv::Mat &depth,
+ const Eigen::Matrix4d &pose,
+ const cv::Scalar &linecolour,
+ double width, double height) {
+
+ double width2 = width/2.0;
+ double height2 = height/2.0;
+
+ Eigen::Vector4d p001 = pose.inverse() * Eigen::Vector4d(width2,height2,0,1);
+ Eigen::Vector4d p011 = pose.inverse() * Eigen::Vector4d(width2,-height2,0,1);
+ Eigen::Vector4d p111 = pose.inverse() * Eigen::Vector4d(-width2,-height2,0,1);
+ Eigen::Vector4d p101 = pose.inverse() * Eigen::Vector4d(-width2,height2,0,1);
+
+ p001 /= p001[3];
+ p011 /= p011[3];
+ p111 /= p111[3];
+ p101 /= p101[3];
+
+ if (p001[2] < 0.1 || p011[2] < 0.1 || p111[2] < 0.1 || p101[2] < 0.1) return;
+
+ draw3DLine(cam, colour, depth, p001, p011, linecolour);
+ draw3DLine(cam, colour, depth, p001, p101, linecolour);
+ draw3DLine(cam, colour, depth, p101, p111, linecolour);
+ draw3DLine(cam, colour, depth, p011, p111, linecolour);
}
void ftl::overlay::drawPoseCone(
- const ftl::rgbd::Camera &cam,
- cv::Mat &colour,
- cv::Mat &depth,
- const Eigen::Matrix4d &pose,
- const cv::Scalar &linecolour,
- double size) {
+ const ftl::rgbd::Camera &cam,
+ cv::Mat &colour,
+ cv::Mat &depth,
+ const Eigen::Matrix4d &pose,
+ const cv::Scalar &linecolour,
+ double size) {
- double size2 = size;
+ double size2 = size;
- Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-size2,-size2,size2,1);
- Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-size2,size2,size2,1);
- Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(size2,-size2,size2,1);
- Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(size2,size2,size2,1);
- Eigen::Vector4d origin = pose.inverse() * Eigen::Vector4d(0,0,0,1);
+ Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-size2,-size2,size2,1);
+ Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-size2,size2,size2,1);
+ Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(size2,-size2,size2,1);
+ Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(size2,size2,size2,1);
+ Eigen::Vector4d origin = pose.inverse() * Eigen::Vector4d(0,0,0,1);
- p110 /= p110[3];
- p100 /= p100[3];
- p010 /= p010[3];
- p000 /= p000[3];
- origin /= origin[3];
+ p110 /= p110[3];
+ p100 /= p100[3];
+ p010 /= p010[3];
+ p000 /= p000[3];
+ origin /= origin[3];
- if (origin[2] < 0.1 || p110[2] < 0.1 || p100[2] < 0.1 || p010[2] < 0.1 || p000[2] < 0.1) return;
+ if (origin[2] < 0.1 || p110[2] < 0.1 || p100[2] < 0.1 || p010[2] < 0.1 || p000[2] < 0.1) return;
- draw3DLine(cam, colour, depth, p000, origin, linecolour);
- draw3DLine(cam, colour, depth, p000, p010, linecolour);
- draw3DLine(cam, colour, depth, p000, p100, linecolour);
+ draw3DLine(cam, colour, depth, p000, origin, linecolour);
+ draw3DLine(cam, colour, depth, p000, p010, linecolour);
+ draw3DLine(cam, colour, depth, p000, p100, linecolour);
- draw3DLine(cam, colour, depth, p010, origin, linecolour);
- draw3DLine(cam, colour, depth, p010, p110, linecolour);
+ draw3DLine(cam, colour, depth, p010, origin, linecolour);
+ draw3DLine(cam, colour, depth, p010, p110, linecolour);
- draw3DLine(cam, colour, depth, p100, origin, linecolour);
- draw3DLine(cam, colour, depth, p100, p110, linecolour);
+ draw3DLine(cam, colour, depth, p100, origin, linecolour);
+ draw3DLine(cam, colour, depth, p100, p110, linecolour);
- draw3DLine(cam, colour, depth, p110, origin, linecolour);
+ draw3DLine(cam, colour, depth, p110, origin, linecolour);
}
void ftl::overlay::drawCamera(
- const ftl::rgbd::Camera &vcam,
- cv::Mat &colour,
- cv::Mat &depth,
- const ftl::rgbd::Camera &camera,
- const Eigen::Matrix4d &pose,
- const cv::Scalar &linecolour,
- double scale, bool frustrum) {
-
- //double size2 = size;
-
- const auto ¶ms = camera;
- double width = (static_cast<double>(params.width) / static_cast<double>(params.fx)) * scale;
- double height = (static_cast<double>(params.height) / static_cast<double>(params.fx)) * scale;
- double width2 = width / 2.0;
- double height2 = height / 2.0;
-
- double principx = (((static_cast<double>(params.width) / 2.0) + params.cx) / static_cast<double>(params.fx)) * scale;
- double principy = (((static_cast<double>(params.height) / 2.0) + params.cy) / static_cast<double>(params.fx)) * scale;
-
- auto ptcoord = params.screenToCam(0,0,scale);
- Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
- ptcoord = params.screenToCam(0,params.height,scale);
- Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
- ptcoord = params.screenToCam(params.width,0,scale);
- Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
- ptcoord = params.screenToCam(params.width,params.height,scale);
- Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
- Eigen::Vector4d origin = pose.inverse() * Eigen::Vector4d(0,0,0,1);
-
- p110 /= p110[3];
- p100 /= p100[3];
- p010 /= p010[3];
- p000 /= p000[3];
- origin /= origin[3];
-
- if (origin[2] < 0.1 || p110[2] < 0.1 || p100[2] < 0.1 || p010[2] < 0.1 || p000[2] < 0.1) return;
-
- draw3DLine(vcam, colour, depth, p000, origin, linecolour);
- draw3DLine(vcam, colour, depth, p000, p010, linecolour);
- draw3DLine(vcam, colour, depth, p000, p100, linecolour);
-
- draw3DLine(vcam, colour, depth, p010, origin, linecolour);
- draw3DLine(vcam, colour, depth, p010, p110, linecolour);
-
- draw3DLine(vcam, colour, depth, p100, origin, linecolour);
- draw3DLine(vcam, colour, depth, p100, p110, linecolour);
-
- draw3DLine(vcam, colour, depth, p110, origin, linecolour);
-
- if (frustrum) {
- const double fscale = 16.0;
- ptcoord = params.screenToCam(0,0,fscale);
- Eigen::Vector4d f110 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
- ptcoord = params.screenToCam(0,params.height,fscale);
- Eigen::Vector4d f100 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
- ptcoord = params.screenToCam(params.width,0,fscale);
- Eigen::Vector4d f010 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
- ptcoord = params.screenToCam(params.width,params.height,fscale);
- Eigen::Vector4d f000 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
-
- f110 /= f110[3];
- f100 /= f100[3];
- f010 /= f010[3];
- f000 /= f000[3];
-
- if (f110[2] < 0.1 || f100[2] < 0.1 || f010[2] < 0.1 || f000[2] < 0.1) return;
-
- draw3DLine(vcam, colour, depth, f000, p000, cv::Scalar(0,255,0,0));
- draw3DLine(vcam, colour, depth, f010, p010, cv::Scalar(0,255,0,0));
- draw3DLine(vcam, colour, depth, f100, p100, cv::Scalar(0,255,0,0));
- draw3DLine(vcam, colour, depth, f110, p110, cv::Scalar(0,255,0,0));
-
- draw3DLine(vcam, colour, depth, f000, f010, cv::Scalar(0,255,0,0));
- draw3DLine(vcam, colour, depth, f000, f100, cv::Scalar(0,255,0,0));
- draw3DLine(vcam, colour, depth, f010, f110, cv::Scalar(0,255,0,0));
- draw3DLine(vcam, colour, depth, f100, f110, cv::Scalar(0,255,0,0));
- }
+ const ftl::rgbd::Camera &vcam,
+ cv::Mat &colour,
+ cv::Mat &depth,
+ const ftl::rgbd::Camera &camera,
+ const Eigen::Matrix4d &pose,
+ const cv::Scalar &linecolour,
+ double scale, bool frustrum) {
+
+ //double size2 = size;
+
+ const auto ¶ms = camera;
+ double width = (static_cast<double>(params.width) / static_cast<double>(params.fx)) * scale;
+ double height = (static_cast<double>(params.height) / static_cast<double>(params.fx)) * scale;
+ double width2 = width / 2.0;
+ double height2 = height / 2.0;
+
+ double principx = (((static_cast<double>(params.width) / 2.0) + params.cx) / static_cast<double>(params.fx)) * scale;
+ double principy = (((static_cast<double>(params.height) / 2.0) + params.cy) / static_cast<double>(params.fx)) * scale;
+
+ auto ptcoord = params.screenToCam(0,0,scale);
+ Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
+ ptcoord = params.screenToCam(0,params.height,scale);
+ Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
+ ptcoord = params.screenToCam(params.width,0,scale);
+ Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
+ ptcoord = params.screenToCam(params.width,params.height,scale);
+ Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
+ Eigen::Vector4d origin = pose.inverse() * Eigen::Vector4d(0,0,0,1);
+
+ p110 /= p110[3];
+ p100 /= p100[3];
+ p010 /= p010[3];
+ p000 /= p000[3];
+ origin /= origin[3];
+
+ if (origin[2] < 0.1 || p110[2] < 0.1 || p100[2] < 0.1 || p010[2] < 0.1 || p000[2] < 0.1) return;
+
+ draw3DLine(vcam, colour, depth, p000, origin, linecolour);
+ draw3DLine(vcam, colour, depth, p000, p010, linecolour);
+ draw3DLine(vcam, colour, depth, p000, p100, linecolour);
+
+ draw3DLine(vcam, colour, depth, p010, origin, linecolour);
+ draw3DLine(vcam, colour, depth, p010, p110, linecolour);
+
+ draw3DLine(vcam, colour, depth, p100, origin, linecolour);
+ draw3DLine(vcam, colour, depth, p100, p110, linecolour);
+
+ draw3DLine(vcam, colour, depth, p110, origin, linecolour);
+
+ if (frustrum) {
+ const double fscale = 16.0;
+ ptcoord = params.screenToCam(0,0,fscale);
+ Eigen::Vector4d f110 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
+ ptcoord = params.screenToCam(0,params.height,fscale);
+ Eigen::Vector4d f100 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
+ ptcoord = params.screenToCam(params.width,0,fscale);
+ Eigen::Vector4d f010 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
+ ptcoord = params.screenToCam(params.width,params.height,fscale);
+ Eigen::Vector4d f000 = pose.inverse() * Eigen::Vector4d(ptcoord.x,ptcoord.y,ptcoord.z,1);
+
+ f110 /= f110[3];
+ f100 /= f100[3];
+ f010 /= f010[3];
+ f000 /= f000[3];
+
+ if (f110[2] < 0.1 || f100[2] < 0.1 || f010[2] < 0.1 || f000[2] < 0.1) return;
+
+ draw3DLine(vcam, colour, depth, f000, p000, cv::Scalar(0,255,0,0));
+ draw3DLine(vcam, colour, depth, f010, p010, cv::Scalar(0,255,0,0));
+ draw3DLine(vcam, colour, depth, f100, p100, cv::Scalar(0,255,0,0));
+ draw3DLine(vcam, colour, depth, f110, p110, cv::Scalar(0,255,0,0));
+
+ draw3DLine(vcam, colour, depth, f000, f010, cv::Scalar(0,255,0,0));
+ draw3DLine(vcam, colour, depth, f000, f100, cv::Scalar(0,255,0,0));
+ draw3DLine(vcam, colour, depth, f010, f110, cv::Scalar(0,255,0,0));
+ draw3DLine(vcam, colour, depth, f100, f110, cv::Scalar(0,255,0,0));
+ }
}
void ftl::overlay::drawText(
- const ftl::rgbd::Camera &cam,
- cv::Mat &colour,
- cv::Mat &depth,
- const std::string &text,
- const Eigen::Vector4d &pos,
- double size,
- const cv::Scalar &textcolour) {
-
- auto pt = cam.camToScreen<int2>(make_float3(pos[0], pos[1], pos[2]));
- if (pos[2] < 0.1) return;
- cv::putText(colour, text, cv::Point(pt.x, colour.rows-pt.y), 0, size, textcolour, 1, cv::LINE_8, true);
+ const ftl::rgbd::Camera &cam,
+ cv::Mat &colour,
+ cv::Mat &depth,
+ const std::string &text,
+ const Eigen::Vector4d &pos,
+ double size,
+ const cv::Scalar &textcolour) {
+
+ auto pt = cam.camToScreen<int2>(make_float3(pos[0], pos[1], pos[2]));
+ if (pos[2] < 0.1) return;
+ cv::putText(colour, text, cv::Point(pt.x, colour.rows-pt.y), 0, size, textcolour, 1, cv::LINE_8, true);
}*/