diff --git a/components/rgbd-sources/src/offilter.cpp b/components/rgbd-sources/src/offilter.cpp index c49e24c7a1f828a78d64bdec581031f985fcc453..bf1a5414dd5973452332e558ae375011cacbc56d 100644 --- a/components/rgbd-sources/src/offilter.cpp +++ b/components/rgbd-sources/src/offilter.cpp @@ -18,7 +18,6 @@ template<typename T> static bool inline isValidDisparity(T d) { return (0.0 < d) OFDisparityFilter::OFDisparityFilter(Size size, int n_frames, float threshold) : n_(0), n_max_(n_frames), threshold_(threshold), size_(size) { - disp_ = Mat::zeros(cv::Size(size.width * n_frames, size.height), CV_64FC1); disp_old_ = cv::cuda::GpuMat(cv::Size(size.width * n_frames, size.height), CV_32FC1); gray_ = Mat::zeros(size, CV_8UC1); @@ -37,7 +36,6 @@ void OFDisparityFilter::filter(cv::cuda::GpuMat &disp, const cv::cuda::GpuMat &o void OFDisparityFilter::filter(Mat &disp, const Mat &gray) { - const int n = n_; n_ = (n_ + 1) % n_max_; nvof_->calc(gray, gray_, flowxy_); @@ -54,75 +52,6 @@ void OFDisparityFilter::filter(Mat &disp, const Mat &gray) GpuMat disp_gpu(disp); filter(disp_gpu, GpuMat(flowxy_up_)); disp_gpu.download(disp); - return; - - vector<float> values(n_max_); - - for (int y = 0; y < size_.height; y++) - { - float *D = disp_.ptr<float>(y); - float *d = disp.ptr<float>(y); - float *flow = flowxy_up_.ptr<float>(y); - - for (int x = 0; x < size_.width; x++) - { - const float flow_l1 = abs(flow[2*x]) + abs(flow[2*x + 1]); - - if (flow_l1 < threshold_) - { - values.clear(); - - if (isValidDisparity(d[x])) - { - bool updated = false; - for (int i = 0; i < n_max_; i++) - { - float &val = D[n_max_ * x + (n_max_ - i + n) % n_max_]; - if (!isValidDisparity(val)) - { - val = d[x]; - updated = true; - } - } - if (!updated) { D[n_max_ * x + n] = d[x]; } - } - - for (int i = 0; i < n_max_; i++) - { - float &val = D[n_max_ * x + i]; - if (isValidDisparity(val)) { values.push_back(val); } - } - - if (values.size() > 0) { - const auto median_it = values.begin() + values.size() / 2; - std::nth_element(values.begin(), median_it , values.end()); - d[x] = *median_it; - } - - /* - if (isValidDepth(d[x]) && isValidDepth(D[x])) - { - D[x] = D[x] * 0.66 + d[x] * (1.0 - 0.66); - } - if (isValidDepth(D[x])) - { - d[x] = D[x]; - } - else - { - D[x] = d[x]; - } - */ - } - else - { - for (int i = 0; i < n_max_; i++) - { - D[n_max_ * x + i] = 0.0; - } - } - } - } } #endif // HAVE_OPTFLOW