diff --git a/applications/gui/src/camera.cpp b/applications/gui/src/camera.cpp
index db1ee404deba24d35168e828aca96ebe34190394..48858d57fb74273d712a0c2e0ef48d87197e6d31 100644
--- a/applications/gui/src/camera.cpp
+++ b/applications/gui/src/camera.cpp
@@ -46,7 +46,8 @@ static Eigen::Affine3d create_rotation_matrix(float ax, float ay, float az) {
return rz * rx * ry;
}
-ftl::gui::Camera::Camera(ftl::gui::Screen *screen, int fsmask, int fid, ftl::codecs::Channel c) : screen_(screen), fsmask_(fsmask), fid_(fid), channel_(c),channels_(0u) {
+ftl::gui::Camera::Camera(ftl::gui::Screen *screen, int fsmask, int fid, ftl::codecs::Channel c)
+ : screen_(screen), fsmask_(fsmask), fid_(fid), texture1_(GLTexture::Type::BGRA), texture2_(GLTexture::Type::BGRA), depth1_(GLTexture::Type::Float), channel_(c),channels_(0u) {
eye_ = Eigen::Vector3d(0.0f, 0.0f, 0.0f);
neye_ = Eigen::Vector4d(0.0f, 0.0f, 0.0f, 0.0f);
rotmat_.setIdentity();
@@ -295,17 +296,21 @@ void ftl::gui::Camera::_draw(std::vector<ftl::rgbd::FrameSet*> &fss) {
// Make sure an OpenGL pixel buffer exists
texture1_.make(state_.getLeft().width, state_.getLeft().height);
+ depth1_.make(state_.getLeft().width, state_.getLeft().height);
if (isStereo()) texture2_.make(state_.getRight().width, state_.getRight().height);
// Map the GL pixel buffer to a GpuMat
frame_.create<cv::cuda::GpuMat>(Channel::Colour) = texture1_.map(renderer_->getCUDAStream());
+ frame_.create<cv::cuda::GpuMat>(Channel::Depth) = depth1_.map(renderer_->getCUDAStream());
+ frame_.createTexture<float>(Channel::Depth);
if (isStereo()) frame_.create<cv::cuda::GpuMat>(Channel::Colour2) = texture2_.map((renderer2_) ? renderer2_->getCUDAStream() : 0);
+ // TODO: Remove;
overlay_.create(state_.getLeft().height, state_.getLeft().width, CV_8UC4);
- frame_.create<cv::Mat>(Channel::Overlay) = overlay_;
+ //frame_.create<cv::Mat>(Channel::Overlay) = overlay_;
- overlay_.setTo(cv::Scalar(0,0,0,0));
- bool enable_overlay = overlayer_->value("enabled", false);
+ //overlay_.setTo(cv::Scalar(0,0,0,0));
+ //bool enable_overlay = overlayer_->value("enabled", false);
{
FTL_Profile("Render",0.034);
@@ -325,11 +330,11 @@ void ftl::gui::Camera::_draw(std::vector<ftl::rgbd::FrameSet*> &fss) {
renderer_->submit(fs, ftl::codecs::Channels<0>(Channel::Colour), transforms_[fs->id]);
if (isStereo()) renderer2_->submit(fs, ftl::codecs::Channels<0>(Channel::Colour), transforms_[fs->id]);
- if (enable_overlay) {
+ //if (enable_overlay) {
// Generate and upload an overlay image.
- overlayer_->apply(*fs, overlay_, state_);
- frame_.upload(Channel::Overlay, renderer_->getCUDAStream());
- }
+ // overlayer_->apply(*fs, overlay_, state_);
+ // frame_.upload(Channel::Overlay, renderer_->getCUDAStream());
+ //}
}
if (channel_ != Channel::Left && channel_ != Channel::Right && channel_ != Channel::None) {
@@ -339,9 +344,9 @@ void ftl::gui::Camera::_draw(std::vector<ftl::rgbd::FrameSet*> &fss) {
}
}
- if (enable_overlay) {
- renderer_->blend(Channel::Overlay);
- }
+ //if (enable_overlay) {
+ // renderer_->blend(Channel::Overlay);
+ //}
renderer_->end();
if (isStereo()) renderer2_->end();
@@ -364,8 +369,12 @@ void ftl::gui::Camera::_draw(std::vector<ftl::rgbd::FrameSet*> &fss) {
channels_ = frame_.getChannels();
+ // Normalize depth map
+ frame_.get<cv::cuda::GpuMat>(Channel::Depth).convertTo(frame_.get<cv::cuda::GpuMat>(Channel::Depth), CV_32F, 1.0/8.0);
+
// Unmap GL buffer from CUDA and finish updating GL texture
texture1_.unmap(renderer_->getCUDAStream());
+ depth1_.unmap(renderer_->getCUDAStream());
if (isStereo()) texture2_.unmap(renderer2_->getCUDAStream());
width_ = texture1_.width();
@@ -582,6 +591,17 @@ void ftl::gui::Camera::active(bool a) {
}
}
+void ftl::gui::Camera::drawOverlay(const Eigen::Vector2f &s) {
+ if (!framesets_) return;
+ //UNIQUE_LOCK(mutex_,lk);
+ for (auto *fs : *framesets_) {
+ if (!usesFrameset(fs->id)) continue;
+
+ // Generate and upload an overlay image.
+ overlayer_->draw(*fs, state_, s);
+ }
+}
+
const void ftl::gui::Camera::captureFrame() {
float now = (float)glfwGetTime();
if (!screen_->isVR() && (now - ftime_) < 0.04f) return;
diff --git a/applications/gui/src/camera.hpp b/applications/gui/src/camera.hpp
index c64bdfcaa12596460b63c00d303f7df1cab8ada9..2072d60f73e8e7580879e0fae97057c853288640 100644
--- a/applications/gui/src/camera.hpp
+++ b/applications/gui/src/camera.hpp
@@ -76,8 +76,13 @@ class Camera {
void draw(std::vector<ftl::rgbd::FrameSet*> &fss);
+ void drawOverlay(const Eigen::Vector2f &);
+
inline int64_t getFrameTimeMS() const { return int64_t(delta_ * 1000.0f); }
+ const ftl::rgbd::Camera &getIntrinsics() const { return state_.getLeft(); }
+ const Eigen::Matrix4d &getPose() const { return state_.getPose(); }
+
/**
* @internal. Used to inform the camera if it is the active camera or not.
*/
@@ -86,6 +91,7 @@ class Camera {
const void captureFrame();
const GLTexture &getLeft() const { return texture1_; }
const GLTexture &getRight() const { return texture2_; }
+ const GLTexture &getDepth() const { return depth1_; }
void snapshot(const std::string &filename);
@@ -116,9 +122,9 @@ class Camera {
int width_;
int height_;
- GLTexture thumb_;
GLTexture texture1_; // first channel (always left at the moment)
GLTexture texture2_; // second channel ("right")
+ GLTexture depth1_;
ftl::gui::PoseWindow *posewin_;
//nlohmann::json meta_;
diff --git a/applications/gui/src/gltexture.cpp b/applications/gui/src/gltexture.cpp
index ec83997a3ba72ad744e4fd429ccc89151f24247a..5084c947c8b67fe4942c663c77fff12ffd752b68 100644
--- a/applications/gui/src/gltexture.cpp
+++ b/applications/gui/src/gltexture.cpp
@@ -10,13 +10,14 @@
using ftl::gui::GLTexture;
-GLTexture::GLTexture() {
+GLTexture::GLTexture(GLTexture::Type type) {
glid_ = std::numeric_limits<unsigned int>::max();
glbuf_ = std::numeric_limits<unsigned int>::max();
cuda_res_ = nullptr;
width_ = 0;
height_ = 0;
changed_ = true;
+ type_ = type;
}
GLTexture::~GLTexture() {
@@ -30,7 +31,11 @@ void GLTexture::update(cv::Mat &m) {
glGenTextures(1, &glid_);
glBindTexture(GL_TEXTURE_2D, glid_);
//cv::Mat m(cv::Size(100,100), CV_8UC3);
- glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, m.cols, m.rows, 0, GL_BGRA, GL_UNSIGNED_BYTE, m.data);
+ if (type_ == Type::BGRA) {
+ glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, m.cols, m.rows, 0, GL_BGRA, GL_UNSIGNED_BYTE, m.data);
+ } else if (type_ == Type::Float) {
+ glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, m.cols, m.rows, 0, GL_RED, GL_FLOAT, m.data);
+ }
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
@@ -60,13 +65,18 @@ void GLTexture::make(int width, int height) {
glGenTextures(1, &glid_);
glBindTexture(GL_TEXTURE_2D, glid_);
//cv::Mat m(cv::Size(100,100), CV_8UC3);
- glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, width, height, 0, GL_BGRA, GL_UNSIGNED_BYTE, nullptr);
+ //glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, width, height, 0, GL_BGRA, GL_UNSIGNED_BYTE, nullptr);
+ if (type_ == Type::BGRA) {
+ glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, width, height, 0, GL_BGRA, GL_UNSIGNED_BYTE, nullptr);
+ } else if (type_ == Type::Float) {
+ glTexImage2D(GL_TEXTURE_2D, 0, GL_R32F, width, height, 0, GL_RED, GL_FLOAT, nullptr);
+ }
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
- //auto err = glGetError();
- //if (err != 0) LOG(ERROR) << "OpenGL Texture error: " << err;
+ auto err = glGetError();
+ if (err != 0) LOG(ERROR) << "OpenGL Texture error: " << err;
glBindTexture(GL_TEXTURE_2D, 0);
@@ -100,7 +110,7 @@ cv::cuda::GpuMat GLTexture::map(cudaStream_t stream) {
size_t size;
cudaSafeCall(cudaGraphicsMapResources(1, &cuda_res_, stream));
cudaSafeCall(cudaGraphicsResourceGetMappedPointer(&devptr, &size, cuda_res_));
- return cv::cuda::GpuMat(height_, width_, CV_8UC4, devptr);
+ return cv::cuda::GpuMat(height_, width_, (type_ == Type::BGRA) ? CV_8UC4 : CV_32F, devptr);
}
void GLTexture::unmap(cudaStream_t stream) {
@@ -112,7 +122,11 @@ void GLTexture::unmap(cudaStream_t stream) {
// Select the appropriate texture
glBindTexture( GL_TEXTURE_2D, glid_);
// Make a texture from the buffer
- glTexSubImage2D( GL_TEXTURE_2D, 0, 0, 0, width_, height_, GL_BGRA, GL_UNSIGNED_BYTE, NULL);
+ if (type_ == Type::BGRA) {
+ glTexSubImage2D( GL_TEXTURE_2D, 0, 0, 0, width_, height_, GL_BGRA, GL_UNSIGNED_BYTE, NULL);
+ } else {
+ glTexSubImage2D( GL_TEXTURE_2D, 0, 0, 0, width_, height_, GL_RED, GL_FLOAT, NULL);
+ }
glBindBuffer( GL_PIXEL_UNPACK_BUFFER, 0);
}
diff --git a/applications/gui/src/gltexture.hpp b/applications/gui/src/gltexture.hpp
index c9e5146bbc26ebfd3175bd37ad1113d0f3360415..c263a055a4582ee45eac96acee9dd6303cea92fb 100644
--- a/applications/gui/src/gltexture.hpp
+++ b/applications/gui/src/gltexture.hpp
@@ -12,7 +12,13 @@ namespace gui {
class GLTexture {
public:
- GLTexture();
+ enum class Type {
+ RGBA,
+ BGRA,
+ Float
+ };
+
+ explicit GLTexture(Type);
~GLTexture();
void update(cv::Mat &m);
@@ -34,6 +40,7 @@ class GLTexture {
int width_;
int height_;
bool changed_;
+ Type type_;
cudaGraphicsResource *cuda_res_;
};
diff --git a/applications/gui/src/screen.cpp b/applications/gui/src/screen.cpp
index 7d144d5cbe727d110133281e5adec09b726ee3ab..7073e6aec85701d57685f0e2faec7326dd51d3dd 100644
--- a/applications/gui/src/screen.cpp
+++ b/applications/gui/src/screen.cpp
@@ -49,12 +49,14 @@ namespace {
R"(#version 330
uniform sampler2D image1;
uniform sampler2D image2;
+ uniform sampler2D depthImage;
uniform float blendAmount;
out vec4 color;
in vec2 uv;
void main() {
color = blendAmount * texture(image1, uv) + (1.0 - blendAmount) * texture(image2, uv);
color.w = 1.0f;
+ gl_FragDepth = texture(depthImage, uv).r;
})";
}
@@ -545,16 +547,25 @@ void ftl::gui::Screen::draw(NVGcontext *ctx) {
//camera_->getLeft().texture();
glActiveTexture(GL_TEXTURE1);
glBindTexture(GL_TEXTURE_2D, (camera_->isStereo() && camera_->getRight().isValid()) ? rightEye_ : leftEye_);
+ glActiveTexture(GL_TEXTURE2);
+ glBindTexture(GL_TEXTURE_2D, camera_->getDepth().texture());
//(camera_->isStereo() && camera_->getRight().isValid()) ? camera_->getRight().texture() : camera_->getLeft().texture();
mShader.setUniform("image1", 0);
mShader.setUniform("image2", 1);
+ mShader.setUniform("depthImage", 2);
mShader.setUniform("blendAmount", (camera_->isStereo()) ? root_->value("blending", 0.5f) : 1.0f);
mShader.setUniform("scaleFactor", scaleFactor);
mShader.setUniform("position", imagePosition);
+
+ glEnable(GL_DEPTH_TEST);
+ glDepthMask(GL_TRUE);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
mShader.drawIndexed(GL_TRIANGLES, 0, 2);
//glDisable(GL_SCISSOR_TEST);
-
glBindBuffer(GL_PIXEL_UNPACK_BUFFER, 0);
+
+ camera_->drawOverlay(screenSize);
}
} else {
// Must periodically render the cameras here to update any thumbnails.
diff --git a/applications/gui/src/screen.hpp b/applications/gui/src/screen.hpp
index 92c619551a3478773e44cc917f80dbe8cb2c23e8..c87ff2ff9ab3ee8780ba991a4d09b325c9225364 100644
--- a/applications/gui/src/screen.hpp
+++ b/applications/gui/src/screen.hpp
@@ -79,7 +79,7 @@ class Screen : public nanogui::Screen {
nanogui::GLShader mShader;
GLuint mImageID;
//Source *src_;
- GLTexture texture_;
+ //GLTexture texture_;
Eigen::Vector3f eye_;
Eigen::Vector4f neye_;
Eigen::Vector3f orientation_;
diff --git a/components/renderers/cpp/CMakeLists.txt b/components/renderers/cpp/CMakeLists.txt
index 516369fddf779c5949ca9ae1b2c79ce9fced866e..9706be7c62bc97045431b213cf0388c66d3a3b51 100644
--- a/components/renderers/cpp/CMakeLists.txt
+++ b/components/renderers/cpp/CMakeLists.txt
@@ -12,10 +12,17 @@ add_library(ftlrender
src/overlay.cpp
)
+# Various preprocessor definitions have been generated by NanoGUI
+add_definitions(${NANOGUI_EXTRA_DEFS})
+
+# On top of adding the path to nanogui/include, you may need extras
+include_directories(${NANOGUI_EXTRA_INCS})
+
target_include_directories(ftlrender PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/ext/nanogui/include>
$<INSTALL_INTERFACE:include>
PRIVATE src)
-target_link_libraries(ftlrender ftlrgbd ftlcommon Eigen3::Eigen Threads::Threads ${OpenCV_LIBS})
+target_link_libraries(ftlrender ftlrgbd ftlcommon Eigen3::Eigen Threads::Threads nanogui ${NANOGUI_EXTRA_LIBS} ${OpenCV_LIBS})
#ADD_SUBDIRECTORY(test)
diff --git a/components/renderers/cpp/include/ftl/render/overlay.hpp b/components/renderers/cpp/include/ftl/render/overlay.hpp
index 477b4419c8adb336693c5126bdd04d1b64c4c60b..bef452f0936c49807d4b722675c054ae5ce1ca80 100644
--- a/components/renderers/cpp/include/ftl/render/overlay.hpp
+++ b/components/renderers/cpp/include/ftl/render/overlay.hpp
@@ -4,19 +4,38 @@
#include <opencv2/core/mat.hpp>
#include <Eigen/Eigen>
#include <ftl/rgbd/frameset.hpp>
+#include <nanogui/glutil.h>
namespace ftl {
namespace overlay {
+enum class Shape {
+ BOX,
+ CAMERA,
+ PLANE,
+ GRID
+};
+
class Overlay : public ftl::Configurable {
public:
explicit Overlay(nlohmann::json &config);
~Overlay();
- void apply(ftl::rgbd::FrameSet &fs, cv::Mat &out, ftl::rgbd::FrameState &state);
+ //void apply(ftl::rgbd::FrameSet &fs, cv::Mat &out, ftl::rgbd::FrameState &state);
+
+ void draw(ftl::rgbd::FrameSet &fs, ftl::rgbd::FrameState &state, const Eigen::Vector2f &);
private:
- cv::Mat over_depth_;
+ nanogui::GLShader oShader;
+ bool init_;
+
+ std::vector<float3> shape_verts_;
+ std::vector<unsigned int> shape_tri_indices_;
+ std::unordered_map<ftl::overlay::Shape, std::tuple<int,int,int,int>> shapes_;
+
+ void _createShapes();
+ void _drawFilledShape(ftl::overlay::Shape shape, const Eigen::Matrix4d &pose, float scale, uchar4 colour);
+ void _drawOutlinedShape(Shape shape, const Eigen::Matrix4d &pose, const Eigen::Vector3f &scale, uchar4 fill, uchar4 outline);
};
void draw3DLine(
diff --git a/components/renderers/cpp/src/CUDARender.cpp b/components/renderers/cpp/src/CUDARender.cpp
index 9a62807cca29a5e296628138cbb63d88a398ca12..3affec4bdf172f61b88e7c24824996ac2e7fcd9b 100644
--- a/components/renderers/cpp/src/CUDARender.cpp
+++ b/components/renderers/cpp/src/CUDARender.cpp
@@ -332,13 +332,13 @@ void CUDARender::_allocateChannels(ftl::rgbd::Frame &out, ftl::codecs::Channel c
// Allocate left channel buffers and clear them
if (chan == Channel::Colour) {
- if (!out.hasChannel(Channel::Depth)) {
+ //if (!out.hasChannel(Channel::Depth)) {
out.create<GpuMat>(Channel::Depth, Format<float>(camera.width, camera.height));
out.create<GpuMat>(Channel::Colour, Format<uchar4>(camera.width, camera.height));
out.create<GpuMat>(Channel::Normals, Format<half4>(camera.width, camera.height));
out.createTexture<uchar4>(Channel::Colour, true); // Force interpolated colour
out.get<GpuMat>(Channel::Depth).setTo(cv::Scalar(1000.0f), cvstream);
- }
+ //}
// Allocate right channel buffers and clear them
} else {
if (!out.hasChannel(Channel::Depth2)) {
diff --git a/components/renderers/cpp/src/overlay.cpp b/components/renderers/cpp/src/overlay.cpp
index 697a5c23243f6a5b784ab51d92b3e852d5b81d00..436163f544e8148cd346f6745b758645b3b1c754 100644
--- a/components/renderers/cpp/src/overlay.cpp
+++ b/components/renderers/cpp/src/overlay.cpp
@@ -1,4 +1,5 @@
#include <ftl/render/overlay.hpp>
+#include <ftl/utility/matrix_conversion.hpp>
#include <opencv2/imgproc.hpp>
@@ -9,27 +10,222 @@
using ftl::overlay::Overlay;
using ftl::codecs::Channel;
+using ftl::overlay::Shape;
+
+namespace {
+ constexpr char const *const overlayVertexShader =
+ R"(#version 330
+ in vec3 vertex;
+ uniform float focal;
+ uniform float width;
+ uniform float height;
+ uniform float far;
+ uniform float near;
+ uniform mat4 pose;
+ uniform vec3 scale;
+
+ void main() {
+ 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);
+ gl_Position = pos;
+ })";
+
+ constexpr char const *const overlayFragmentShader =
+ R"(#version 330
+ uniform vec4 blockColour;
+ out vec4 color;
+
+ void main() {
+ color = blockColour;
+ })";
+}
Overlay::Overlay(nlohmann::json &config) : ftl::Configurable(config) {
-
+ init_ = false;
}
Overlay::~Overlay() {
}
-void Overlay::apply(ftl::rgbd::FrameSet &fs, cv::Mat &out, ftl::rgbd::FrameState &state) {
- over_depth_.create(out.size(), CV_32F);
+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},
+
+ // Plane Y simple
+ {-1.0, 0.0, -1.0},
+ {1.0, 0.0, -1.0},
+ {1.0, 0.0, 1.0},
+ {-1.0, 0.0, 1.0}
+ };
+
+ 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
+ };
+
+ shapes_[Shape::BOX] = {0,30, 30, 12*2};
+ shapes_[Shape::CAMERA] = {54, 4*3, 66, 8*2};
+
+ 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];
+ 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)));
+}
+
+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);
+ 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::draw(ftl::rgbd::FrameSet &fs, ftl::rgbd::FrameState &state, const Eigen::Vector2f &screenSize) {
+ double zfar = 8.0f;
+ auto intrin = state.getLeft();
+ intrin = intrin.scaled(screenSize[0], screenSize[1]);
+
+ if (!init_) {
+ oShader.init("OverlayShader", overlayVertexShader, overlayFragmentShader);
+ oShader.bind();
+ _createShapes();
+ init_ = true;
+ } else {
+ oShader.bind();
+ }
+
+ float3 tris[] = {
+ {0.5f, -0.7f, 2.0f},
+ {0.2f, -0.5f, 2.0f},
+ {0.8f, -0.4f, 2.0f}
+ };
+
+ auto pose = MatrixConversion::toCUDA(state.getPose().cast<float>().inverse());
+
+ tris[0] = pose * tris[0];
+ tris[1] = pose * tris[1];
+ tris[2] = pose * tris[2];
+
+ glFlush();
+
+ glDepthMask(GL_FALSE);
+ glEnable( GL_BLEND );
+ glBlendEquation( GL_FUNC_ADD );
+ glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
+ glEnable(GL_LINE_SMOOTH);
+
+ oShader.setUniform("focal", intrin.fx);
+ oShader.setUniform("width", float(intrin.width));
+ oShader.setUniform("height", float(intrin.height));
+ oShader.setUniform("far", zfar);
+ oShader.setUniform("near", 0.0f); // TODO: but make sure CUDA depth is also normalised like this
+
+ /*oShader.setUniform("blockColour", Eigen::Vector4f(1.0f,1.0f,0.0f,0.5f));
+ oShader.uploadAttrib("vertex", sizeof(tris), 3, sizeof(float), GL_FLOAT, false, tris);
+ oShader.drawArray(GL_TRIANGLES, 0, 3);
+
+ oShader.setUniform("blockColour", Eigen::Vector4f(1.0f,1.0f,0.0f,1.0f));
+ //oShader.uploadAttrib("vertex", sizeof(tris), 3, sizeof(float), GL_FLOAT, false, tris);
+ oShader.drawArray(GL_LINE_LOOP, 0, 3);*/
+
+ //glFinish();
if (value("show_poses", false)) {
for (size_t i=0; i<fs.frames.size(); ++i) {
- auto pose = fs.frames[i].getPose().inverse() * state.getPose();
- Eigen::Vector4d pos = pose.inverse() * Eigen::Vector4d(0,0,0,1);
- pos /= pos[3];
+ auto pose = fs.frames[i].getPose(); //.inverse() * state.getPose();
auto name = fs.frames[i].get<std::string>("name");
- 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));
+ _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));
+
+ //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));
}
}
@@ -39,13 +235,16 @@ void Overlay::apply(ftl::rgbd::FrameSet &fs, cv::Mat &out, ftl::rgbd::FrameState
fs.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>();
+ //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);
- 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::drawText(state.getLeft(), out, over_depth_, s.label, pos, 0.5, cv::Scalar(0,0,255,100));
+ _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::drawText(state.getLeft(), out, over_depth_, s.label, pos, 0.5, cv::Scalar(0,0,255,100));
}
}
@@ -59,13 +258,17 @@ void Overlay::apply(ftl::rgbd::FrameSet &fs, cv::Mat &out, ftl::rgbd::FrameState
Eigen::Vector4d pos = pose.inverse() * Eigen::Vector4d(0,0,0,1);
pos /= pos[3];
- ftl::overlay::drawBox(state.getLeft(), out, over_depth_, pose, cv::Scalar(0,0,255,100), s.size.cast<double>());
- ftl::overlay::drawText(state.getLeft(), out, over_depth_, s.label, pos, 0.5, cv::Scalar(0,0,255,100));
+ //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));
}
}
}
}
+ glDisable(GL_LINE_SMOOTH);
+ glDisable(GL_BLEND);
+ glDisable(GL_DEPTH_TEST);
+
//cv::flip(out, out, 0);
}
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 84a20c710b61d2282b658ea341005c789e0910a6..f2659128145d0908d75db567541ef974b5d14aa0 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -295,7 +295,7 @@ Camera Camera::scaled(int width, int height) const {
float scaleX = (float)width / (float)cam.width;
float scaleY = (float)height / (float)cam.height;
- CHECK( abs(scaleX - scaleY) < 0.00000001f );
+ //CHECK( abs(scaleX - scaleY) < 0.00000001f );
Camera newcam = cam;
newcam.width = width;