diff --git a/applications/reconstruct/src/ilw/ilw.cpp b/applications/reconstruct/src/ilw/ilw.cpp
index 5aa62b530835adb8a2e50c84f5310d114c58041b..a4a31e27d44d3338c0dda88cc1512e9e6c3845cf 100644
--- a/applications/reconstruct/src/ilw/ilw.cpp
+++ b/applications/reconstruct/src/ilw/ilw.cpp
@@ -22,6 +22,7 @@ ILW::ILW(nlohmann::json &config) : ftl::Configurable(config) {
params_.colour_smooth = value("colour_smooth", 50.0f);
params_.spatial_smooth = value("spatial_smooth", 0.04f);
params_.cost_ratio = value("cost_ratio", 0.2f);
+ params_.cost_threshold = value("cost_threshold", 1.0f);
discon_mask_ = value("discontinuity_mask",2);
fill_depth_ = value("fill_depth", false);
@@ -65,6 +66,10 @@ ILW::ILW(nlohmann::json &config) : ftl::Configurable(config) {
params_.cost_ratio = value("cost_ratio", 0.2f);
});
+ on("cost_threshold", [this](const ftl::config::Event &e) {
+ params_.cost_threshold = value("cost_threshold", 1.0f);
+ });
+
params_.flags = 0;
if (value("ignore_bad", false)) params_.flags |= ftl::cuda::kILWFlag_IgnoreBad;
if (value("ignore_bad_colour", false)) params_.flags |= ftl::cuda::kILWFlag_SkipBadColour;
@@ -104,13 +109,7 @@ bool ILW::process(ftl::rgbd::FrameSet &fs, cudaStream_t stream) {
params_.range = 0.1f;
for (int i=0; i<iterations_; ++i) {
- int win;
- switch (i) {
- case 0: win = 17; break;
- case 1: win = 9; break;
- default: win = 5; break;
- }
- _phase1(fs, win, stream);
+ _phase1(fs, value("cost_function",0), stream);
//for (int j=0; j<3; ++j) {
_phase2(fs, motion_rate_, stream);
//}
diff --git a/applications/reconstruct/src/ilw/ilw.cu b/applications/reconstruct/src/ilw/ilw.cu
index af5916cf0e0234786839098e01d6508d5402e422..8e6bd9009ada579a68a339545d6ee340c4043149 100644
--- a/applications/reconstruct/src/ilw/ilw.cu
+++ b/applications/reconstruct/src/ilw/ilw.cu
@@ -85,12 +85,25 @@ void ftl::cuda::preprocess_depth(
//==============================================================================
-//#define COR_WIN_RADIUS 17
-//#define COR_WIN_SIZE (COR_WIN_RADIUS * COR_WIN_RADIUS)
+template<int FUNCTION>
+__device__ float costFunction(const ftl::cuda::ILWParams ¶ms, float dweight, float cweight);
-#define WINDOW_RADIUS 1
+template <>
+__device__ float costFunction<0>(const ftl::cuda::ILWParams ¶ms, float dweight, float cweight) {
+ return 1.0f - (params.cost_ratio * (cweight) + (1.0f - params.cost_ratio) * dweight);
+}
+
+template <>
+__device__ float costFunction<1>(const ftl::cuda::ILWParams ¶m, float dweight, float cweight) {
+ return 1.0f - (cweight * cweight * dweight);
+}
-template<int COR_STEPS>
+template <>
+__device__ float costFunction<2>(const ftl::cuda::ILWParams ¶m, float dweight, float cweight) {
+ return 1.0f - (dweight * dweight * cweight);
+}
+
+template<int COR_STEPS, int FUNCTION>
__global__ void correspondence_energy_vector_kernel(
TextureObject<float> d1,
TextureObject<float> d2,
@@ -157,27 +170,28 @@ __global__ void correspondence_energy_vector_kernel(
//if ((params.flags & ftl::cuda::kILWFlag_IgnoreBad) && world2.x == MINF) continue;
+ // Generate a depth correspondence value
const float depth2 = d2.tex2D((int)screen.x, (int)screen.y);
-
- // Determine degree of correspondence
- float cost = ftl::cuda::weighting(fabs(depth2 - camPos.z), params.spatial_smooth);
- // Point is too far away to even count
- //if (cost == 1.0f) continue;
+ const float dweight = ftl::cuda::weighting(fabs(depth2 - camPos.z), params.spatial_smooth);
+ // Generate a colour correspondence value
const uchar4 colour2 = c2.tex2D((int)screen.x, (int)screen.y);
+ const float cweight = ftl::cuda::colourWeighting(colour1, colour2, params.colour_smooth);
+
- // Mix ratio of colour and distance costs
- const float ccost = ftl::cuda::colourWeighting(colour1, colour2, params.colour_smooth);
- //if ((params.flags & ftl::cuda::kILWFlag_SkipBadColour) && ccost == 1.0f) continue;
-
// Cost eq 1: summed contributions
- cost = 1.0f - (params.cost_ratio * (ccost) + (1.0f - params.cost_ratio) * cost);
+ //cost = 1.0f - (params.cost_ratio * (ccost) + (1.0f - params.cost_ratio) * cost);
// Cost eq 2: Multiplied
//cost = 1.0f - (ccost * ccost * cost);
+ const float cost = costFunction<FUNCTION>(params, dweight, cweight);
+
+ // Cost is so bad, don't even consider this a valid option
+ if (cost >= params.cost_threshold) continue;
+
++count;
- avgcost += (params.flags & ftl::cuda::kILWFlag_ColourConfidenceOnly) ? ccost : cost;
+ avgcost += cost;
if (cost < bestcost) {
bestdepth = depth_adjust;
bestcost = cost;
@@ -227,7 +241,7 @@ void ftl::cuda::correspondence(
float4x4 &pose1_inv,
float4x4 &pose2,
const Camera &cam1,
- const Camera &cam2, const ILWParams ¶ms, int win,
+ const Camera &cam2, const ILWParams ¶ms, int func,
cudaStream_t stream) {
const dim3 gridSize((d1.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (d1.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
@@ -235,13 +249,12 @@ void ftl::cuda::correspondence(
//printf("COR SIZE %d,%d\n", p1.width(), p1.height());
- correspondence_energy_vector_kernel<16><<<gridSize, blockSize, 0, stream>>>(d1, d2, c1, c2, dout, conf, pose1, pose1_inv, pose2, cam1, cam2, params);
+ switch (func) {
+ case 0: correspondence_energy_vector_kernel<16,0><<<gridSize, blockSize, 0, stream>>>(d1, d2, c1, c2, dout, conf, pose1, pose1_inv, pose2, cam1, cam2, params);
+ case 1: correspondence_energy_vector_kernel<16,1><<<gridSize, blockSize, 0, stream>>>(d1, d2, c1, c2, dout, conf, pose1, pose1_inv, pose2, cam1, cam2, params);
+ case 2: correspondence_energy_vector_kernel<16,2><<<gridSize, blockSize, 0, stream>>>(d1, d2, c1, c2, dout, conf, pose1, pose1_inv, pose2, cam1, cam2, params);
+ }
- //switch (win) {
- //case 17 : correspondence_energy_vector_kernel<17><<<gridSize, blockSize, 0, stream>>>(p1, p2, c1, c2, vout, eout, pose1, pose1_inv, pose2, cam1, cam2, params); break;
- //case 9 : correspondence_energy_vector_kernel<9><<<gridSize, blockSize, 0, stream>>>(p1, p2, c1, c2, vout, eout, pose1, pose1_inv, pose2, cam1, cam2, params); break;
- //case 5 : correspondence_energy_vector_kernel<5><<<gridSize, blockSize, 0, stream>>>(p1, p2, c1, c2, vout, eout, pose1, pose1_inv, pose2, cam1, cam2, params); break;
- //}
cudaSafeCall( cudaGetLastError() );
}
diff --git a/applications/reconstruct/src/ilw/ilw_cuda.hpp b/applications/reconstruct/src/ilw/ilw_cuda.hpp
index 5a336a0de8b55b4488f641cde8df7af01b7460d5..aac0493a86af1cfd69925aa054ee3d34c901ec5f 100644
--- a/applications/reconstruct/src/ilw/ilw_cuda.hpp
+++ b/applications/reconstruct/src/ilw/ilw_cuda.hpp
@@ -12,7 +12,7 @@ struct ILWParams {
float spatial_smooth;
float colour_smooth;
float cost_ratio;
- float threshold;
+ float cost_threshold;
float range;
uint flags;
};