From 029de4afa326886c8854a6d106fb357d33ea4be3 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Thu, 28 Mar 2019 10:32:13 +0200
Subject: [PATCH] Fix point cloud display and work better with libsgm
---
cv-node/CMakeLists.txt | 2 +
cv-node/src/algorithms/fixstars_sgm.cpp | 14 ++++--
cv-node/src/algorithms/nick1.cu | 59 +++++++++++++++++--------
cv-node/src/algorithms/rtcensus.cpp | 10 +++--
cv-node/src/display.cpp | 19 ++------
5 files changed, 64 insertions(+), 40 deletions(-)
diff --git a/cv-node/CMakeLists.txt b/cv-node/CMakeLists.txt
index 2830734c8..55a3dc0cc 100644
--- a/cv-node/CMakeLists.txt
+++ b/cv-node/CMakeLists.txt
@@ -80,6 +80,7 @@ set(CVNODESRC
src/disparity.cpp
src/middlebury.cpp
src/algorithms/rtcensus.cpp
+ src/algorithms/rtcensus_sgm.cpp
src/algorithms/opencv_sgbm.cpp
src/algorithms/opencv_bm.cpp
)
@@ -93,6 +94,7 @@ if (CUDA_FOUND)
"src/algorithms/opencv_cuda_bm.cpp"
"src/algorithms/opencv_cuda_bp.cpp"
"src/algorithms/rtcensus.cu"
+ "src/algorithms/rtcensus_sgm.cu"
"src/algorithms/consistency.cu"
"src/algorithms/sparse_census.cu"
"src/algorithms/tex_filter.cu"
diff --git a/cv-node/src/algorithms/fixstars_sgm.cpp b/cv-node/src/algorithms/fixstars_sgm.cpp
index 216cf8e93..f36c7a16e 100644
--- a/cv-node/src/algorithms/fixstars_sgm.cpp
+++ b/cv-node/src/algorithms/fixstars_sgm.cpp
@@ -13,15 +13,23 @@ void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
Mat left_disp;
Mat right_disp;
+ Mat lbw, rbw;
+ cv::cvtColor(l, lbw, cv::COLOR_BGR2GRAY);
+ cv::cvtColor(r, rbw, cv::COLOR_BGR2GRAY);
+
if (!ssgm_) {
ssgm_ = new sgm::StereoSGM(l.cols, l.rows, max_disp_, 8, 8, sgm::EXECUTE_INOUT_HOST2HOST);
}
- if (disp.cols != l.cols || disp.rows != l.rows) {
+ //disp = Mat();
+
+ //if (disp.cols != l.cols || disp.rows != l.rows) {
disp = Mat(cv::Size(l.cols, l.rows), CV_8UC1);
- }
+ //}
+
+ ssgm_->execute(lbw.data, rbw.data, disp.data);
- ssgm_->execute(l.data, r.data, disp.data);
+ disp.convertTo(disp, CV_32F, 1.0f);
}
diff --git a/cv-node/src/algorithms/nick1.cu b/cv-node/src/algorithms/nick1.cu
index 8858dcecc..44de52257 100644
--- a/cv-node/src/algorithms/nick1.cu
+++ b/cv-node/src/algorithms/nick1.cu
@@ -86,7 +86,7 @@ __device__ bool is_edge_right(uchar4 *line, int x, int n) {
return (colour_error(line[x-1],line[x]) <= EDGE_SENSITIVITY && colour_error(line[x],line[x+1]) > EDGE_SENSITIVITY);
}
-__global__ void filter_kernel(cudaTextureObject_t t, cudaTextureObject_t d,
+/*__global__ void filter_kernel(cudaTextureObject_t t, cudaTextureObject_t d,
cudaTextureObject_t prevD,
cudaTextureObject_t prevT, PtrStepSz<float> f, int num_disp) {
@@ -159,11 +159,24 @@ __global__ void filter_kernel(cudaTextureObject_t t, cudaTextureObject_t d,
} else f(v,u) = NAN;
}
}
-}
+}*/
+
+__device__ float neighbour_factor(float a, cudaTextureObject_t p, int u, int v) {
+ float f = 1.0f;
+
+ for (int m=-1; m<=1; m++) {
+ for (int n=-1; n<=1; n++) {
+ float2 neighbour = tex2D<float2>(p, u+n, v+m);
+ if (neighbour.x > 8.0f && abs(neighbour.y-a) < 1.0f) f += neighbour.x / 10.0f;
+ }
+ }
+
+ return f;
+}
/* Use Prewitt operator */
-__global__ void edge_invar1_kernel(cudaTextureObject_t t, ftl::cuda::TextureObject<float2> o) {
+__global__ void edge_invar1_kernel(cudaTextureObject_t t, cudaTextureObject_t p, ftl::cuda::TextureObject<float2> o) {
for (STRIDE_Y(v,o.height())) {
for (STRIDE_X(u,o.width())) {
float gx = ((tex2D<uchar4>(t, u-1, v-1).z - tex2D<uchar4>(t, u+1, v-1).z) +
@@ -175,10 +188,15 @@ __global__ void edge_invar1_kernel(cudaTextureObject_t t, ftl::cuda::TextureObje
float g = sqrt(gx*gx+gy*gy);
float a = atan2(gy,gx);
-
- // TODO adapt threshold using histeresis
- o(u,v) = (g > 10.0f) ? make_float2(g,abs(a)) : make_float2(NAN,NAN);
- }
+
+ if (g > 1.0f) {
+ float2 n = tex2D<float2>(p, u, v);
+ float avg = (n.x > g && abs(n.y-a) < 0.2) ? (g+n.x) / 2.0f : g;
+ o(u,v) = make_float2(avg,abs(a));
+ } else {
+ o(u,v) = make_float2(NAN,NAN);
+ }
+ }
}
}
@@ -189,7 +207,7 @@ __device__ void edge_follow(float &sum, int &count, cudaTextureObject_t i1, int
float sumchange = 0.0f;
float2 pixel_i1 = tex2D<float2>(i1,u,v);
- for (int j=0; j<10; j++) {
+ for (int j=0; j<50; j++) {
// Vertical edge = 0, so to follow it don't move in x
int dx = ((pixel_i1.y >= 0.785 && pixel_i1.y <= 2.356) ) ? 0 : 1;
int dy = (dx == 1) ? 0 : 1;
@@ -208,8 +226,8 @@ __device__ void edge_follow(float &sum, int &count, cudaTextureObject_t i1, int
float diff = 10000.0f;
int nu, nv;
- for (int i=-5; i<=5; i++) {
- float2 pix = tex2D<float2>(i1,u2+dx*i+dy*1, v2+dy*i+dx*1);
+ for (int i=-2; i<=2; i++) {
+ float2 pix = tex2D<float2>(i1,u2+dx*i+dy*sign, v2+dy*i+dx*sign);
if (isnan(pix.x)) continue;
float d = abs(pix.x-pixel_i1.x)*abs(pix.y-pixel_i1.y);
@@ -249,7 +267,7 @@ __global__ void edge_invar2_kernel(cudaTextureObject_t i1, ftl::cuda::TextureObj
for (STRIDE_X(u,o.width())) {
float2 pixel_i1 = tex2D<float2>(i1,u,v);
- if (isnan(pixel_i1.x)) {
+ if (isnan(pixel_i1.x) || pixel_i1.x < 10.0f) {
o(u,v) = NAN;
continue;
}
@@ -271,11 +289,13 @@ __global__ void edge_invar2_kernel(cudaTextureObject_t i1, ftl::cuda::TextureObj
edge_follow(sum_b, count_b, i1, u, v, -1);
- // Output length of edge
- if (count_a+count_b > 5) {
- o(u,v) = ((sum_a+sum_b) / (float)(count_a+count_b)) * 300.0f + 50.0f;
+ // Output curvature of edge
+ if (count_a+count_b > 10) {
+ float curvature = ((sum_a+sum_b) / (float)(count_a+count_b));
+ //o(u,v) = curvature * 300.0f + 50.0f;
+ o(u,v) = (count_a+count_b) * 3.0f;
} else {
- o(u,v) = 200.0f;
+ o(u,v) = NAN;
}
//o(u,v) = (sumchange / (float)(j-1))*100.0f;
@@ -286,6 +306,7 @@ __global__ void edge_invar2_kernel(cudaTextureObject_t i1, ftl::cuda::TextureObj
}
}
+ftl::cuda::TextureObject<float2> prevEdge1;
ftl::cuda::TextureObject<float> prevDisp;
ftl::cuda::TextureObject<uchar4> prevImage;
@@ -305,21 +326,21 @@ void nick1_call(const PtrStepSz<uchar4> &l, const PtrStepSz<uchar4> &r, const Pt
grid.x = cv::cuda::device::divUp(l.cols - 2 * RADIUS2, BLOCK_W);
grid.y = cv::cuda::device::divUp(l.rows - 2 * RADIUS2, ROWSperTHREAD);
- edge_invar1_kernel<<<grid,threads>>>(texLeft.cudaTexture(), inv1);
+ edge_invar1_kernel<<<grid,threads>>>(texLeft.cudaTexture(), prevEdge1.cudaTexture(), inv1);
cudaSafeCall( cudaGetLastError() );
edge_invar2_kernel<<<grid,threads>>>(inv1.cudaTexture(), output);
cudaSafeCall( cudaGetLastError() );
- //prevImage.free();
- //prevImage = texLeft;
+ prevEdge1.free();
+ prevEdge1 = inv1;
//if (&stream == Stream::Null())
cudaSafeCall( cudaDeviceSynchronize() );
texLeft.free();
texRight.free();
- inv1.free();
+ //inv1.free();
output.free();
}
diff --git a/cv-node/src/algorithms/rtcensus.cpp b/cv-node/src/algorithms/rtcensus.cpp
index 2c3222552..5dcff9d29 100644
--- a/cv-node/src/algorithms/rtcensus.cpp
+++ b/cv-node/src/algorithms/rtcensus.cpp
@@ -206,10 +206,14 @@ void RTCensus::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
void RTCensus::computeCPU(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
size_t d_min = min_disp_;
size_t d_max = max_disp_;
+
+ Mat lbw, rbw;
+ cv::cvtColor(l, lbw, cv::COLOR_BGR2GRAY);
+ cv::cvtColor(r, rbw, cv::COLOR_BGR2GRAY);
auto start = std::chrono::high_resolution_clock::now();
- auto census_R = sparse_census_16x16(r);
- auto census_L = sparse_census_16x16(l);
+ auto census_R = sparse_census_16x16(rbw);
+ auto census_L = sparse_census_16x16(lbw);
std::chrono::duration<double> elapsed = std::chrono::high_resolution_clock::now() - start;
LOG(INFO) << "Census in " << elapsed.count() << "s";
@@ -224,7 +228,7 @@ void RTCensus::computeCPU(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
auto disp_L = d_sub(dsi_ca_L, l.cols, l.rows, d_max-d_min);
LOG(INFO) << "Disp done";
- disp = disp_R; //consistency(disp_R, disp_L);
+ disp = consistency(disp_R, disp_L);
// TODO confidence and texture filtering
}
diff --git a/cv-node/src/display.cpp b/cv-node/src/display.cpp
index 0c1d13a7b..74cc94b4a 100644
--- a/cv-node/src/display.cpp
+++ b/cv-node/src/display.cpp
@@ -17,30 +17,19 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) {
Mat idepth;
if (config_["points"]) {
- cv::Mat Q_32F; // = (Mat_<double>(4,4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); //(4,4,CV_32F);
+ cv::Mat Q_32F;
calibrate_.getQ().convertTo(Q_32F,CV_32F);
cv::Mat_<cv::Vec3f> XYZ(depth.rows,depth.cols); // Output point cloud
- reprojectImageTo3D(depth, XYZ, Q_32F, false);
+ reprojectImageTo3D(depth+20.0f, XYZ, Q_32F, true);
- //cv::imshow("Points",XYZ);
+ // Remove all invalid pixels from point cloud
+ XYZ.setTo(Vec3f(NAN,NAN,NAN), depth == 0.0f);
cv::viz::WCloud cloud_widget = cv::viz::WCloud( XYZ, rgb );
cloud_widget.setRenderingProperty( cv::viz::POINT_SIZE, 2 );
- /* Rotation using rodrigues */
- /// Rotate around (1,1,1)
- /*rot_vec.at<float>(0,0) = 0.0f;
- rot_vec.at<float>(0,1) = 0.0f;
- rot_vec.at<float>(0,2) = CV_PI * 1.0f;*/
-
- //Mat rot_mat;
- //Rodrigues(rot_vec, rot_mat);
-
- /// Construct pose
- //Affine3f pose(rot_mat, Vec3f(0, 20.0, 0));
window_->showWidget( "coosys", viz::WCoordinateSystem() );
window_->showWidget( "Depth", cloud_widget );
- //window_->setWidgetPose("Depth", pose);
window_->spinOnce( 1, true );
}
--
GitLab