diff --git a/lib/libstereo/src/algorithms/clustersf.cu b/lib/libstereo/src/algorithms/clustersf.cu
index cd6aaad1eac46f6f172c18c9229844a1eb5ee031..27bbd38b39906726395d934406b566e595b3180f 100644
--- a/lib/libstereo/src/algorithms/clustersf.cu
+++ b/lib/libstereo/src/algorithms/clustersf.cu
@@ -38,16 +38,28 @@ void StereoCSF::compute(cv::InputArray l, cv::InputArray r, cv::OutputArray disp
mat2gray(l, impl_->l);
mat2gray(r, impl_->r);
- SalientGradient sgl = {make_short2(1000, 800), 200, impl_->l.data(), impl_->gl.data(), impl_->temp.data(), impl_->buckets_l.data(), impl_->l.width, impl_->l.height};
+ short2 focal_pt = {short(1000), short(800)};
+
+ SalientGradient sgl = {focal_pt, 1000, impl_->l.data(), impl_->gl.data(), impl_->temp.data(), impl_->buckets_l.data(), impl_->l.width, impl_->l.height};
parallel1DWarpSM(sgl, l.rows(), l.cols());
SalientGradientGrouped sgr = {impl_->r.data(), impl_->gr.data(), impl_->temp.data(), impl_->buckets_r.data(), impl_->r.width, impl_->r.height};
parallel1DWarpSM(sgr, r.rows(), r.cols());
impl_->focal.toGpuMat().setTo(cv::Scalar(0));
- FocalCluster fc = {make_short2(1000, 800), impl_->buckets_l.data(), impl_->buckets_r.data(), impl_->focal.data(), 1024};
+ FocalCluster fc = {focal_pt, impl_->buckets_l.data(), impl_->buckets_r.data(), impl_->focal.data(), 1024};
parallel1DWarp(fc, l.rows(), 1);
+ Array2D<float> disp_array(l.cols(), l.rows());
+ FocalSelector fs = {focal_pt, impl_->buckets_l.data(), impl_->buckets_r.data(), impl_->focal.data(), disp_array.data(), 1024};
+ parallel1DWarp(fs, l.rows(), 1);
+
+ disp_array.toGpuMat().download(disparity);
+
+ cv::Mat gradtmp;
+ impl_->gl.toGpuMat().download(gradtmp);
+ cv::imshow("Gradient Left", gradtmp);
+
cv::Mat tmp;
impl_->focal.toGpuMat().download(tmp);
@@ -59,7 +71,7 @@ void StereoCSF::compute(cv::InputArray l, cv::InputArray r, cv::OutputArray disp
std::cout << "Focal Disparity = " << maxloc[1] << std::endl;
- tmp.convertTo(tmp, CV_8UC1, 10);
+ tmp.convertTo(tmp, CV_8UC1, 255.0/maxval);
cv::resize(tmp,tmp, cv::Size(tmp.cols, 100));
cv::applyColorMap(tmp, tmp, cv::COLORMAP_TURBO);
cv::imshow("Disparity Hist", tmp);
diff --git a/lib/libstereo/src/filters/focal_cluster.hpp b/lib/libstereo/src/filters/focal_cluster.hpp
index 26e6cda17a69e428c48366ea8aa508829a2ea186..84e8abfdc124cad0cc2f213a0e0f78f638fa9111 100644
--- a/lib/libstereo/src/filters/focal_cluster.hpp
+++ b/lib/libstereo/src/filters/focal_cluster.hpp
@@ -3,6 +3,7 @@
#include "../util.hpp"
#include "../array1d.hpp"
+#include "../array2d.hpp"
#include "../bucket1d.hpp"
#include "../bucket2d.hpp"
@@ -36,4 +37,47 @@ struct FocalCluster {
}
};
+struct FocalSelector {
+ short2 focal_pt;
+ Bucket1D<short2, 64>::Data left;
+ Bucket2D<ushort, 64>::Data right;
+ Array1D<int>::Data histogram;
+ Array2D<float>::Data disparity;
+
+ int max_disparity = 1024;
+
+ __device__ void operator()(ushort2 thread, ushort2 stride, ushort2 size) {
+ for (int y=thread.y; y<size.y; y+=stride.y) {
+ int count = left(y);
+ // Stride a warp of threads over the features
+ for (int f=thread.x; f<count; f+=stride.x) {
+ // For each feature or features near to focal point
+ short2 feature = left(y,f);
+ int distx = feature.x - focal_pt.x;
+
+ int best_disp = 0;
+ int max_v = 0;
+
+ // - For each feature in right image that matches
+ const ushort *ptr = right.ptr(y, feature.y);
+ int count2 = right(y,feature.y);
+ for (int i=0; i<count2; ++i) {
+ // - Add focal dist to feature X and add to histogram
+ int d = max(0,focal_pt.x - int(ptr[i]) + distx);
+
+ int v = histogram(d);
+ if (v > max_v) {
+ max_v = v;
+ best_disp = d;
+ }
+ }
+
+ if (max_v > 0) {
+ disparity(y,feature.x) = float(best_disp);
+ }
+ }
+ }
+ }
+};
+
#endif
diff --git a/lib/libstereo/src/filters/salient_gradient.hpp b/lib/libstereo/src/filters/salient_gradient.hpp
index 969cce77d5d1dbd27e6c35840aeff79bbd1a8598..d1bbbcc9d9a00faa3cd48fb5d7a38853fddf56a6 100644
--- a/lib/libstereo/src/filters/salient_gradient.hpp
+++ b/lib/libstereo/src/filters/salient_gradient.hpp
@@ -87,19 +87,24 @@ struct SalientGradient {
// Apply threshold
for (int x=thread.x; x<size.x; x+=stride.x) {
- uchar focal_thresh = max(255,int((1.0f - weighting(float(abs(focal_pt.x - x)), float(radius)))*255.0f));
- uchar thresh = max(gthresh, focal_thresh);
- for (int u=-3; u<=3; ++u) {
- thresh = (x+u >= 0 && x+u < width) ? max(thresh, magnitude(y,x+u)) : thresh;
- }
-
- uchar m = magnitude(y,x);
- //if (m < thresh) angle(y,x) = 0;
+ float dx = focal_pt.x - x;
+ float dy = focal_pt.y - y;
+ float dist = sqrt(dx*dx + dy*dy);
+ float weight = weighting(dist, float(radius));
+ float thresh = gthresh; //max(gthresh, focal_thresh);
+ //for (int u=-3; u<=3; ++u) {
+ // thresh = (x+u >= 0 && x+u < width) ? max(thresh, float(magnitude(y,x+u))) : thresh;
+ //}
+
+ float m = float(magnitude(y,x)) * weight;
+ if (m < thresh) angle(y,x) = 0;
//output(y,x) = (m < thresh) ? 0 : 255;
if (m >= thresh) {
int a = angle(y,x);
buckets.add(y, make_short2(x, a));
}
+
+ angle(y,x) = uchar(weight*255.0f);
}
}
}
@@ -175,9 +180,9 @@ struct SalientGradientGrouped {
// Apply threshold
for (int x=thread.x; x<size.x; x+=stride.x) {
uchar thresh = gthresh;
- for (int u=-3; u<=3; ++u) {
- thresh = (x+u >= 0 && x+u < width) ? max(thresh, magnitude(y,x+u)) : thresh;
- }
+ //for (int u=-3; u<=3; ++u) {
+ // thresh = (x+u >= 0 && x+u < width) ? max(thresh, magnitude(y,x+u)) : thresh;
+ //}
uchar m = magnitude(y,x);
//if (m < thresh) angle(y,x) = 0;