Skip to content
Snippets Groups Projects
Commit 50d8d9a6 authored by Nicolas Pope's avatar Nicolas Pope
Browse files

Merge branch 'feature/133/ilw' into 'master'

Implements #133 point alignment

Closes #133

See merge request nicolas.pope/ftl!116
parents 76f6202b d6e1198f
No related branches found
No related tags found
1 merge request!116Implements #133 point alignment
Pipeline #14781 passed
Showing with 171 additions and 60 deletions
...@@ -22,6 +22,7 @@ ILW::ILW(nlohmann::json &config) : ftl::Configurable(config) { ...@@ -22,6 +22,7 @@ ILW::ILW(nlohmann::json &config) : ftl::Configurable(config) {
params_.colour_smooth = value("colour_smooth", 50.0f); params_.colour_smooth = value("colour_smooth", 50.0f);
params_.spatial_smooth = value("spatial_smooth", 0.04f); params_.spatial_smooth = value("spatial_smooth", 0.04f);
params_.cost_ratio = value("cost_ratio", 0.75f); params_.cost_ratio = value("cost_ratio", 0.75f);
discon_mask_ = value("discontinuity_mask",2);
on("ilw_align", [this](const ftl::config::Event &e) { on("ilw_align", [this](const ftl::config::Event &e) {
enabled_ = value("ilw_align", true); enabled_ = value("ilw_align", true);
...@@ -39,6 +40,10 @@ ILW::ILW(nlohmann::json &config) : ftl::Configurable(config) { ...@@ -39,6 +40,10 @@ ILW::ILW(nlohmann::json &config) : ftl::Configurable(config) {
motion_window_ = value("motion_window", 3); motion_window_ = value("motion_window", 3);
}); });
on("discontinuity_mask", [this](const ftl::config::Event &e) {
discon_mask_ = value("discontinuity_mask", 2);
});
on("use_Lab", [this](const ftl::config::Event &e) { on("use_Lab", [this](const ftl::config::Event &e) {
use_lab_ = value("use_Lab", false); use_lab_ = value("use_Lab", false);
}); });
...@@ -116,7 +121,7 @@ bool ILW::_phase0(ftl::rgbd::FrameSet &fs, cudaStream_t stream) { ...@@ -116,7 +121,7 @@ bool ILW::_phase0(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
auto &t = f.createTexture<float4>(Channel::Points, Format<float4>(f.get<GpuMat>(Channel::Colour).size())); auto &t = f.createTexture<float4>(Channel::Points, Format<float4>(f.get<GpuMat>(Channel::Colour).size()));
auto pose = MatrixConversion::toCUDA(s->getPose().cast<float>()); //.inverse()); auto pose = MatrixConversion::toCUDA(s->getPose().cast<float>()); //.inverse());
ftl::cuda::point_cloud(t, f.createTexture<float>(Channel::Depth), s->parameters(), pose, stream); ftl::cuda::point_cloud(t, f.createTexture<float>(Channel::Depth), s->parameters(), pose, discon_mask_, stream);
// TODO: Create energy vector texture and clear it // TODO: Create energy vector texture and clear it
// Create energy and clear it // Create energy and clear it
......
...@@ -69,6 +69,7 @@ class ILW : public ftl::Configurable { ...@@ -69,6 +69,7 @@ class ILW : public ftl::Configurable {
float motion_rate_; float motion_rate_;
int motion_window_; int motion_window_;
bool use_lab_; bool use_lab_;
int discon_mask_;
}; };
} }
......
...@@ -136,6 +136,9 @@ static void run(ftl::Configurable *root) { ...@@ -136,6 +136,9 @@ static void run(ftl::Configurable *root) {
ftl::rgbd::Group group; ftl::rgbd::Group group;
ftl::ILW *align = ftl::create<ftl::ILW>(root, "merge"); ftl::ILW *align = ftl::create<ftl::ILW>(root, "merge");
int o = root->value("origin_pose", 0) % sources.size();
virt->setPose(sources[o]->getPose());
// Generate virtual camera render when requested by streamer // Generate virtual camera render when requested by streamer
virt->onRender([splat,virt,&scene_B,align](ftl::rgbd::Frame &out) { virt->onRender([splat,virt,&scene_B,align](ftl::rgbd::Frame &out) {
virt->setTimestamp(scene_B.timestamp); virt->setTimestamp(scene_B.timestamp);
......
...@@ -17,6 +17,11 @@ void normal_visualise(ftl::cuda::TextureObject<float4> &norm, ...@@ -17,6 +17,11 @@ void normal_visualise(ftl::cuda::TextureObject<float4> &norm,
const ftl::rgbd::Camera &camera, const float4x4 &pose, const ftl::rgbd::Camera &camera, const float4x4 &pose,
cudaStream_t stream); cudaStream_t stream);
void normal_filter(ftl::cuda::TextureObject<float4> &norm,
ftl::cuda::TextureObject<float4> &points,
const ftl::rgbd::Camera &camera, const float4x4 &pose,
float thresh, cudaStream_t stream);
} }
} }
......
...@@ -16,7 +16,7 @@ struct ClipSpace { ...@@ -16,7 +16,7 @@ struct ClipSpace {
void point_cloud(ftl::cuda::TextureObject<float4> &output, void point_cloud(ftl::cuda::TextureObject<float4> &output,
ftl::cuda::TextureObject<float> &depth, ftl::cuda::TextureObject<float> &depth,
const ftl::rgbd::Camera &params, const ftl::rgbd::Camera &params,
const float4x4 &pose, cudaStream_t stream); const float4x4 &pose, uint discon, cudaStream_t stream);
void clipping(ftl::cuda::TextureObject<float4> &points, void clipping(ftl::cuda::TextureObject<float4> &points,
const ClipSpace &clip, cudaStream_t stream); const ClipSpace &clip, cudaStream_t stream);
......
...@@ -8,7 +8,7 @@ ...@@ -8,7 +8,7 @@
namespace ftl { namespace ftl {
namespace render { namespace render {
static const uint kShowBlockBorders = 0x00000001; // Deprecated: from voxels system static const uint kShowDisconMask = 0x00000001;
static const uint kNoSplatting = 0x00000002; static const uint kNoSplatting = 0x00000002;
static const uint kNoUpsampling = 0x00000004; static const uint kNoUpsampling = 0x00000004;
static const uint kNoTexturing = 0x00000008; static const uint kNoTexturing = 0x00000008;
......
...@@ -43,6 +43,7 @@ class Splatter : public ftl::render::Renderer { ...@@ -43,6 +43,7 @@ class Splatter : public ftl::render::Renderer {
ftl::rgbd::FrameSet *scene_; ftl::rgbd::FrameSet *scene_;
ftl::cuda::ClipSpace clip_; ftl::cuda::ClipSpace clip_;
bool clipping_; bool clipping_;
float norm_filter_;
}; };
} }
......
...@@ -20,14 +20,14 @@ __global__ void computeNormals_kernel(ftl::cuda::TextureObject<float4> output, ...@@ -20,14 +20,14 @@ __global__ void computeNormals_kernel(ftl::cuda::TextureObject<float4> output,
const float3 MC = make_float3(input.tex2D((int)x+0, (int)y-1)); //[(y-1)*width+(x+0)]; const float3 MC = make_float3(input.tex2D((int)x+0, (int)y-1)); //[(y-1)*width+(x+0)];
const float3 CM = make_float3(input.tex2D((int)x-1, (int)y+0)); //[(y+0)*width+(x-1)]; const float3 CM = make_float3(input.tex2D((int)x-1, (int)y+0)); //[(y+0)*width+(x-1)];
//if(CC.x != MINF && PC.x != MINF && CP.x != MINF && MC.x != MINF && CM.x != MINF) { if(CC.x != MINF) { // && PC.x != MINF && CP.x != MINF && MC.x != MINF && CM.x != MINF) {
const float3 n = cross(PC-MC, CP-CM); const float3 n = cross(PC-MC, CP-CM);
const float l = length(n); const float l = length(n);
if(l > 0.0f) { if(l > 0.0f) {
output(x,y) = make_float4(n/-l, 1.0f); output(x,y) = make_float4(n/-l, 1.0f);
} }
//} }
} }
} }
...@@ -51,6 +51,7 @@ __global__ void smooth_normals_kernel(ftl::cuda::TextureObject<float4> norms, ...@@ -51,6 +51,7 @@ __global__ void smooth_normals_kernel(ftl::cuda::TextureObject<float4> norms,
const float3 p = make_float3(points.tex2D((int)x+u,(int)y+v)); const float3 p = make_float3(points.tex2D((int)x+u,(int)y+v));
if (p.x == MINF) continue; if (p.x == MINF) continue;
const float s = ftl::cuda::spatialWeighting(p0, p, smoothing); const float s = ftl::cuda::spatialWeighting(p0, p, smoothing);
//const float s = 1.0f;
if (s > 0.0f) { if (s > 0.0f) {
const float4 n = norms.tex2D((int)x+u,(int)y+v); const float4 n = norms.tex2D((int)x+u,(int)y+v);
...@@ -76,7 +77,7 @@ void ftl::cuda::normals(ftl::cuda::TextureObject<float4> &output, ...@@ -76,7 +77,7 @@ void ftl::cuda::normals(ftl::cuda::TextureObject<float4> &output,
computeNormals_kernel<<<gridSize, blockSize, 0, stream>>>(temp, input); computeNormals_kernel<<<gridSize, blockSize, 0, stream>>>(temp, input);
cudaSafeCall( cudaGetLastError() ); cudaSafeCall( cudaGetLastError() );
smooth_normals_kernel<1><<<gridSize, blockSize, 0, stream>>>(temp, output, input, 0.04f); smooth_normals_kernel<3><<<gridSize, blockSize, 0, stream>>>(temp, output, input, 0.04f);
cudaSafeCall( cudaGetLastError() ); cudaSafeCall( cudaGetLastError() );
#ifdef _DEBUG #ifdef _DEBUG
...@@ -96,14 +97,19 @@ __global__ void vis_normals_kernel(ftl::cuda::TextureObject<float4> norm, ...@@ -96,14 +97,19 @@ __global__ void vis_normals_kernel(ftl::cuda::TextureObject<float4> norm,
if(x >= norm.width() || y >= norm.height()) return; if(x >= norm.width() || y >= norm.height()) return;
output(x,y) = 0.0f; output(x,y) = 0.0f;
float3 ray = make_float3(0.0f, 0.0f, 1.0f); //pose * camera.screenToCam(x,y,1.0f); float3 ray = pose.getFloat3x3() * camera.screenToCam(x,y,1.0f);
ray = ray / length(ray); ray = ray / length(ray);
float3 n = make_float3(norm.tex2D((int)x,(int)y)); float3 n = make_float3(norm.tex2D((int)x,(int)y));
float l = length(n); float l = length(n);
if (l == 0) return; if (l == 0) return;
n /= l; n /= l;
output(x,y) = (1.0f + dot(ray, n))*3.5f; // FIXME: Do not hard code these value scalings const float d = dot(ray, n);
output(x,y) = (1.0f + d)*3.5f; // FIXME: Do not hard code these value scalings
//if (d > 0.2f) {
// output(x,y) = d * 7.0f;
//}
} }
void ftl::cuda::normal_visualise(ftl::cuda::TextureObject<float4> &norm, void ftl::cuda::normal_visualise(ftl::cuda::TextureObject<float4> &norm,
...@@ -122,3 +128,47 @@ void ftl::cuda::normal_visualise(ftl::cuda::TextureObject<float4> &norm, ...@@ -122,3 +128,47 @@ void ftl::cuda::normal_visualise(ftl::cuda::TextureObject<float4> &norm,
//cutilCheckMsg(__FUNCTION__); //cutilCheckMsg(__FUNCTION__);
#endif #endif
} }
//==============================================================================
__global__ void filter_normals_kernel(ftl::cuda::TextureObject<float4> norm,
ftl::cuda::TextureObject<float4> output,
ftl::rgbd::Camera camera, float4x4 pose, float thresh) {
const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
if(x >= norm.width() || y >= norm.height()) return;
float3 ray = pose.getFloat3x3() * camera.screenToCam(x,y,1.0f);
ray = ray / length(ray);
float3 n = make_float3(norm.tex2D((int)x,(int)y));
float l = length(n);
if (l == 0) {
output(x,y) = make_float4(MINF);
return;
}
n /= l;
const float d = dot(ray, n);
if (d <= thresh) {
output(x,y) = make_float4(MINF);
}
}
void ftl::cuda::normal_filter(ftl::cuda::TextureObject<float4> &norm,
ftl::cuda::TextureObject<float4> &output,
const ftl::rgbd::Camera &camera, const float4x4 &pose,
float thresh,
cudaStream_t stream) {
const dim3 gridSize((norm.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (norm.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
filter_normals_kernel<<<gridSize, blockSize, 0, stream>>>(norm, output, camera, pose, thresh);
cudaSafeCall( cudaGetLastError() );
#ifdef _DEBUG
cudaSafeCall(cudaDeviceSynchronize());
//cutilCheckMsg(__FUNCTION__);
#endif
}
...@@ -2,25 +2,61 @@ ...@@ -2,25 +2,61 @@
#define T_PER_BLOCK 8 #define T_PER_BLOCK 8
template <int RADIUS>
__global__ void point_cloud_kernel(ftl::cuda::TextureObject<float4> output, ftl::cuda::TextureObject<float> depth, ftl::rgbd::Camera params, float4x4 pose) __global__ void point_cloud_kernel(ftl::cuda::TextureObject<float4> output, ftl::cuda::TextureObject<float> depth, ftl::rgbd::Camera params, float4x4 pose)
{ {
const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x; const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y; const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
if (x < params.width && y < params.height) { if (x < params.width && y < params.height) {
output(x,y) = make_float4(MINF, MINF, MINF, MINF);
const float d = depth.tex2D((int)x, (int)y);
float p = d;
if (d >= params.minDepth && d <= params.maxDepth) {
/* Orts-Escolano S. et al. 2016. Holoportation: Virtual 3D teleportation in real-time. */
// Is there a discontinuity nearby?
for (int u=-RADIUS; u<=RADIUS; ++u) {
for (int v=-RADIUS; v<=RADIUS; ++v) {
// If yes, the flag using w = -1
if (fabs(depth.tex2D((int)x+u, (int)y+v) - d) > 0.1f) p = -1.0f;
}
}
output(x,y) = make_float4(pose * params.screenToCam(x, y, d), p);
}
}
}
template <>
__global__ void point_cloud_kernel<0>(ftl::cuda::TextureObject<float4> output, ftl::cuda::TextureObject<float> depth, ftl::rgbd::Camera params, float4x4 pose)
{
const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
if (x < params.width && y < params.height) {
output(x,y) = make_float4(MINF, MINF, MINF, MINF);
float d = depth.tex2D((int)x, (int)y); float d = depth.tex2D((int)x, (int)y);
output(x,y) = (d >= params.minDepth && d <= params.maxDepth) ? if (d >= params.minDepth && d <= params.maxDepth) {
make_float4(pose * params.screenToCam(x, y, d), 0.0f) : output(x,y) = make_float4(pose * params.screenToCam(x, y, d), d);
make_float4(MINF, MINF, MINF, MINF); }
} }
} }
void ftl::cuda::point_cloud(ftl::cuda::TextureObject<float4> &output, ftl::cuda::TextureObject<float> &depth, const ftl::rgbd::Camera &params, const float4x4 &pose, cudaStream_t stream) { void ftl::cuda::point_cloud(ftl::cuda::TextureObject<float4> &output, ftl::cuda::TextureObject<float> &depth, const ftl::rgbd::Camera &params, const float4x4 &pose, uint discon, cudaStream_t stream) {
const dim3 gridSize((params.width + T_PER_BLOCK - 1)/T_PER_BLOCK, (params.height + T_PER_BLOCK - 1)/T_PER_BLOCK); const dim3 gridSize((params.width + T_PER_BLOCK - 1)/T_PER_BLOCK, (params.height + T_PER_BLOCK - 1)/T_PER_BLOCK);
const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK); const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
point_cloud_kernel<<<gridSize, blockSize, 0, stream>>>(output, depth, params, pose); switch (discon) {
case 4 : point_cloud_kernel<4><<<gridSize, blockSize, 0, stream>>>(output, depth, params, pose); break;
case 3 : point_cloud_kernel<3><<<gridSize, blockSize, 0, stream>>>(output, depth, params, pose); break;
case 2 : point_cloud_kernel<2><<<gridSize, blockSize, 0, stream>>>(output, depth, params, pose); break;
case 1 : point_cloud_kernel<1><<<gridSize, blockSize, 0, stream>>>(output, depth, params, pose); break;
default: point_cloud_kernel<0><<<gridSize, blockSize, 0, stream>>>(output, depth, params, pose);
}
cudaSafeCall( cudaGetLastError() ); cudaSafeCall( cudaGetLastError() );
#ifdef _DEBUG #ifdef _DEBUG
......
...@@ -49,6 +49,11 @@ Splatter::Splatter(nlohmann::json &config, ftl::rgbd::FrameSet *fs) : ftl::rende ...@@ -49,6 +49,11 @@ Splatter::Splatter(nlohmann::json &config, ftl::rgbd::FrameSet *fs) : ftl::rende
on("clipping_enabled", [this](const ftl::config::Event &e) { on("clipping_enabled", [this](const ftl::config::Event &e) {
clipping_ = value("clipping_enabled", true); clipping_ = value("clipping_enabled", true);
}); });
norm_filter_ = value("normal_filter", -1.0f);
on("normal_filter", [this](const ftl::config::Event &e) {
norm_filter_ = value("normal_filter", -1.0f);
});
} }
Splatter::~Splatter() { Splatter::~Splatter() {
...@@ -80,22 +85,6 @@ void Splatter::renderChannel( ...@@ -80,22 +85,6 @@ void Splatter::renderChannel(
continue; continue;
} }
// Needs to create points channel first?
if (!f.hasChannel(Channel::Points)) {
//LOG(INFO) << "Creating points... " << s->parameters().width;
auto &t = f.createTexture<float4>(Channel::Points, Format<float4>(f.get<GpuMat>(Channel::Colour).size()));
auto pose = MatrixConversion::toCUDA(s->getPose().cast<float>()); //.inverse());
ftl::cuda::point_cloud(t, f.createTexture<float>(Channel::Depth), s->parameters(), pose, stream);
//LOG(INFO) << "POINTS Added";
}
// Clip first?
if (clipping_) {
ftl::cuda::clipping(f.createTexture<float4>(Channel::Points), clip_, stream);
}
ftl::cuda::dibr_merge( ftl::cuda::dibr_merge(
f.createTexture<float4>(Channel::Points), f.createTexture<float4>(Channel::Points),
temp_.getTexture<int>(Channel::Depth), temp_.getTexture<int>(Channel::Depth),
...@@ -235,9 +224,10 @@ bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, cuda ...@@ -235,9 +224,10 @@ bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, cuda
// Parameters object to pass to CUDA describing the camera // Parameters object to pass to CUDA describing the camera
SplatParams params; SplatParams params;
params.m_flags = 0; params.m_flags = 0;
if (src->value("splatting", true) == false) params.m_flags |= ftl::render::kNoSplatting; if (src->value("show_discontinuity_mask", false)) params.m_flags |= ftl::render::kShowDisconMask;
if (src->value("upsampling", true) == false) params.m_flags |= ftl::render::kNoUpsampling; //if (src->value("splatting", true) == false) params.m_flags |= ftl::render::kNoSplatting;
if (src->value("texturing", true) == false) params.m_flags |= ftl::render::kNoTexturing; //if (src->value("upsampling", true) == false) params.m_flags |= ftl::render::kNoUpsampling;
//if (src->value("texturing", true) == false) params.m_flags |= ftl::render::kNoTexturing;
params.m_viewMatrix = MatrixConversion::toCUDA(src->getPose().cast<float>().inverse()); params.m_viewMatrix = MatrixConversion::toCUDA(src->getPose().cast<float>().inverse());
params.m_viewMatrixInverse = MatrixConversion::toCUDA(src->getPose().cast<float>()); params.m_viewMatrixInverse = MatrixConversion::toCUDA(src->getPose().cast<float>());
params.camera = camera; params.camera = camera;
...@@ -251,6 +241,42 @@ bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, cuda ...@@ -251,6 +241,42 @@ bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, cuda
temp_.createTexture<int>(Channel::Depth); temp_.createTexture<int>(Channel::Depth);
// First make sure each input has normals
temp_.createTexture<float4>(Channel::Normals);
for (int i=0; i<scene_->frames.size(); ++i) {
auto &f = scene_->frames[i];
auto s = scene_->sources[i];
// Needs to create points channel first?
if (!f.hasChannel(Channel::Points)) {
//LOG(INFO) << "Creating points... " << s->parameters().width;
auto &t = f.createTexture<float4>(Channel::Points, Format<float4>(f.get<GpuMat>(Channel::Colour).size()));
auto pose = MatrixConversion::toCUDA(s->getPose().cast<float>()); //.inverse());
ftl::cuda::point_cloud(t, f.createTexture<float>(Channel::Depth), s->parameters(), pose, 0, stream);
//LOG(INFO) << "POINTS Added";
}
// Clip first?
if (clipping_) {
ftl::cuda::clipping(f.createTexture<float4>(Channel::Points), clip_, stream);
}
if (!f.hasChannel(Channel::Normals)) {
auto &g = f.get<GpuMat>(Channel::Colour);
ftl::cuda::normals(f.createTexture<float4>(Channel::Normals, Format<float4>(g.cols, g.rows)),
temp_.getTexture<float4>(Channel::Normals), // FIXME: Uses assumption of vcam res same as input res
f.getTexture<float4>(Channel::Points), stream);
if (norm_filter_ > -0.1f) {
Eigen::Matrix4f matrix = s->getPose().cast<float>();
auto pose = MatrixConversion::toCUDA(matrix);
ftl::cuda::normal_filter(f.getTexture<float4>(Channel::Normals), f.getTexture<float4>(Channel::Points), s->parameters(), pose, norm_filter_, stream);
}
}
}
renderChannel(params, out, Channel::Colour, stream); renderChannel(params, out, Channel::Colour, stream);
Channel chan = src->getChannel(); Channel chan = src->getChannel();
...@@ -258,31 +284,13 @@ bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, cuda ...@@ -258,31 +284,13 @@ bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, cuda
{ {
temp_.get<GpuMat>(Channel::Depth).convertTo(out.get<GpuMat>(Channel::Depth), CV_32F, 1.0f / 1000.0f, cvstream); temp_.get<GpuMat>(Channel::Depth).convertTo(out.get<GpuMat>(Channel::Depth), CV_32F, 1.0f / 1000.0f, cvstream);
} else if (chan == Channel::Normals) { } else if (chan == Channel::Normals) {
//temp_.get<GpuMat>(Channel::Depth).convertTo(out.get<GpuMat>(Channel::Depth), CV_32F, 1.0f / 1000.0f, cvstream);
//ftl::cuda::point_cloud(temp_.createTexture<float4>(Channel::Points, Format<float4>(camera.width, camera.height)),
// temp_.createTexture<float>(Channel::Depth), camera, params.m_viewMatrixInverse, stream);
//ftl::cuda::normals(temp_.getTexture<float4>(Channel::Normals), temp_.getTexture<float4>(Channel::Points), stream);
//ftl::cuda::normal_visualise(temp_.getTexture<float4>(Channel::Normals), temp_.getTexture<float>(Channel::Contribution), camera);
// First make sure each input has normals
temp_.createTexture<float4>(Channel::Normals);
for (auto &f : scene_->frames) {
if (!f.hasChannel(Channel::Normals)) {
auto &g = f.get<GpuMat>(Channel::Colour);
LOG(INFO) << "Make normals channel";
ftl::cuda::normals(f.createTexture<float4>(Channel::Normals, Format<float4>(g.cols, g.rows)),
temp_.getTexture<float4>(Channel::Normals), // FIXME: Uses assumption of vcam res same as input res
f.getTexture<float4>(Channel::Points), stream);
}
}
out.create<GpuMat>(Channel::Normals, Format<float4>(camera.width, camera.height)); out.create<GpuMat>(Channel::Normals, Format<float4>(camera.width, camera.height));
// Render normal attribute // Render normal attribute
renderChannel(params, out, Channel::Normals, stream); renderChannel(params, out, Channel::Normals, stream);
// Convert normal to single float value // Convert normal to single float value
ftl::cuda::normal_visualise(out.getTexture<float4>(Channel::Normals), temp_.getTexture<float>(Channel::Contribution), camera, params.m_viewMatrix, stream); ftl::cuda::normal_visualise(out.getTexture<float4>(Channel::Normals), temp_.getTexture<float>(Channel::Contribution), camera, params.m_viewMatrixInverse, stream);
// Put in output as single float // Put in output as single float
cv::cuda::swap(temp_.get<GpuMat>(Channel::Contribution), out.create<GpuMat>(Channel::Normals)); cv::cuda::swap(temp_.get<GpuMat>(Channel::Contribution), out.create<GpuMat>(Channel::Normals));
......
...@@ -24,11 +24,11 @@ using ftl::render::SplatParams; ...@@ -24,11 +24,11 @@ using ftl::render::SplatParams;
const int x = blockIdx.x*blockDim.x + threadIdx.x; const int x = blockIdx.x*blockDim.x + threadIdx.x;
const int y = blockIdx.y*blockDim.y + threadIdx.y; const int y = blockIdx.y*blockDim.y + threadIdx.y;
const float3 worldPos = make_float3(points.tex2D(x, y)); const float4 worldPos = points.tex2D(x, y);
if (worldPos.x == MINF) return; if (worldPos.x == MINF || (!(params.m_flags & ftl::render::kShowDisconMask) && worldPos.w < 0.0f)) return;
// Find the virtual screen position of current point // Find the virtual screen position of current point
const float3 camPos = params.m_viewMatrix * worldPos; const float3 camPos = params.m_viewMatrix * make_float3(worldPos);
if (camPos.z < params.camera.minDepth) return; if (camPos.z < params.camera.minDepth) return;
if (camPos.z > params.camera.maxDepth) return; if (camPos.z > params.camera.maxDepth) return;
...@@ -61,7 +61,7 @@ __device__ inline float4 make_float4(const uchar4 &c) { ...@@ -61,7 +61,7 @@ __device__ inline float4 make_float4(const uchar4 &c) {
#define ENERGY_THRESHOLD 0.1f #define ENERGY_THRESHOLD 0.1f
#define SMOOTHING_MULTIPLIER_A 10.0f // For surface search #define SMOOTHING_MULTIPLIER_A 10.0f // For surface search
#define SMOOTHING_MULTIPLIER_B 4.0f // For z contribution #define SMOOTHING_MULTIPLIER_B 4.0f // For z contribution
#define SMOOTHING_MULTIPLIER_C 4.0f // For colour contribution #define SMOOTHING_MULTIPLIER_C 1.0f // For colour contribution
#define ACCUM_DIAMETER 8 #define ACCUM_DIAMETER 8
...@@ -84,12 +84,12 @@ __global__ void dibr_attribute_contrib_kernel( ...@@ -84,12 +84,12 @@ __global__ void dibr_attribute_contrib_kernel(
const int x = (blockIdx.x*blockDim.x + threadIdx.x) / WARP_SIZE; const int x = (blockIdx.x*blockDim.x + threadIdx.x) / WARP_SIZE;
const int y = blockIdx.y*blockDim.y + threadIdx.y; const int y = blockIdx.y*blockDim.y + threadIdx.y;
const float3 worldPos = make_float3(points.tex2D(x, y)); const float4 worldPos = points.tex2D(x, y);
//const float3 normal = make_float3(tex2D<float4>(camera.normal, x, y)); //const float3 normal = make_float3(tex2D<float4>(camera.normal, x, y));
if (worldPos.x == MINF) return; if (worldPos.x == MINF) return;
//const float r = (camera.poseInverse * worldPos).z / camera.params.fx; //const float r = (camera.poseInverse * worldPos).z / camera.params.fx;
const float3 camPos = params.m_viewMatrix * worldPos; const float3 camPos = params.m_viewMatrix * make_float3(worldPos);
if (camPos.z < params.camera.minDepth) return; if (camPos.z < params.camera.minDepth) return;
if (camPos.z > params.camera.maxDepth) return; if (camPos.z > params.camera.maxDepth) return;
const uint2 screenPos = params.camera.camToScreen<uint2>(camPos); const uint2 screenPos = params.camera.camToScreen<uint2>(camPos);
...@@ -103,8 +103,9 @@ __global__ void dibr_attribute_contrib_kernel( ...@@ -103,8 +103,9 @@ __global__ void dibr_attribute_contrib_kernel(
const float d = ((float)depth_in.tex2D((int)screenPos.x, (int)screenPos.y)/1000.0f); const float d = ((float)depth_in.tex2D((int)screenPos.x, (int)screenPos.y)/1000.0f);
//if (abs(d - camPos.z) > DEPTH_THRESHOLD) return; //if (abs(d - camPos.z) > DEPTH_THRESHOLD) return;
// TODO:(Nick) Should just one thread load these to shared mem? const float4 colour = (params.m_flags & ftl::render::kShowDisconMask && worldPos.w < 0.0f) ?
const float4 colour = make_float4(colour_in.tex2D(x, y)); make_float4(0.0f,0.0f,255.0f,255.0f) : // Show discontinuity mask in red
make_float4(colour_in.tex2D(x, y));
//const float4 normal = tex2D<float4>(camera.normal, x, y); //const float4 normal = tex2D<float4>(camera.normal, x, y);
// Each thread in warp takes an upsample point and updates corresponding depth buffer. // Each thread in warp takes an upsample point and updates corresponding depth buffer.
......
...@@ -8,7 +8,8 @@ class VirtualImpl : public ftl::rgbd::detail::Source { ...@@ -8,7 +8,8 @@ class VirtualImpl : public ftl::rgbd::detail::Source {
public: public:
explicit VirtualImpl(ftl::rgbd::Source *host, const ftl::rgbd::Camera &params) : ftl::rgbd::detail::Source(host) { explicit VirtualImpl(ftl::rgbd::Source *host, const ftl::rgbd::Camera &params) : ftl::rgbd::detail::Source(host) {
params_ = params; params_ = params;
capabilities_ = ftl::rgbd::kCapMovable | ftl::rgbd::kCapVideo | ftl::rgbd::kCapStereo; capabilities_ = ftl::rgbd::kCapVideo | ftl::rgbd::kCapStereo;
if (!host->value("locked", false)) capabilities_ |= ftl::rgbd::kCapMovable;
} }
~VirtualImpl() { ~VirtualImpl() {
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment