diff --git a/components/operators/src/fusion/fusion.cpp b/components/operators/src/fusion/fusion.cpp index 5b5a9c8bb7f10da6dd92c8e281a409230e70e13d..5e0a624b4029eb7ad8d1e00bbda9dc9a60ca5e9f 100644 --- a/components/operators/src/fusion/fusion.cpp +++ b/components/operators/src/fusion/fusion.cpp @@ -21,8 +21,8 @@ Fusion::~Fusion() { } bool Fusion::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) { - float mls_spatial = config()->value("mls_spatial", 0.01f); - float mls_feature = config()->value("mls_feature", 20.0f); + float mls_smoothing = config()->value("mls_smoothing", 2.0f); + //float mls_feature = config()->value("mls_feature", 20.0f); int mls_iters = config()->value("mls_iterations", 2); if (weights_.size() != in.frames.size()) weights_.resize(in.frames.size()); @@ -128,8 +128,8 @@ bool Fusion::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream weights_[j], f2.getLeft(), pose2, - mls_spatial, - mls_feature, + mls_smoothing, + mls_smoothing, stream ); } diff --git a/components/operators/src/fusion/smoothing/mls_multi_weighted.cu b/components/operators/src/fusion/smoothing/mls_multi_weighted.cu index dd889f53289cb6f5fe3a792b6e690868f730b75d..4d62d3bdaf2d8a07f6548aa74e194022e5e65561 100644 --- a/components/operators/src/fusion/smoothing/mls_multi_weighted.cu +++ b/components/operators/src/fusion/smoothing/mls_multi_weighted.cu @@ -70,12 +70,25 @@ __device__ inline float featureWeight(int f1, int f2) { // points as an indication of miss-alignment. Both spatial distance and // feature distance could be used to adjust parameters. - /*if (s.x >= 0 && s.x < camera_in.width && s.y >= 0 && s.y <= camera_in.height) { + // FIXME: For own image, need to do something different than the below. + // Otherwise smoothing factors become 0. + + float spatial_smoothing = (depth_origin == depth_in) ? 0.005f : 0.03f; // 3cm default + float hf_intensity_smoothing = (depth_origin == depth_in) ? 100.0f : 50.0f; + float mean_smoothing = (depth_origin == depth_in) ? 100.0f : 100.0f; + if (depth_origin != depth_in && s.x >= 0 && s.x < camera_in.width && s.y >= 0 && s.y <= camera_in.height) { // Get depth at exact reprojection point const float d = depth_in[s.x+s.y*dpitch_i]; // Get feature at exact reprojection point - const uchar2 feature2 = feature_in[s.x+y+(s.y+v)*fpitch_i]; - }*/ + const uchar2 feature2 = feature_in[s.x+s.y*fpitch_i]; + if (d > camera_in.minDepth && d < camera_in.maxDepth) { + spatial_smoothing = min(spatial_smoothing, smoothing * fabsf(camPos.z - d)); + } + hf_intensity_smoothing = smoothing * fabsf(float(feature2.x) - float(feature1.x)); + mean_smoothing = smoothing * fabsf(float(feature2.y) - float(feature1.y)); + } + + if (spatial_smoothing < 0.001f || hf_intensity_smoothing <= 1.0f || mean_smoothing <= 1.0f) return; // Neighbourhood @@ -94,9 +107,9 @@ __device__ inline float featureWeight(int f1, int f2) { // Rule: spatially close and feature close is strong // Spatially far or feature far, then poor. // So take the minimum, must be close and feature close to get good value - const float w_high_int = ftl::cuda::weighting(float(abs(int(feature1.x)-int(feature2.x))), fsmoothing); - const float w_mean_int = ftl::cuda::weighting(float(abs(int(feature1.y)-int(feature2.y))), 100.0f); - const float w_space = ftl::cuda::spatialWeighting(X,Xi,smoothing); + const float w_high_int = ftl::cuda::weighting(float(abs(int(feature1.x)-int(feature2.x))), hf_intensity_smoothing); + const float w_mean_int = ftl::cuda::weighting(float(abs(int(feature1.y)-int(feature2.y))), mean_smoothing); + const float w_space = ftl::cuda::spatialWeighting(X,Xi,spatial_smoothing); // TODO: Distance from cam squared // TODO: Angle from cam (dot of normal and ray) const float w = (length(Ni) > 0.0f)