Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision

Target

Select target project
  • nicolaspope/ftl
1 result
Select Git revision
Show changes
......@@ -25,13 +25,14 @@ using ftl::render::CUDARender;
using ftl::codecs::Channel;
using ftl::codecs::Channels;
using ftl::rgbd::Format;
using ftl::rgbd::VideoFrame;
using cv::cuda::GpuMat;
using std::stoul;
using ftl::cuda::Mask;
using ftl::render::parseCUDAColour;
using ftl::render::parseCVColour;
CUDARender::CUDARender(nlohmann::json &config) : ftl::render::FSRenderer(config), scene_(nullptr) {
CUDARender::CUDARender(nlohmann::json &config) : ftl::render::FSRenderer(config), temp_d_(ftl::data::Frame::make_standalone()), temp_(temp_d_.cast<ftl::rgbd::Frame>()), scene_(nullptr) {
/*if (config["clipping"].is_object()) {
auto &c = config["clipping"];
float rx = c.value("pitch", 0.0f);
......@@ -59,27 +60,29 @@ CUDARender::CUDARender(nlohmann::json &config) : ftl::render::FSRenderer(config)
colouriser_ = ftl::create<ftl::render::Colouriser>(this, "colouriser");
on("clipping_enabled", [this](const ftl::config::Event &e) {
on("touch_sensitivity", touch_dist_, 0.04f);
on("clipping_enabled", [this]() {
clipping_ = value("clipping_enabled", true);
});
norm_filter_ = value("normal_filter", -1.0f);
on("normal_filter", [this](const ftl::config::Event &e) {
on("normal_filter", [this]() {
norm_filter_ = value("normal_filter", -1.0f);
});
backcull_ = value("back_cull", true);
on("back_cull", [this](const ftl::config::Event &e) {
on("back_cull", [this]() {
backcull_ = value("back_cull", true);
});
mesh_ = value("meshing", true);
on("meshing", [this](const ftl::config::Event &e) {
on("meshing", [this]() {
mesh_ = value("meshing", true);
});
background_ = parseCVColour(value("background", std::string("#4c4c4c")));
on("background", [this](const ftl::config::Event &e) {
on("background", [this]() {
background_ = parseCVColour(value("background", std::string("#4c4c4c")));
});
......@@ -96,12 +99,18 @@ CUDARender::CUDARender(nlohmann::json &config) : ftl::render::FSRenderer(config)
}
}
cudaSafeCall(cudaStreamCreate(&stream_));
//cudaSafeCall(cudaStreamCreate(&stream_));
stream_ = 0;
last_frame_ = -1;
temp_.store();
// Allocate collisions buffer
cudaSafeCall(cudaMalloc(&collisions_, 1024*sizeof(ftl::cuda::Collision)));
}
CUDARender::~CUDARender() {
delete colouriser_;
cudaFree(collisions_);
}
void CUDARender::_renderChannel(ftl::rgbd::Frame &output, ftl::codecs::Channel in, const Eigen::Matrix4d &t, cudaStream_t stream) {
......@@ -110,12 +119,12 @@ void CUDARender::_renderChannel(ftl::rgbd::Frame &output, ftl::codecs::Channel i
if (in == Channel::None) return;
for (size_t i=0; i < scene_->frames.size(); ++i) {
if (!scene_->hasFrame(i)) continue;
auto &f = scene_->frames[i];
//if (!scene_->hasFrame(i)) continue;
auto &f = scene_->frames[i].cast<ftl::rgbd::Frame>();
if (!f.hasChannel(in)) {
LOG(ERROR) << "Reprojecting unavailable channel";
return;
//LOG(ERROR) << "Reprojecting unavailable channel";
continue;
}
_adjustDepthThresholds(f.getLeftCamera());
......@@ -169,14 +178,14 @@ void CUDARender::_renderChannel(ftl::rgbd::Frame &output, ftl::codecs::Channel i
void CUDARender::_dibr(ftl::rgbd::Frame &out, const Eigen::Matrix4d &t, cudaStream_t stream) {
cv::cuda::Stream cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
temp_.get<GpuMat>(Channel::Depth2).setTo(cv::Scalar(0x7FFFFFFF), cvstream);
temp_.set<GpuMat>(Channel::Depth2).setTo(cv::Scalar(0x7FFFFFFF), cvstream);
for (size_t i=0; i < scene_->frames.size(); ++i) {
if (!scene_->hasFrame(i)) continue;
auto &f = scene_->frames[i];
auto &f = scene_->frames[i].cast<ftl::rgbd::Frame>();
//auto *s = scene_->sources[i];
if (f.empty(Channel::Colour)) {
if (!f.has(Channel::Colour)) {
LOG(ERROR) << "Missing required channel";
continue;
}
......@@ -233,23 +242,35 @@ void CUDARender::_mesh(ftl::rgbd::Frame &out, const Eigen::Matrix4d &t, cudaStre
bool do_blend = value("mesh_blend", false);
float blend_alpha = value("blend_alpha", 0.02f);
if (do_blend) {
temp_.get<GpuMat>(Channel::Depth).setTo(cv::Scalar(0x7FFFFFFF), cvstream);
temp_.get<GpuMat>(Channel::Weights).setTo(cv::Scalar(0.0f), cvstream);
temp_.set<GpuMat>(Channel::Depth).setTo(cv::Scalar(0x7FFFFFFF), cvstream);
temp_.set<GpuMat>(Channel::Weights).setTo(cv::Scalar(0.0f), cvstream);
} else {
temp_.get<GpuMat>(Channel::Depth2).setTo(cv::Scalar(0x7FFFFFFF), cvstream);
temp_.set<GpuMat>(Channel::Depth2).setTo(cv::Scalar(0x7FFFFFFF), cvstream);
}
int valid_count = 0;
// FIXME: Is it possible to remember previously if there should be depth?
bool use_depth = scene_->anyHasChannel(Channel::Depth) || scene_->anyHasChannel(Channel::GroundTruth);
// For each source depth map
for (size_t i=0; i < scene_->frames.size(); ++i) {
if (!scene_->hasFrame(i)) continue;
auto &f = scene_->frames[i];
//if (!scene_->hasFrame(i)) continue;
auto &f = scene_->frames[i].cast<ftl::rgbd::Frame>();
//auto *s = scene_->sources[i];
if (f.empty(Channel::Colour)) {
LOG(ERROR) << "Missing required channel";
if (!f.has(Channel::Colour)) {
//LOG(ERROR) << "Missing required channel";
continue;
}
// We have the needed depth data?
if (use_depth && !f.hasOwn(Channel::Depth) && !f.hasOwn(Channel::GroundTruth)) {
continue;
}
++valid_count;
//auto pose = MatrixConversion::toCUDA(t.cast<float>() * f.getPose().cast<float>());
auto transform = pose_ * MatrixConversion::toCUDA(t.cast<float>() * f.getPose().cast<float>());
......@@ -260,20 +281,22 @@ void CUDARender::_mesh(ftl::rgbd::Frame &out, const Eigen::Matrix4d &t, cudaStre
auto &screenbuffer = _getScreenBuffer(bufsize);
// Calculate and save virtual view screen position of each source pixel
if (f.hasChannel(Channel::Depth)) {
ftl::cuda::screen_coord(
f.createTexture<float>(Channel::Depth),
depthbuffer,
screenbuffer,
params_, transform, f.getLeftCamera(), stream
);
} else if (f.hasChannel(Channel::GroundTruth)) {
ftl::cuda::screen_coord(
f.createTexture<float>(Channel::GroundTruth),
depthbuffer,
screenbuffer,
params_, transform, f.getLeftCamera(), stream
);
if (use_depth) {
if (f.hasChannel(Channel::Depth)) {
ftl::cuda::screen_coord(
f.createTexture<float>(Channel::Depth),
depthbuffer,
screenbuffer,
params_, transform, f.getLeftCamera(), stream
);
} else if (f.hasChannel(Channel::GroundTruth)) {
ftl::cuda::screen_coord(
f.createTexture<float>(Channel::GroundTruth),
depthbuffer,
screenbuffer,
params_, transform, f.getLeftCamera(), stream
);
}
} else {
// Constant depth version
ftl::cuda::screen_coord(
......@@ -285,9 +308,11 @@ void CUDARender::_mesh(ftl::rgbd::Frame &out, const Eigen::Matrix4d &t, cudaStre
// Must reset depth channel if blending
if (do_blend) {
temp_.get<GpuMat>(Channel::Depth).setTo(cv::Scalar(0x7FFFFFFF), cvstream);
temp_.set<GpuMat>(Channel::Depth).setTo(cv::Scalar(0x7FFFFFFF), cvstream);
}
depth_out_.to_gpumat().setTo(cv::Scalar(1000.0f), cvstream);
// Decide on and render triangles around each point
ftl::cuda::triangle_render1(
depthbuffer,
......@@ -303,7 +328,8 @@ void CUDARender::_mesh(ftl::rgbd::Frame &out, const Eigen::Matrix4d &t, cudaStre
// Blend this sources mesh with previous meshes
ftl::cuda::mesh_blender(
temp_.getTexture<int>(Channel::Depth),
out.createTexture<float>(_getDepthChannel()),
//out.createTexture<float>(_getDepthChannel()),
depth_out_,
f.createTexture<short>(Channel::Weights),
temp_.createTexture<float>(Channel::Weights),
params_,
......@@ -315,20 +341,28 @@ void CUDARender::_mesh(ftl::rgbd::Frame &out, const Eigen::Matrix4d &t, cudaStre
}
}
if (valid_count == 0) return;
// Convert from int depth to float depth
//temp_.get<GpuMat>(Channel::Depth2).convertTo(out.get<GpuMat>(Channel::Depth), CV_32F, 1.0f / 100000.0f, cvstream);
if (do_blend) {
ftl::cuda::dibr_normalise(
out.getTexture<float>(_getDepthChannel()),
out.getTexture<float>(_getDepthChannel()),
//out.getTexture<float>(_getDepthChannel()),
//out.getTexture<float>(_getDepthChannel()),
depth_out_,
depth_out_,
temp_.getTexture<float>(Channel::Weights),
stream_
);
} else {
ftl::cuda::merge_convert_depth(temp_.getTexture<int>(Channel::Depth2), out.createTexture<float>(_getDepthChannel()), 1.0f / 100000.0f, stream_);
//ftl::cuda::merge_convert_depth(temp_.getTexture<int>(Channel::Depth2), out.createTexture<float>(_getDepthChannel()), 1.0f / 100000.0f, stream_);
ftl::cuda::merge_convert_depth(temp_.getTexture<int>(Channel::Depth2), depth_out_, 1.0f / 100000.0f, stream_);
}
// Now merge new render to any existing frameset render, detecting collisions
ftl::cuda::touch_merge(depth_out_, out.createTexture<float>(_getDepthChannel()), collisions_, 1024, touch_dist_, stream_);
//filters_->filter(out, src, stream);
// Generate normals for final virtual image
......@@ -347,29 +381,30 @@ 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)) {
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);
out.create<VideoFrame>(Channel::Depth).createGPU(Format<float>(camera.width, camera.height));
out.create<VideoFrame>(Channel::Colour).createGPU(Format<uchar4>(camera.width, camera.height));
out.create<VideoFrame>(Channel::Normals).createGPU(Format<half4>(camera.width, camera.height));
out.createTexture<uchar4>(Channel::Colour, ftl::rgbd::Format<uchar4>(camera.width, camera.height), true); // Force interpolated colour
out.set<GpuMat>(Channel::Depth).setTo(cv::Scalar(1000.0f), cvstream);
//}
// Allocate right channel buffers and clear them
} else {
if (!out.hasChannel(Channel::Depth2)) {
out.create<GpuMat>(Channel::Depth2, Format<float>(camera.width, camera.height));
out.create<GpuMat>(Channel::Colour2, Format<uchar4>(camera.width, camera.height));
out.create<GpuMat>(Channel::Normals2, Format<half4>(camera.width, camera.height));
out.createTexture<uchar4>(Channel::Colour2, true); // Force interpolated colour
out.get<GpuMat>(Channel::Depth2).setTo(cv::Scalar(1000.0f), cvstream);
}
//if (!out.hasChannel(Channel::Depth2)) {
out.create<VideoFrame>(Channel::Depth2).createGPU(Format<float>(camera.width, camera.height));
out.create<VideoFrame>(Channel::Colour2).createGPU(Format<uchar4>(camera.width, camera.height));
out.create<VideoFrame>(Channel::Normals2).createGPU(Format<half4>(camera.width, camera.height));
out.createTexture<uchar4>(Channel::Colour2, ftl::rgbd::Format<uchar4>(camera.width, camera.height), true); // Force interpolated colour
out.set<GpuMat>(Channel::Depth2).setTo(cv::Scalar(1000.0f), cvstream);
//}
}
temp_.create<GpuMat>(Channel::Depth, Format<int>(camera.width, camera.height));
temp_.create<GpuMat>(Channel::Depth2, Format<int>(camera.width, camera.height));
temp_.create<GpuMat>(Channel::Normals, Format<half4>(camera.width, camera.height));
temp_.create<GpuMat>(Channel::Weights, Format<float>(camera.width, camera.height));
temp_.create<VideoFrame>(Channel::Depth).createGPU(Format<int>(camera.width, camera.height));
temp_.create<VideoFrame>(Channel::Depth2).createGPU(Format<int>(camera.width, camera.height));
temp_.create<VideoFrame>(Channel::Normals).createGPU(Format<half4>(camera.width, camera.height));
temp_.create<VideoFrame>(Channel::Weights).createGPU(Format<float>(camera.width, camera.height));
temp_.createTexture<int>(Channel::Depth);
depth_out_.create(camera.width, camera.height);
accum_.create(camera.width, camera.height);
contrib_.create(camera.width, camera.height);
......@@ -404,6 +439,7 @@ void CUDARender::_updateParameters(ftl::rgbd::Frame &out, ftl::codecs::Channel c
params_.disconDisparities = value("discon_disparities", 2.0f);
params_.accumulationMode = static_cast<ftl::render::AccumulationFunction>(value("accumulation_func", 0));
params_.m_flags = 0;
params_.projection = static_cast<ftl::rgbd::Projection>(value("projection",0));
if (value("normal_weight_colours", true)) params_.m_flags |= ftl::render::kNormalWeightColours;
if (value("channel_weights", false)) params_.m_flags |= ftl::render::kUseWeightsChannel;
}
......@@ -417,7 +453,7 @@ void CUDARender::_postprocessColours(ftl::rgbd::Frame &out) {
out.getTexture<half4>(_getNormalsChannel()),
out.getTexture<uchar4>(out_chan_),
col, pose,
stream_
stream_
);
}
......@@ -437,7 +473,7 @@ void CUDARender::_postprocessColours(ftl::rgbd::Frame &out) {
params_.camera,
stream_
);
} else if (out.hasChannel(_getDepthChannel()) && out.hasChannel(out_chan_)) {
} else if (mesh_ && out.hasChannel(_getDepthChannel()) && out.hasChannel(out_chan_)) {
ftl::cuda::fix_bad_colour(
out.getTexture<float>(_getDepthChannel()),
out.getTexture<uchar4>(out_chan_),
......@@ -466,17 +502,27 @@ void CUDARender::_renderPass2(Channels<0> chans, const Eigen::Matrix4d &t) {
for (auto chan : chans) {
ftl::codecs::Channel mapped = chan;
if (chan == Channel::Colour && scene_->firstFrame().hasChannel(Channel::ColourHighRes)) mapped = Channel::ColourHighRes;
// FIXME: Doesn't seem to work
//if (chan == Channel::Colour && scene_->firstFrame().hasChannel(Channel::ColourHighRes)) mapped = Channel::ColourHighRes;
_renderChannel(*out_, mapped, t, stream_);
}
}
void CUDARender::cancel() {
out_ = nullptr;
scene_ = nullptr;
stage_ = Stage::Finished;
cudaSafeCall(cudaStreamSynchronize(stream_));
}
void CUDARender::begin(ftl::rgbd::Frame &out, ftl::codecs::Channel chan) {
if (stage_ != Stage::Finished) {
throw FTL_Error("Already rendering");
}
stream_ = out.stream();
out_ = &out;
const auto &camera = out.getLeftCamera();
cv::cuda::Stream cvstream = cv::cuda::StreamAccessor::wrapStream(stream_);
......@@ -491,7 +537,7 @@ void CUDARender::begin(ftl::rgbd::Frame &out, ftl::codecs::Channel chan) {
// Apply a colour background
if (env_image_.empty() || !value("environment_enabled", false)) {
out.get<GpuMat>(chan).setTo(background_, cvstream);
out.set<GpuMat>(chan).setTo(background_, cvstream);
} else {
auto pose = poseInverse_.getFloat3x3();
ftl::cuda::equirectangular_reproject(
......@@ -502,6 +548,9 @@ void CUDARender::begin(ftl::rgbd::Frame &out, ftl::codecs::Channel chan) {
sets_.clear();
stage_ = Stage::ReadySubmit;
// Reset collision data.
cudaSafeCall(cudaMemsetAsync(collisions_, 0, sizeof(int), stream_));
}
void CUDARender::render() {
......@@ -566,14 +615,45 @@ void CUDARender::_endSubmit() {
void CUDARender::_end() {
_postprocessColours(*out_);
// Final OpenGL flip
ftl::cuda::flip(out_->getTexture<uchar4>(out_chan_), stream_);
ftl::cuda::flip(out_->getTexture<float>(_getDepthChannel()), stream_);
// Final OpenGL flip (easier to do in shader?)
/*ftl::cuda::flip(out_->getTexture<uchar4>(out_chan_), stream_);*/
/*ftl::cuda::flip(out_->getTexture<float>(_getDepthChannel()), stream_);*/
cudaSafeCall(cudaMemcpyAsync(collisions_host_, collisions_, sizeof(ftl::cuda::Collision)*1024, cudaMemcpyDeviceToHost, stream_));
cudaSafeCall(cudaStreamSynchronize(stream_));
// Convert collisions into camera coordinates.
collision_points_.resize(collisions_host_[0].screen);
for (uint i=1; i<collisions_host_[0].screen+1; ++i) {
collision_points_[i-1] = make_float4(collisions_host_[i].x(), collisions_host_[i].y(), collisions_host_[i].depth, collisions_host_[i].strength());
}
// Do something with the collisions
/*if (collisions_host_[0].screen > 0) {
float x = 0.0f;
float y = 0.0f;
float d = 0.0f;
float w = 0.0f;
for (uint i=1; i<collisions_host_[0].screen+1; ++i) {
float inum = collisions_host_[i].strength();
int ix = collisions_host_[i].x();
int iy = collisions_host_[i].y();
x += float(ix)*inum;
y += float(iy)*inum;
d += collisions_host_[i].depth*inum;
w += inum;
}
x /= w;
y /= w;
d /= w;
LOG(INFO) << "Collision at: " << x << "," << y << ", " << d;
}*/
}
bool CUDARender::submit(ftl::rgbd::FrameSet *in, Channels<0> chans, const Eigen::Matrix4d &t) {
bool CUDARender::submit(ftl::data::FrameSet *in, Channels<0> chans, const Eigen::Matrix4d &t) {
if (stage_ != Stage::ReadySubmit) {
throw FTL_Error("Renderer not ready for submits");
}
......@@ -587,9 +667,8 @@ bool CUDARender::submit(ftl::rgbd::FrameSet *in, Channels<0> chans, const Eigen:
bool success = true;
try {
_renderPass1(in->pose);
//cudaSafeCall(cudaStreamSynchronize(stream_));
} catch (std::exception &e) {
_renderPass1(t);
} catch (const ftl::exception &e) {
LOG(ERROR) << "Exception in render: " << e.what();
success = false;
}
......@@ -597,9 +676,9 @@ 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 = in->pose;
s.transform = t;
last_frame_ = scene_->timestamp;
last_frame_ = scene_->timestamp();
scene_ = nullptr;
return success;
}
......@@ -27,12 +27,15 @@ __global__ void clipping_kernel(ftl::cuda::TextureObject<float> depth, ftl::cuda
const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
if (x < depth.width() && y < depth.height()) {
float d = depth(x,y);
float4 p = make_float4(camera.screenToCam(x,y,d), 0.0f);
const float sx = float(x) / float(colour.width()) * float(depth.width());
const float sy = float(y) / float(colour.height()) * float(depth.height());
if (sx >= 0.0f && sx < depth.width() && sy < depth.height() && sy >= 0.0f) {
float d = depth(sx,sy);
float4 p = make_float4(camera.screenToCam(sx,sy,d), 0.0f);
if (d <= camera.minDepth || d >= camera.maxDepth || isClipped(p, clip)) {
depth(x,y) = 0.0f;
depth(sx,sy) = 0.0f;
colour(x,y) = make_uchar4(0,0,0,0);
}
}
......@@ -54,7 +57,7 @@ void ftl::cuda::clipping(ftl::cuda::TextureObject<float> &depth,
const ftl::rgbd::Camera &camera,
const ClipSpace &clip, cudaStream_t stream) {
const dim3 gridSize((depth.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (depth.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
const dim3 gridSize((colour.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (colour.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
clipping_kernel<<<gridSize, blockSize, 0, stream>>>(depth, colour, camera, clip);
......
......@@ -2,6 +2,7 @@
#include "splatter_cuda.hpp"
#include <ftl/cuda/colour_cuda.hpp>
#include <ftl/cuda/normals.hpp>
#include <ftl/operators/cuda/mask.hpp>
#include <opencv2/cudaarithm.hpp>
#include <opencv2/cudaimgproc.hpp>
......@@ -113,9 +114,13 @@ Colouriser::~Colouriser() {
}
TextureObject<uchar4> &Colouriser::colourise(ftl::rgbd::Frame &f, Channel c, cudaStream_t stream) {
const auto &vf = f.get<ftl::rgbd::VideoFrame>(c);
if (!vf.isGPU()) {
f.upload(c);
}
switch (c) {
case Channel::Overlay : return f.createTexture<uchar4>(c);
case Channel::ColourHighRes :
case Channel::Overlay :
case Channel::Colour :
case Channel::Colour2 : return _processColour(f,c,stream);
case Channel::GroundTruth :
......@@ -183,7 +188,7 @@ TextureObject<uchar4> &Colouriser::_processColour(ftl::rgbd::Frame &f, Channel c
bool colour_sources = value("colour_sources", false);
if (!colour_sources && show_mask == 0) {
return f.createTexture<uchar4>(c);
return f.createTexture<uchar4>(c, true);
}
cv::cuda::Stream cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
......@@ -192,7 +197,7 @@ TextureObject<uchar4> &Colouriser::_processColour(ftl::rgbd::Frame &f, Channel c
auto &buf = _getBuffer(size.width, size.height);
if (colour_sources) {
auto colour = HSVtoRGB(360 / 8 * f.id, 0.6, 0.85);
auto colour = HSVtoRGB(360 / 8 * f.source(), 0.6, 0.85);
buf.to_gpumat().setTo(colour, cvstream);
}
......
......@@ -9,10 +9,12 @@
using ftl::cuda::TextureObject;
using ftl::render::Parameters;
using ftl::rgbd::Projection;
/*
* DIBR point cloud with a depth check
*/
template <Projection PROJECT>
__global__ void dibr_merge_kernel(TextureObject<float> depth,
TextureObject<int> depth_out,
float4x4 transform,
......@@ -26,11 +28,13 @@ using ftl::render::Parameters;
const float3 camPos = transform * cam.screenToCam(x,y,d0);
const float d = camPos.z;
//const float d = camPos.z;
const uint2 screenPos = params.camera.camToScreen<uint2>(camPos);
const unsigned int cx = screenPos.x;
const unsigned int cy = screenPos.y;
//const uint2 screenPos = params.camera.camToScreen<uint2>(camPos);
const float3 screenPos = params.camera.project<PROJECT>(camPos);
const unsigned int cx = (unsigned int)(screenPos.x+0.5f);
const unsigned int cy = (unsigned int)(screenPos.y+0.5f);
const float d = screenPos.z;
if (d > params.camera.minDepth && d < params.camera.maxDepth && cx < depth_out.width() && cy < depth_out.height()) {
// Transform estimated point to virtual cam space and output z
atomicMin(&depth_out(cx,cy), d * 100000.0f);
......@@ -67,7 +71,11 @@ void ftl::cuda::dibr_merge(TextureObject<float> &depth, TextureObject<int> &dept
const dim3 gridSize((depth.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (depth.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
dibr_merge_kernel<<<gridSize, blockSize, 0, stream>>>(depth, depth_out, transform, cam, params);
if (params.projection == Projection::PERSPECTIVE) {
dibr_merge_kernel<Projection::PERSPECTIVE><<<gridSize, blockSize, 0, stream>>>(depth, depth_out, transform, cam, params);
} else {
dibr_merge_kernel<Projection::ORTHOGRAPHIC><<<gridSize, blockSize, 0, stream>>>(depth, depth_out, transform, cam, params);
}
cudaSafeCall( cudaGetLastError() );
}
......
#include "gltexture.hpp"
#include <ftl/utility/gltexture.hpp>
#include <nanogui/opengl.h>
#include <loguru.hpp>
#include <ftl/cuda_common.hpp>
#include <cuda_gl_interop.h>
#include <opencv2/core/cuda_stream_accessor.hpp>
#include <ftl/exception.hpp>
using ftl::gui::GLTexture;
void log_error() {
auto err = glGetError();
if (err != 0) LOG(ERROR) << "OpenGL Texture error: " << err;
}
using ftl::utility::GLTexture;
GLTexture::GLTexture(GLTexture::Type type) {
GLTexture::GLTexture() {
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;
type_ = Type::RGBA;
}
GLTexture::~GLTexture() {
//glDeleteTextures(1, &glid_);
free(); // Note: Do not simply remove this...
}
void GLTexture::update(cv::Mat &m) {
LOG(INFO) << "DEPRECATED";
if (m.rows == 0) return;
if (glid_ == std::numeric_limits<unsigned int>::max()) {
glGenTextures(1, &glid_);
glBindTexture(GL_TEXTURE_2D, glid_);
//cv::Mat m(cv::Size(100,100), CV_8UC3);
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);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
} else {
//glBindTexture(GL_TEXTURE_2D, glid_);
// TODO Allow for other formats
//glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, m.cols, m.rows, 0, GL_BGRA, GL_UNSIGNED_BYTE, m.data);
}
auto err = glGetError();
if (err != 0) LOG(ERROR) << "OpenGL Texture error: " << err;
}
void GLTexture::make(int width, int height) {
if (width != width_ || height != height_) {
void GLTexture::make(int width, int height, Type type) {
if (width != width_ || height != height_ || type_ != type) {
free();
}
......@@ -58,31 +39,33 @@ void GLTexture::make(int width, int height) {
width_ = width;
height_ = height;
stride_ = ((width*4) % ALIGNMENT != 0) ? ((width*4) + (ALIGNMENT - ((width*4) % ALIGNMENT))) / 4 : width;
stride_ = ((width*4) % ALIGNMENT != 0) ?
((width*4) + (ALIGNMENT - ((width*4) % ALIGNMENT))) / 4:
width;
type_ = type;
if (width == 0 || height == 0) {
throw FTL_Error("Invalid texture size");
}
if (glid_ == std::numeric_limits<unsigned int>::max()) {
glGenTextures(1, &glid_);
glBindTexture(GL_TEXTURE_2D, glid_);
glPixelStorei(GL_UNPACK_ROW_LENGTH, stride_);
glGenTextures(1, &glid_); log_error();
glBindTexture(GL_TEXTURE_2D, glid_); log_error();
glPixelStorei(GL_UNPACK_ROW_LENGTH, stride_); log_error();
//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);
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);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, width, height, 0, GL_BGRA, GL_UNSIGNED_BYTE, nullptr);
}
log_error();
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;
log_error();
glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
......@@ -91,11 +74,12 @@ void GLTexture::make(int width, int height) {
glGenBuffers(1, &glbuf_);
// Make this the current UNPACK buffer (OpenGL is state-based)
glBindBuffer(GL_PIXEL_UNPACK_BUFFER, glbuf_);
// Allocate data for the buffer. 4-channel 8-bit image
// Allocate data for the buffer. 4-channel 8-bit image or 1-channel float
glBufferData(GL_PIXEL_UNPACK_BUFFER, stride_ * height * 4, NULL, GL_DYNAMIC_COPY);
cudaSafeCall(cudaGraphicsGLRegisterBuffer(&cuda_res_, glbuf_, cudaGraphicsRegisterFlagsWriteDiscard));
glBindBuffer(GL_PIXEL_UNPACK_BUFFER, 0);
log_error();
}
}
......@@ -114,6 +98,7 @@ void GLTexture::free() {
}
cv::cuda::GpuMat GLTexture::map(cudaStream_t stream) {
mtx_.lock();
void *devptr;
size_t size;
cudaSafeCall(cudaGraphicsMapResources(1, &cuda_res_, stream));
......@@ -122,23 +107,27 @@ cv::cuda::GpuMat GLTexture::map(cudaStream_t stream) {
}
void GLTexture::unmap(cudaStream_t stream) {
// note: code must not throw, otherwise mtx_.unlock() does not happen
cudaSafeCall(cudaGraphicsUnmapResources(1, &cuda_res_, stream));
changed_ = true;
//glActiveTexture(GL_TEXTURE0);
glBindBuffer( GL_PIXEL_UNPACK_BUFFER, glbuf_);
glBindBuffer(GL_PIXEL_UNPACK_BUFFER, glbuf_);
// Select the appropriate texture
glBindTexture( GL_TEXTURE_2D, glid_);
glBindTexture(GL_TEXTURE_2D, glid_);
glPixelStorei(GL_UNPACK_ROW_LENGTH, stride_);
// Make a texture from the buffer
if (type_ == Type::BGRA) {
glTexSubImage2D( GL_TEXTURE_2D, 0, 0, 0, width_, height_, GL_BGRA, GL_UNSIGNED_BYTE, NULL);
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);
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width_, height_, GL_RED, GL_FLOAT, NULL);
}
glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
glBindTexture(GL_TEXTURE_2D, 0);
glBindBuffer( GL_PIXEL_UNPACK_BUFFER, 0);
glBindBuffer(GL_PIXEL_UNPACK_BUFFER, 0);
mtx_.unlock();
}
unsigned int GLTexture::texture() const {
......@@ -153,6 +142,47 @@ unsigned int GLTexture::texture() const {
return glid_;
} else {
return glid_;
throw FTL_Error("No OpenGL texture; use make() first");
}
}
void GLTexture::copyFrom(const ftl::cuda::TextureObject<uchar4> &buffer, cudaStream_t stream) {
if (buffer.width() == 0 || buffer.height() == 0) {
return;
}
make(buffer.width(), buffer.height(), ftl::utility::GLTexture::Type::BGRA);
auto dst = map(stream);
cudaSafeCall(cudaMemcpy2D( dst.data, dst.step, buffer.devicePtr(), buffer.pitch(),
buffer.width()*4, buffer.height(), cudaMemcpyDeviceToDevice));
unmap(stream);
}
void GLTexture::copyFrom(const cv::Mat &im, cudaStream_t stream) {
if (im.rows == 0 || im.cols == 0 || im.channels() != 4 || im.type() != CV_8UC4) {
LOG(ERROR) << __FILE__ << ":" << __LINE__ << ": " << "bad OpenCV format";
return;
}
auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
make(im.cols, im.rows, ftl::utility::GLTexture::Type::BGRA);
auto dst = map(stream);
dst.upload(im);
unmap(stream);
}
void GLTexture::copyFrom(const cv::cuda::GpuMat &im, cudaStream_t stream) {
if (im.rows == 0 || im.cols == 0 || im.channels() != 4 || im.type() != CV_8UC4) {
LOG(ERROR) << __FILE__ << ":" << __LINE__ << ": " << "bad OpenCV format";
return;
}
auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
make(im.cols, im.rows, ftl::utility::GLTexture::Type::BGRA);
auto dst = map(stream);
im.copyTo(dst, cvstream);
unmap(stream);
}