diff --git a/lib/libstereo/src/algorithms/clustersf.cu b/lib/libstereo/src/algorithms/clustersf.cu
index f6c944548f769271492fb135419aea7bda708f5d..5db9db3d6ef72db963d3df7a1e3b4a2c1c6b4751 100644
--- a/lib/libstereo/src/algorithms/clustersf.cu
+++ b/lib/libstereo/src/algorithms/clustersf.cu
@@ -5,6 +5,7 @@
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
+#include <opencv2/cudaarithm.hpp>
struct StereoCSF::Impl {
Array2D<uchar> l;
@@ -41,21 +42,42 @@ void StereoCSF::compute(cv::InputArray l, cv::InputArray r, cv::OutputArray disp
Array2D<float> disp_array(l.cols(), l.rows());
disp_array.toGpuMat().setTo(cv::Scalar(0.0f));
- 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());
-
+ Array2D<float> conf_array(l.cols(), l.rows());
+ conf_array.toGpuMat().setTo(cv::Scalar(0.0f));
+
+ //int fx=1000;
+ //int fy=800;
for (int fx = 200; fx < l.cols()-200; fx += 50) {
for (int fy = 200; fy < l.rows()-200; fy += 50) {
- short2 focal_pt = {short(fx), short(fy)};
- 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());
- impl_->focal.toGpuMat().setTo(cv::Scalar(0));
-
- FocalCluster fc = {focal_pt, impl_->buckets_l.data(), impl_->buckets_r.data(), impl_->focal.data(), 1024};
- parallel1DWarp(fc, l.rows(), 1);
-
- FocalSelector fs = {focal_pt, impl_->buckets_l.data(), impl_->buckets_r.data(), impl_->focal.data(), disp_array.data(), 1024};
- parallel1DWarp(fs, l.rows(), 1);
+ short2 focal_pt_est = {short(l.cols()/2), short(l.rows()/2)};
+ int focal_radius_est = 100000;
+ int focal_radius = 1000;
+
+ for (int i=0; i<3; ++i) {
+ SalientGradientGrouped sgr = {focal_pt_est, focal_radius_est, 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());
+
+ short2 focal_pt = {short(fx), short(fy)};
+ SalientGradient sgl = {focal_pt, focal_radius, 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());
+ impl_->focal.toGpuMat().setTo(cv::Scalar(0));
+
+ FocalCluster fc = {focal_pt, impl_->buckets_l.data(), impl_->buckets_r.data(), impl_->focal.data(), 1024};
+ parallel1DWarp(fc, l.rows(), 1);
+
+ cv::Point max_disp;
+ cv::cuda::minMaxLoc(impl_->focal.toGpuMat(), nullptr, nullptr, nullptr, &max_disp);
+
+ FocalSelector fs = {focal_pt, int(max_disp.x), impl_->buckets_l.data(), impl_->buckets_r.data(), impl_->focal.data(), disp_array.data(), conf_array.data(), 1024};
+ parallel1DWarp(fs, l.rows(), 1);
+
+ // Update right focal point estimate and radius
+ focal_pt_est.y = focal_pt.y;
+ focal_pt_est.x = focal_pt.x-max_disp.x;
+ focal_radius_est = focal_radius;
+ focal_radius /= 2;
+ //std::cout << "Focal disparity = " << max_disp.x << std::endl;
+ }
}
}
@@ -63,7 +85,7 @@ void StereoCSF::compute(cv::InputArray l, cv::InputArray r, cv::OutputArray disp
cv::Mat gradtmp;
impl_->gl.toGpuMat().download(gradtmp);
- //cv::imshow("Gradient Left", gradtmp);
+ cv::imshow("Gradient Left", gradtmp);
cv::Mat tmp;
impl_->focal.toGpuMat().download(tmp);
@@ -78,7 +100,12 @@ void StereoCSF::compute(cv::InputArray l, cv::InputArray r, cv::OutputArray disp
tmp.convertTo(tmp, CV_8UC1, 255.0/maxval);
cv::resize(tmp,tmp, cv::Size(tmp.cols, 100));
- cv::applyColorMap(tmp, tmp, cv::COLORMAP_TURBO);
+
+ //#if OPENCV_VERSION >= 40102
+ //cv::applyColorMap(tmp, tmp, cv::COLORMAP_TURBO);
+ //#else
+ cv::applyColorMap(tmp, tmp, cv::COLORMAP_INFERNO);
+ //#endif
cv::imshow("Disparity Hist", tmp);
cv::Mat imgleft, imgright;
diff --git a/lib/libstereo/src/filters/focal_cluster.hpp b/lib/libstereo/src/filters/focal_cluster.hpp
index 84e8abfdc124cad0cc2f213a0e0f78f638fa9111..bc1126b9aecc9472da3d77a5aa31d6ecaddf5edf 100644
--- a/lib/libstereo/src/filters/focal_cluster.hpp
+++ b/lib/libstereo/src/filters/focal_cluster.hpp
@@ -39,10 +39,12 @@ struct FocalCluster {
struct FocalSelector {
short2 focal_pt;
+ int focal_disp;
Bucket1D<short2, 64>::Data left;
Bucket2D<ushort, 64>::Data right;
Array1D<int>::Data histogram;
Array2D<float>::Data disparity;
+ Array2D<float>::Data confidence;
int max_disparity = 1024;
@@ -73,7 +75,11 @@ struct FocalSelector {
}
if (max_v > 0) {
- disparity(y,feature.x) = float(best_disp);
+ float conf = 1.0f - (float(abs(best_disp-focal_disp)) / float(max_disparity));
+ if (conf > confidence(y,feature.x)) {
+ disparity(y,feature.x) = float(best_disp);
+ confidence(y,feature.x) = conf;
+ }
}
}
}
diff --git a/lib/libstereo/src/filters/salient_gradient.hpp b/lib/libstereo/src/filters/salient_gradient.hpp
index 1c08985d55f5cb95d99c8839c198357fed242f28..10ca19b114426a177491ea2100d28a386c09e9f8 100644
--- a/lib/libstereo/src/filters/salient_gradient.hpp
+++ b/lib/libstereo/src/filters/salient_gradient.hpp
@@ -61,6 +61,14 @@ struct SalientGradient {
return rh*rh*rh*rh;
}
+ __device__ inline float calcWeight(int x, int y) {
+ 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));
+ return weight;
+ }
+
__device__ void operator()(ushort2 thread, ushort2 stride, ushort2 size, WarpSharedMemory &wsm) {
static const float PI = 3.14f;
static constexpr float PI2 = PI/2.0f;
@@ -73,6 +81,7 @@ struct SalientGradient {
for (int x=thread.x; x<size.x; x+=stride.x) {
auto g = calculateGradient(x,y);
+ //float weight = calcWeight(x,y);
angle(y,x) = uchar(min(15,int((g.y+PI2) / PI * 16.0f)));
magnitude(y,x) = uchar(g.x);
@@ -87,10 +96,7 @@ struct SalientGradient {
// Apply threshold
for (int x=thread.x; x<size.x; x+=stride.x) {
- 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 weight = calcWeight(x,y);
float thresh = gthresh; //max(gthresh, focal_thresh);
//for (int u=-3; u<=3; ++u) {
// thresh = (x+u >= 0 && x+u < width) ? max(thresh, weight*float(magnitude(y,x+u))) : thresh;
@@ -117,6 +123,8 @@ struct SalientGradient {
* features based upon scanline and orientation.
*/
struct SalientGradientGrouped {
+ short2 focal_pt;
+ int radius;
Array2D<uchar>::Data image;
Array2D<uchar>::Data angle;
Array2D<uchar>::Data magnitude;
@@ -153,6 +161,21 @@ struct SalientGradientGrouped {
return __ffs(t);
}
+ __device__ inline float weighting(float r, float h) {
+ if (r >= h) return 0.0f;
+ float rh = r / h;
+ rh = 1.0f - rh*rh;
+ return rh*rh*rh*rh;
+ }
+
+ __device__ inline float calcWeight(int x, int y) {
+ 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));
+ return weight;
+ }
+
__device__ void operator()(ushort2 thread, ushort2 stride, ushort2 size, WarpSharedMemory &wsm) {
static const float PI = 3.14f;
static constexpr float PI2 = PI/2.0f;
@@ -179,12 +202,13 @@ struct SalientGradientGrouped {
// Apply threshold
for (int x=thread.x; x<size.x; x+=stride.x) {
- uchar thresh = gthresh;
+ float weight = calcWeight(x,y);
+ float thresh = gthresh;
//for (int u=-3; u<=3; ++u) {
- // thresh = (x+u >= 0 && x+u < width) ? max(thresh, magnitude(y,x+u)) : thresh;
+ // thresh = (x+u >= 0 && x+u < width) ? max(thresh, weight*float(magnitude(y,x+u))) : thresh;
//}
- uchar m = magnitude(y,x);
+ float m = float(magnitude(y,x))*weight;
//if (m < thresh) angle(y,x) = 0;
//output(y,x) = (m < thresh) ? 0 : 255;
if (m >= thresh) {