diff --git a/lib/libstereo/include/stereo.hpp b/lib/libstereo/include/stereo.hpp index 4b4091ffc73d94a9a86bb1f7e7df51ab6c69c5f6..b34706daad684eb27c8c2b95894a22a0fa739dc3 100644 --- a/lib/libstereo/include/stereo.hpp +++ b/lib/libstereo/include/stereo.hpp @@ -135,6 +135,11 @@ public: int paths = AggregationDirections::HORIZONTAL | AggregationDirections::VERTICAL | AggregationDirections::DIAGONAL; + /** normalization of variance to range [alpha, beta] */ + float alpha = 0.2; + float beta = 1.0; + /** variance window size */ + int var_window = 9; bool debug = false; }; Parameters params; diff --git a/lib/libstereo/src/algorithms/hcensussgm.cu b/lib/libstereo/src/algorithms/hcensussgm.cu index f16be84e7859bd0c207d5f8a1896bd672b65d88a..27b61fd3d58c7952948fae1ae58b206ca4316661 100644 --- a/lib/libstereo/src/algorithms/hcensussgm.cu +++ b/lib/libstereo/src/algorithms/hcensussgm.cu @@ -3,25 +3,63 @@ #include "../costs/census.hpp" #include "../costs/dual.hpp" #include <opencv2/cudawarping.hpp> +#include <opencv2/cudafilters.hpp> -typedef MultiCosts<CensusMatchingCost,3> MatchingCost; +typedef MultiCostsWeighted<CensusMatchingCost,3> MatchingCost; + +static void variance_mask(cv::InputArray in, cv::OutputArray out, int wsize=3) { + if (in.isGpuMat() && out.isGpuMat()) { + cv::cuda::GpuMat im; + cv::cuda::GpuMat im2; + cv::cuda::GpuMat mean; + cv::cuda::GpuMat mean2; + + mean.create(in.size(), CV_32FC1); + mean2.create(in.size(), CV_32FC1); + im2.create(in.size(), CV_32FC1); + + if (in.type() != CV_32FC1) { + in.getGpuMat().convertTo(im, CV_32FC1); + } + else { + im = in.getGpuMat(); + } + + cv::cuda::multiply(im, im, im2); + auto filter = cv::cuda::createBoxFilter(CV_32FC1, CV_32FC1, cv::Size(wsize,wsize)); + filter->apply(im, mean); // E[X] + filter->apply(im2, mean2); // E[X^2] + cv::cuda::multiply(mean, mean, mean); // (E[X])^2 + + // NOTE: floating point accuracy in subtraction + // (cv::cuda::createBoxFilter only supports 8 bit integer types) + cv::cuda::subtract(mean2, mean, out.getGpuMatRef()); // E[X^2] - (E[X])^2 + } + else { throw std::exception(); /* todo CPU version */ } +} struct StereoHierCensusSgm::Impl : public StereoSgm<MatchingCost, StereoHierCensusSgm::Parameters> { CensusMatchingCost cost_fine; CensusMatchingCost cost_medium; CensusMatchingCost cost_coarse; Array2D<uchar> l; - Array2D<uchar> r; + Array2D<uchar> r; + Array2D<float> var_fine; + Array2D<float> var_medium; + Array2D<float> var_coarse; Impl(StereoHierCensusSgm::Parameters ¶ms, int width, int height, int dmin, int dmax) : StereoSgm(params, width, height, dmin, dmax), cost_fine(width, height, dmin, dmax), cost_medium(width, height, dmin, dmax), cost_coarse(width, height, dmin, dmax), - l(width, height), r(width, height) { - cost.add(0, cost_fine); - cost.add(1, cost_medium); - cost.add(2, cost_coarse); + l(width, height), r(width, height), + var_fine(width, height), + var_medium(width, height), + var_coarse(width, height) { + cost.add(0, cost_fine, var_fine); + cost.add(1, cost_medium, var_medium); + cost.add(2, cost_coarse, var_coarse); } }; @@ -42,8 +80,8 @@ void StereoHierCensusSgm::compute(cv::InputArray l, cv::InputArray r, cv::Output mat2gray(r, impl_->r); timer_set(); - static constexpr int DOWNSCALE_MEDIUM = 2; - static constexpr int DOWNSCALE_COARSE = 4; + static constexpr int DOWNSCALE_MEDIUM = 4; + static constexpr int DOWNSCALE_COARSE = 6; Array2D<uchar> medium_l(l.cols()/DOWNSCALE_MEDIUM, l.rows()/DOWNSCALE_MEDIUM); Array2D<uchar> medium_r(r.cols()/DOWNSCALE_MEDIUM, r.rows()/DOWNSCALE_MEDIUM); @@ -54,6 +92,18 @@ void StereoHierCensusSgm::compute(cv::InputArray l, cv::InputArray r, cv::Output cv::cuda::resize(impl_->l.toGpuMat(), coarse_l.toGpuMat(), cv::Size(coarse_l.width, coarse_l.height)); cv::cuda::resize(impl_->r.toGpuMat(), coarse_r.toGpuMat(), cv::Size(coarse_r.width, coarse_r.height)); + cv::cuda::GpuMat var_fine = impl_->var_fine.toGpuMat(); + variance_mask(impl_->l.toGpuMat(), var_fine, params.var_window); + cv::cuda::normalize(var_fine, var_fine, params.alpha, params.beta, cv::NORM_MINMAX, -1); + + cv::cuda::GpuMat var_medium = impl_->var_medium.toGpuMat(); + variance_mask(medium_l.toGpuMat(), var_medium, params.var_window); + cv::cuda::normalize(var_medium, var_medium, params.alpha, params.beta, cv::NORM_MINMAX, -1); + + cv::cuda::GpuMat var_coarse = impl_->var_coarse.toGpuMat(); + variance_mask(coarse_l.toGpuMat(), var_coarse, params.var_window); + cv::cuda::normalize(var_coarse, var_coarse, params.alpha, params.beta, cv::NORM_MINMAX, -1); + // CT impl_->cost_fine.set(impl_->l, impl_->r); impl_->cost_medium.set(impl_->l, impl_->r, medium_l, medium_r); diff --git a/lib/libstereo/src/costs/dual.hpp b/lib/libstereo/src/costs/dual.hpp index 2c42ddaeebb8611b19613b55760841298a029359..cb4942005aaa900896c2f39c54105dd7608d078d 100644 --- a/lib/libstereo/src/costs/dual.hpp +++ b/lib/libstereo/src/costs/dual.hpp @@ -44,6 +44,29 @@ namespace impl { static constexpr Type COST_MAX = A::COST_MAX * N; }; + template <typename A, int N> + struct MultiCostsWeighted : DSImplBase<typename A::Type> { + typedef typename A::Type Type; + + MultiCostsWeighted(ushort w, ushort h, ushort dmin, ushort dmax) : DSImplBase<Type>({w,h,dmin,dmax}) {} + + __host__ __device__ inline Type operator()(const int y, const int x, const int d) const { + float cost = 0; + float pw = 1.0f; + #pragma unroll + for (int n=0; n<N; ++n) { + float w = 1.0f-weights[n](y,x); + cost += pw*w*float(costs[n](y,x,d)); + pw *= 1.0f-w; + } + return round(cost); + } + + A costs[N]; + Array2D<float>::Data weights[N]; + static constexpr Type COST_MAX = A::COST_MAX; + }; + template <typename A, typename B> struct DualCostsWeighted : DSImplBase<typename A::Type> { static_assert(std::is_same<typename A::Type, typename B::Type>::value, "Cost types must be the same"); @@ -115,6 +138,35 @@ protected: A *costs[N]; }; +template <typename A, int N> +class MultiCostsWeighted : public DSBase<impl::MultiCostsWeighted<typename A::DataType,N>> { +public: + typedef impl::MultiCostsWeighted<typename A::DataType,N> DataType; + typedef typename A::DataType::Type Type; + + MultiCostsWeighted(int width, int height, int disp_min, int disp_max) + : DSBase<DataType>(width, height, disp_min, disp_max) {}; + + void add(int n, A &c, Array2D<float> &w) { + if (n >= N || n < 0) throw std::exception(); + costs[n] = &c; + weights[n] = &w; + } + + void set() { + for (int n=0; n<N; ++n) { + this->data().costs[n] = costs[n]->data(); + this->data().weights[n] = weights[n]->data(); + } + } + + static constexpr Type COST_MAX = A::COST_MAX; + +protected: + A *costs[N]; + Array2D<float> *weights[N]; +}; + /** * Combine two cost functions with weight. Weight in Array2D<float> same size * as input. Weight values assumed in range [0,1] and cost is calculated as