openCV learning and NanRui fuse wire detection

.gitignore

@@ -16,6 +16,8 @@ msg/_*.py
 build_isolated/
 devel_isolated/
 .vscode/
+*.txt
+*.jpg
 
 # Generated by dynamic reconfigure
 *.cfgc

cpp_pkgs/src/basicTest.cpp

@@ -80,7 +80,8 @@ int main(){
     trace2(1);
     std::cout<<int(-2.5)<<std::endl;
     //std::cout.setf(std::ios::hex, std::ios::basefield);
-    //std::cout<< ( -2 )<<std::endl;
+    std::cout<< atan2(sin(1.58),cos(1.58))<<std::endl;
+    std::cout<< atan2(sin(-1.58),cos(-1.58))<<std::endl;
 }
 
-#endif
+#endif

cuda_test/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>cuda_test</name>
+  <version>0.0.0</version>
+  <description>The cuda_test package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="youchen@todo.todo">youchen</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/cuda_test</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

cuda_test/src/Steger.cpp

@@ -0,0 +1,597 @@
+#include "Steger.hpp"
+#include <stdlib.h>
+#include <time.h>
+#include <cuda.h>
+#include <cuda_runtime.h>
+#include "detectlines.h"
+#define SQRT_2_PI_INV 0.398942280401432677939946059935
+#define SQRTPI 1.772453850905516027
+#define SQRT2 1.41421356237309504880
+#define UPPERLIMIT 20.0 
+
+#define P10 242.66795523053175
+#define P11 21.979261618294152
+#define P12 6.9963834886191355
+#define P13 -.035609843701815385
+#define Q10 215.05887586986120
+#define Q11 91.164905404514901
+#define Q12 15.082797630407787
+#define Q13 1.0
+
+#define P20 300.4592610201616005
+#define P21 451.9189537118729422
+#define P22 339.3208167343436870
+#define P23 152.9892850469404039
+#define P24 43.16222722205673530
+#define P25 7.211758250883093659
+#define P26 .5641955174789739711
+#define P27 -.0000001368648573827167067
+#define Q20 300.4592609569832933
+#define Q21 790.9509253278980272
+#define Q22 931.3540948506096211
+#define Q23 638.9802644656311665
+#define Q24 277.5854447439876434
+#define Q25 77.00015293522947295
+#define Q26 12.78272731962942351
+#define Q27 1.0
+
+#define P30 -.00299610707703542174
+#define P31 -.0494730910623250734
+#define P32 -.226956593539686930
+#define P33 -.278661308609647788
+#define P34 -.0223192459734184686
+#define Q30 .0106209230528467918
+#define Q31 .191308926107829841
+#define Q32 1.05167510706793207
+#define Q33 1.98733201817135256
+#define Q34 1.0
+
+
+void compute_eigenvals(
+double dfdrr,
+double dfdrc,
+double dfdcc,
+double eigval[2],
+double eigvec[2][2])
+{
+	double theta, t, c, s, e1, e2, n1, n2; /* , phi; */
+
+	/* Compute the eigenvalues and eigenvectors of the Hessian cv::Matrix. */
+	if (dfdrc != 0.0) {
+		theta = 0.5*(dfdcc - dfdrr) / dfdrc;
+		t = 1.0 / (fabs(theta) + sqrt(theta*theta + 1.0));
+		if (theta < 0.0) t = -t;
+		c = 1.0 / sqrt(t*t + 1.0);
+		s = t*c;
+		e1 = dfdrr - t*dfdrc;
+		e2 = dfdcc + t*dfdrc;
+	}
+	else {
+		c = 1.0;
+		s = 0.0;
+		e1 = dfdrr;
+		e2 = dfdcc;
+	}
+	n1 = c;
+	n2 = -s;
+
+	/* If the absolute value of an eigenvalue is larger than the other, put that
+	eigenvalue into first position.  If both are of equal absolute value, put
+	the negative one first. */
+	if (fabs(e1) > fabs(e2)) {
+		eigval[0] = e1;
+		eigval[1] = e2;
+		eigvec[0][0] = n1;
+		eigvec[0][1] = n2;
+		eigvec[1][0] = -n2;
+		eigvec[1][1] = n1;
+	}
+	else if (fabs(e1) < fabs(e2)) {
+		eigval[0] = e2;
+		eigval[1] = e1;
+		eigvec[0][0] = -n2;
+		eigvec[0][1] = n1;
+		eigvec[1][0] = n1;
+		eigvec[1][1] = n2;
+	}
+	else {
+		if (e1 < e2) {
+			eigval[0] = e1;
+			eigval[1] = e2;
+			eigvec[0][0] = n1;
+			eigvec[0][1] = n2;
+			eigvec[1][0] = -n2;
+			eigvec[1][1] = n1;
+		}
+		else {
+			eigval[0] = e2;
+			eigval[1] = e1;
+			eigvec[0][0] = -n2;
+			eigvec[0][1] = n1;
+			eigvec[1][0] = n1;
+			eigvec[1][1] = n2;
+		}
+	}
+}
+
+void solve_linear(
+double a,
+double b,
+double *t,
+long   *num)
+{
+	if (a == 0.0) {
+		*num = 0;
+		return;
+	}
+	else {
+		*num = 1;
+		*t = -b / a;
+		return;
+	}
+}
+
+void compute_line_points(
+	float   *ku[5],
+unsigned char    *ismax,
+float   *ev,
+float   *nx,
+float   *ny,
+float   *px,
+float   *py,
+long    width,
+long    height,
+double  low,
+double  high,
+long    mode)
+{
+	long    r, c, l;
+	double  k[5];
+	double  eigval[2];
+	double  eigvec[2][2];
+	double  a, b, t;
+	long    num;
+	double  n1, n2;
+	double  p1, p2;
+	double  val;
+
+	for (r = 0; r < height; r++)
+	{
+		for (c = 0; c < width; c++) 
+		{
+			l = LINCOOR(r, c, width);
+			k[0] = ku[0][l];
+			k[1] = ku[1][l];
+			k[2] = ku[2][l];
+			k[3] = ku[3][l];
+			k[4] = ku[4][l];
+			ev[l] = 0.0;
+			nx[l] = 0.0;
+			ny[l] = 0.0;
+			compute_eigenvals(k[2], k[3], k[4], eigval, eigvec);
+			if (mode == MODE_LIGHT)
+				val = -eigval[0];
+			else
+				val = eigval[0];
+			if (val > 0.0) {
+				ev[l] = val;
+				n1 = eigvec[0][0];
+				n2 = eigvec[0][1];
+				a = k[2] * n1*n1 + 2.0*k[3] * n1*n2 + k[4] * n2*n2;
+				b = k[0] * n1 + k[1] * n2;
+				solve_linear(a, b, &t, &num);
+				if (num != 0) {
+					p1 = t*n1;
+					p2 = t*n2;
+					if (fabs(p1) <= PIXEL_BOUNDARY && fabs(p2) <= PIXEL_BOUNDARY) 
+					{
+						if (val >= low) 
+						{
+							if (val >= high)
+								ismax[l] = 2;
+							else
+								ismax[l] = 1;
+						}
+						nx[l] = n1;
+						ny[l] = n2;
+						px[l] = r + p1;
+						py[l] = c + p2;
+					}
+				}
+			}
+		}
+	}
+}
+
+
+
+Steger::Steger()
+{
+	m_sigma=3.0;
+	m_low=0.5;
+	m_high=3.5;
+
+	m_mask_0_r=0;
+	m_mask_0_c=0;
+	m_mask_1_r=0;
+	m_mask_1_c=0;
+	m_mask_2_r=0;
+	m_mask_2_c=0;
+
+	m_image=0;
+	m_mid_data=0;
+	m_width=0;
+	m_height=0;
+	for (int i = 0; i < 5; i++)
+		m_k[i] = 0;
+}
+
+Steger::Steger(double sigma, double low, double high)
+{
+
+	m_sigma = sigma;
+	m_low = low;
+	m_high = high;
+	/*if (m_mask_0_r){ cudaFree(m_mask_0_r); m_mask_0_r = 0; m_l_0r = 0; }
+	if (m_mask_0_c){ cudaFree(m_mask_0_c); m_mask_0_c = 0; m_l_0c = 0; }
+	if (m_mask_1_r){ cudaFree(m_mask_1_r); m_mask_1_r = 0; m_l_1r = 0; }
+	if (m_mask_1_c){ cudaFree(m_mask_1_c); m_mask_1_c = 0; m_l_1c = 0; }
+	if (m_mask_2_r){ cudaFree(m_mask_2_r); m_mask_2_r = 0; m_l_2r = 0; }
+	if (m_mask_2_c){ cudaFree(m_mask_2_c); m_mask_2_c = 0; m_l_2c = 0; }*/
+	
+	m_mask_0_r = compute_gauss_mask_0(&m_l_0r, sigma);
+	m_mask_0_c = compute_gauss_mask_0(&m_l_0c, sigma);
+	m_mask_1_r = compute_gauss_mask_1(&m_l_1r, sigma);
+	m_mask_1_c = compute_gauss_mask_1(&m_l_1c, sigma);
+	m_mask_2_r = compute_gauss_mask_2(&m_l_2r, sigma);
+	m_mask_2_c = compute_gauss_mask_2(&m_l_2c, sigma);
+	m_image = 0;
+	m_mid_data = 0;
+	for (int i = 0; i < 5; i++)
+		m_k[i] = 0;
+}
+Steger::~Steger()
+{
+	if (m_mask_0_r){ cudaFree(m_mask_0_r); m_mask_0_r = 0; m_l_0r = 0; }
+	if (m_mask_0_c){ cudaFree(m_mask_0_c); m_mask_0_c = 0; m_l_0c = 0; }
+	if (m_mask_1_r){ cudaFree(m_mask_1_r); m_mask_1_r = 0; m_l_1r = 0; }
+	if (m_mask_1_c){ cudaFree(m_mask_1_c); m_mask_1_c = 0; m_l_1c = 0; }
+	if (m_mask_2_r){ cudaFree(m_mask_2_r); m_mask_2_r = 0; m_l_2r = 0; }
+	if (m_mask_2_c){ cudaFree(m_mask_2_c); m_mask_2_c = 0; m_l_2c = 0; }
+	if (m_image){ cudaFree(m_image); }
+	if (m_mid_data){ cudaFree(m_mid_data); }
+	for (int i = 0; i < 5; i++)
+	{
+		if (m_k[i])
+			cudaFree(m_k[i]);
+	}
+}
+
+bool Steger::detect(cv::Mat src, cv::Mat& out)
+{
+	if (src.empty())return false;
+	if (m_width != src.cols || m_height != src.rows)
+	{
+		if (m_image){ 
+			cudaFree(m_image);	
+		}
+		if (m_mid_data){ 
+			cudaFree(m_mid_data); 
+		}
+		for (int i = 0; i < 5; i++)
+		{
+			if (m_k[i])
+				cudaFree(m_k[i]);
+			cudaMalloc((void**)&m_k[i], src.cols*src.rows*sizeof(float));
+		}
+		cudaMalloc((void**)&m_mid_data, src.cols*src.rows*sizeof(float));
+		cudaMalloc((void**)&m_image, src.cols*src.rows*sizeof(float));
+		m_width = src.cols;
+		m_height = src.rows;
+	}
+
+	clock_t t[6];
+	t[0] = clock();
+	cudaMemcpy(m_image, src.data, m_width*m_height*sizeof(float),cudaMemcpyHostToDevice);
+	
+	
+	run_gpu();
+	unsigned char    *ismax;
+	float  *ev, *n1, *n2, *p1, *p2;
+
+	ismax = (unsigned char *)malloc(m_width*m_height*sizeof(unsigned char));
+	ev = (float*)malloc(m_width*m_height* sizeof(float));
+	n1 = (float*)malloc(m_width*m_height* sizeof(float));
+	n2 = (float*)malloc(m_width*m_height* sizeof(float));
+	p1 = (float*)malloc(m_width*m_height* sizeof(float));
+	p2 = (float*)malloc(m_width*m_height* sizeof(float));
+	memset(ismax, 0, m_width*m_height*sizeof(unsigned char));
+	memset(ev, 0, m_width*m_height*sizeof(float));
+	t[1] = clock();
+	contour **contours, *cont;
+	long   num_cont;
+
+	float* k[5];
+	for (int i = 0; i < 5; i++)
+	{
+		k[i] = (float*)malloc(m_width*m_height* sizeof(float));
+		cudaMemcpy(k[i], m_k[i], m_width*m_height*sizeof(float), cudaMemcpyDeviceToHost);
+	}
+	t[2] = clock();
+	compute_line_points(k, ismax, ev, n1, n2, p1, p2, m_width, m_height, m_low, m_high, MODE_LIGHT);
+	t[3] = clock();
+	compute_contours(ismax, ev, n1, n2, p1, p2, k[0], k[1], &contours, &num_cont, m_sigma,
+		false, MODE_LIGHT, m_low, m_high, m_width, m_height);
+	t[4] = clock();
+	int i, j, points;
+
+	for (i = 0; i < num_cont; i++){
+		cont = contours[i];
+		points = cont->num;
+		for (j = 0; j < points; j++){
+			int y = int(cont->row[j] + 0.5);
+			int x = (int)(cont->col[j] + 0.5);
+			out.at<float>(y, x) = 1.0;
+		}
+	}
+	t[5] = clock();
+	free(p2);
+	free(p1);
+	free(n2);
+	free(n1);
+	free(ev);
+	free(ismax);
+
+	for (i = 4; i >= 0; i--)
+		free(k[i]);
+
+	for (i = 0; i < 5; i++)
+	{
+		printf("t%d---t%d : %f\n", i, i + 1, double(t[i + 1] - t[i]) / CLOCKS_PER_SEC);
+	}
+	
+	return true;
+}
+
+bool Steger::detect(cv::Mat src, std::vector<cv::Point2f>& points)
+{
+	if (src.empty())return false;
+	if (m_width != src.cols || m_height != src.rows)
+	{
+		if (m_image) {
+			cudaFree(m_image);
+		}
+		if (m_mid_data) {
+			cudaFree(m_mid_data);
+		}
+		for (int i = 0; i < 5; i++)
+		{
+			if (m_k[i])
+				cudaFree(m_k[i]);
+			cudaMalloc((void**)&m_k[i], src.cols*src.rows * sizeof(float));
+		}
+		cudaMalloc((void**)&m_mid_data, src.cols*src.rows * sizeof(float));
+		cudaMalloc((void**)&m_image, src.cols*src.rows * sizeof(float));
+		m_width = src.cols;
+		m_height = src.rows;
+	}
+
+	clock_t t[6];
+	t[0] = clock();
+	cudaMemcpy(m_image, src.data, m_width*m_height * sizeof(float), cudaMemcpyHostToDevice);
+
+
+	run_gpu();
+	unsigned char    *ismax;
+	float  *ev, *n1, *n2, *p1, *p2;
+
+	ismax = (unsigned char *)malloc(m_width*m_height * sizeof(unsigned char));
+	ev = (float*)malloc(m_width*m_height * sizeof(float));
+	n1 = (float*)malloc(m_width*m_height * sizeof(float));
+	n2 = (float*)malloc(m_width*m_height * sizeof(float));
+	p1 = (float*)malloc(m_width*m_height * sizeof(float));
+	p2 = (float*)malloc(m_width*m_height * sizeof(float));
+	memset(ismax, 0, m_width*m_height * sizeof(unsigned char));
+	memset(ev, 0, m_width*m_height * sizeof(float));
+	t[1] = clock();
+	contour **contours, *cont;
+	long   num_cont;
+
+	float* k[5];
+	for (int i = 0; i < 5; i++)
+	{
+		k[i] = (float*)malloc(m_width*m_height * sizeof(float));
+		cudaMemcpy(k[i], m_k[i], m_width*m_height * sizeof(float), cudaMemcpyDeviceToHost);
+	}
+	t[2] = clock();
+	compute_line_points(k, ismax, ev, n1, n2, p1, p2, m_width, m_height, m_low, m_high, MODE_LIGHT);
+	t[3] = clock();
+	compute_contours(ismax, ev, n1, n2, p1, p2, k[0], k[1], &contours, &num_cont, m_sigma,
+		false, MODE_LIGHT, m_low, m_high, m_width, m_height);
+	t[4] = clock();
+	int i, j, point_num;
+
+	points.clear();
+	for (i = 0; i < num_cont; i++) {
+		cont = contours[i];
+		point_num = cont->num;
+		for (j = 0; j < point_num; j++) {
+			cv::Point2f point(cont->col[j], cont->row[j]);
+			points.push_back(point);
+		}
+	}
+	t[5] = clock();
+	free(p2);
+	free(p1);
+	free(n2);
+	free(n1);
+	free(ev);
+	free(ismax);
+
+	for (i = 4; i >= 0; i--)
+		free(k[i]);
+
+	for (i = 0; i < 5; i++)
+	{
+		printf("t%d---t%d : %f\n", i, i + 1, double(t[i + 1] - t[i]) / CLOCKS_PER_SEC);
+	}
+
+	return true;
+}
+
+float* Steger::compute_gauss_mask_0(long* num,double sigma)
+{
+	long   i, n;
+	float limit;
+	float *h, *mask,*h_c;
+	
+	limit = MASK_SIZE(MAX_SIZE_MASK_0, sigma); /* Error < 0.001 on each side */
+	n = (long)limit;
+	cudaMalloc((void**)&h, (2 * n + 1)*sizeof(float));
+	h_c =(float*)malloc((2*n+1)*sizeof(float));
+	mask = h_c + n;
+	for (i = -n + 1; i <= n - 1; i++)
+		mask[i] = phi0(-i + 0.5, sigma) - phi0(-i - 0.5, sigma);
+	mask[-n] = 1.0 - phi0(n - 0.5, sigma);
+	mask[n] = phi0(-n + 0.5, sigma);
+	cudaMemcpy(h, h_c, (2 * n + 1)*sizeof(float), cudaMemcpyHostToDevice);
+	free(h_c);
+	*num = n;
+
+	return h;
+}
+
+float* Steger::compute_gauss_mask_1(long   *num, double sigma)
+{
+	long   i, n;
+	float limit;
+	float *h, *mask,*h_c;
+
+	limit = MASK_SIZE(MAX_SIZE_MASK_1, sigma); /* Error < 0.001 on each side */
+	n = (long)limit;
+	cudaMalloc((void**)&h, (2 * n + 1)*sizeof(float));
+	h_c = (float*)malloc((2 * n + 1)*sizeof(float));
+	mask = h_c + n;
+	for (i = -n + 1; i <= n - 1; i++)
+		mask[i] = phi1(-i + 0.5, sigma) - phi1(-i - 0.5, sigma);
+	mask[-n] = -phi1(n - 0.5, sigma);
+	mask[n] = phi1(-n + 0.5, sigma);
+	cudaMemcpy(h, h_c, (2 * n + 1)*sizeof(float), cudaMemcpyHostToDevice);
+	free(h_c);
+	*num = n;
+	return h;
+}
+
+/* Second derivative of Gaussian smoothing mask */
+float* Steger::compute_gauss_mask_2(long   *num, double sigma)
+{
+	long   i, n;
+	float limit;
+	float *h, *mask,*h_c;
+
+	limit = MASK_SIZE(MAX_SIZE_MASK_2, sigma); /* Error < 0.001 on each side */
+	n = (long)limit;
+	cudaMalloc((void**)&h, (2 * n + 1)*sizeof(float));
+	h_c = (float*)malloc((2 * n + 1)*sizeof(float));
+	mask = h_c + n;
+	for (i = -n + 1; i <= n - 1; i++)
+		mask[i] = phi2(-i + 0.5, sigma) - phi2(-i - 0.5, sigma);
+	mask[-n] = -phi2(n - 0.5, sigma);
+	mask[n] = phi2(-n + 0.5, sigma);
+	cudaMemcpy(h, h_c, (2 * n + 1)*sizeof(float), cudaMemcpyHostToDevice);
+	*num = n;
+	return h;
+}
+
+
+float Steger::phi0(float x, float sigma)
+{
+	return normal(x / sigma);
+}
+
+
+
+/* The Gaussian function */
+float Steger::phi1(float x, float sigma)
+{
+	float t;
+
+	t = x / sigma;
+	float ret = SQRT_2_PI_INV / sigma*exp(-0.5*t*t);
+	return ret;
+}
+
+
+
+/* First derivative of the Gaussian function */
+float Steger::phi2(float x, float sigma)
+{
+	float t;
+
+	t = x / sigma;
+	float ret = -x*SQRT_2_PI_INV / pow(float(sigma), float(3.0))*exp(-0.5*t*t);
+	return ret;
+}
+
+
+float Steger::normal(float x)
+{
+	int    sn;
+	float R1, R2, y, y2, y3, y4, y5, y6, y7;
+	float erf, erfc, z, z2, z3, z4;
+	float phi;
+
+	if (x < -UPPERLIMIT) return 0.0;
+	if (x > UPPERLIMIT) return 1.0;
+
+	y = x / SQRT2;
+	if (y < 0) {
+		y = -y;
+		sn = -1;
+	}
+	else
+		sn = 1;
+
+	y2 = y * y;
+	y4 = y2 * y2;
+	y6 = y4 * y2;
+
+	if (y < 0.46875) {
+		R1 = P10 + P11 * y2 + P12 * y4 + P13 * y6;
+		R2 = Q10 + Q11 * y2 + Q12 * y4 + Q13 * y6;
+		erf = y * R1 / R2;
+		if (sn == 1)
+			phi = 0.5 + 0.5*erf;
+		else
+			phi = 0.5 - 0.5*erf;
+	}
+	else if (y < 4.0) {
+		y3 = y2 * y;
+		y5 = y4 * y;
+		y7 = y6 * y;
+		R1 = P20 + P21 * y + P22 * y2 + P23 * y3 +
+			P24 * y4 + P25 * y5 + P26 * y6 + P27 * y7;
+		R2 = Q20 + Q21 * y + Q22 * y2 + Q23 * y3 +
+			Q24 * y4 + Q25 * y5 + Q26 * y6 + Q27 * y7;
+		erfc = exp(-y2) * R1 / R2;
+		if (sn == 1)
+			phi = 1.0 - 0.5*erfc;
+		else
+			phi = 0.5*erfc;
+	}
+	else {
+		z = y4;
+		z2 = z * z;
+		z3 = z2 * z;
+		z4 = z2 * z2;
+		R1 = P30 + P31 * z + P32 * z2 + P33 * z3 + P34 * z4;
+		R2 = Q30 + Q31 * z + Q32 * z2 + Q33 * z3 + Q34 * z4;
+		erfc = (exp(-y2) / y) * (1.0 / SQRTPI + R1 / (R2 * y2));
+		if (sn == 1)
+			phi = 1.0 - 0.5*erfc;
+		else
+			phi = 0.5*erfc;
+	}
+
+	return phi;
+}

cuda_test/src/Steger.cu

@@ -0,0 +1,105 @@
+
+#include "Steger.hpp"
+#include "time.h"
+#include <stdio.h>
+#define BLOCK_SIZE		32768
+#define THREAD_SIZE		512
+
+__global__ void kenel_convolve_rows_gauss(float* image, float* mask, long n, float* h, long width, long height)
+{
+	long N = width*height;
+	const int bid = blockIdx.x;
+	const int tid = threadIdx.x;
+	for (int i = bid*THREAD_SIZE + tid; i < N; i += BLOCK_SIZE*THREAD_SIZE)
+	{
+		if (i == 0)
+			printf("in-----------------------\n");
+		int r = i / width;
+		int c = i % width;
+		///inner
+		if (r >= n&&r < height - n)
+		{
+			float sum = 0.0;
+			for (int j = -n; j <= n; j++)
+				sum += (float)(image[i + j*width])*mask[j+n];
+			h[i] = sum;
+		}
+		else
+		{
+			float sum = 0.0;
+			for (int j = -n; j <= n; j++)
+				sum += (float)(image[LINCOOR(BR(r + j), c, width)])*mask[j+n];
+			h[i] = sum;
+		}
+	}
+}
+
+
+__global__ void kenel_convolve_cols_gauss(float *h,
+	float  *mask,
+	long    n,
+	float   *k,
+	long    width,
+	long    height)
+{
+	long N = width*height;
+	const int bid = blockIdx.x;
+	const int tid = threadIdx.x;
+
+	for (int i = bid*THREAD_SIZE + tid; i < N; i += BLOCK_SIZE*THREAD_SIZE)
+	{
+		if (i == 0)
+			printf("cols--------------\n");
+		int r = i / width;
+		int c = i % width;
+		int j = 0;
+		float sum = 0.0;
+		if (c>=n&&c < width - n)
+		{
+			sum = 0.0;
+			for (j = -n; j <= n; j++)
+				sum += h[i + j] * mask[j+n];
+			k[i] = sum;
+		}
+		if (c < n)
+		{
+			sum = 0.0;
+			for (j = -n; j <= n; j++)
+				sum += h[LINCOOR(r, BC(c + j), width)] * mask[j+n];
+			k[i] = sum;
+		}
+		if (c >= width - n&&c < width)
+		{
+			sum = 0.0;
+			for (j = -n; j <= n; j++)
+				sum += h[LINCOOR(r, BC(c + j), width)] * mask[j+n];
+			k[i] = sum;
+		}
+		
+	}
+}
+
+void Steger::run_gpu()
+{
+	
+	cudaMemset(m_mid_data, 0, m_width*m_height*sizeof(float));
+	kenel_convolve_rows_gauss <<<BLOCK_SIZE, THREAD_SIZE, 0 >>>(m_image, m_mask_1_r, m_l_1r, m_mid_data, m_width, m_height);
+	kenel_convolve_cols_gauss <<<BLOCK_SIZE, THREAD_SIZE, 0 >>>(m_mid_data, m_mask_0_c, m_l_0c, m_k[0], m_width, m_height);
+
+	cudaMemset(m_mid_data, 0, m_width*m_height*sizeof(float));
+	kenel_convolve_rows_gauss <<<BLOCK_SIZE, THREAD_SIZE, 0 >>>(m_image, m_mask_0_r, m_l_0r, m_mid_data, m_width, m_height);
+	kenel_convolve_cols_gauss <<<BLOCK_SIZE, THREAD_SIZE, 0 >>>(m_mid_data, m_mask_1_c, m_l_1c, m_k[1], m_width, m_height);
+
+	cudaMemset(m_mid_data, 0, m_width*m_height*sizeof(float));
+	kenel_convolve_rows_gauss <<<BLOCK_SIZE, THREAD_SIZE, 0 >>>(m_image, m_mask_2_r, m_l_2r, m_mid_data, m_width, m_height);
+	kenel_convolve_cols_gauss <<<BLOCK_SIZE, THREAD_SIZE, 0 >>>(m_mid_data, m_mask_0_c, m_l_0c, m_k[2], m_width, m_height);
+
+	cudaMemset(m_mid_data, 0, m_width*m_height*sizeof(float));
+	kenel_convolve_rows_gauss <<<BLOCK_SIZE, THREAD_SIZE, 0 >>>(m_image, m_mask_1_r, m_l_1r, m_mid_data, m_width, m_height);
+	kenel_convolve_cols_gauss <<<BLOCK_SIZE, THREAD_SIZE, 0 >>>(m_mid_data, m_mask_1_c, m_l_1c, m_k[3], m_width, m_height);
+
+	cudaMemset(m_mid_data, 0, m_width*m_height*sizeof(float));
+	kenel_convolve_rows_gauss <<<BLOCK_SIZE, THREAD_SIZE, 0 >>>(m_image, m_mask_0_r, m_l_0r, m_mid_data, m_width, m_height);
+	kenel_convolve_cols_gauss <<<BLOCK_SIZE, THREAD_SIZE, 0 >>>(m_mid_data, m_mask_2_c, m_l_2c, m_k[4], m_width, m_height);
+
+}

cuda_test/src/Steger.hpp

@@ -0,0 +1,106 @@
+#ifndef ___STEGER_HPP__
+#define ___STEGER_HPP__
+#include <math.h>
+#define MAX_SIZE_MASK_0  3.09023230616781    /* Size for Gaussian mask */
+#define MAX_SIZE_MASK_1  3.46087178201605    /* Size for 1st derivative mask */
+#define MAX_SIZE_MASK_2  3.82922419517181    /* Size for 2nd derivative mask */
+
+#define MASK_SIZE(MAX,sigma) ceil(MAX*sigma) /* Maximum mask index */
+#include "opencv/highgui.h"
+#include "opencv2/opencv.hpp"
+
+
+#define LINCOOR(row,col,width) (long)((row)*(width)+(col))
+
+#define BR(row) ((row) < 0 ? -(row) : \
+                 (row) >= height ? height - (row) + height - 2 : (row))
+
+/* Mirror the column coordinate at the borders of the image; width must be a
+defined variable in the calling function containing the image width. */
+#define BC(col) ((col) < 0 ? -(col) : \
+                 (col) >= width ? width - (col) + width - 2 : (col))
+
+/* Absolute value of x */
+#ifndef ABS
+#define ABS(x) ((x) >= 0 ? (x) : -(x))
+#endif
+
+/* Sign of x */
+#ifndef SGN
+#define SGN(x) ((x) == 0 ? 0 : ((x) > 0 ? 1 : -1))
+#endif
+
+#define MODE_LIGHT 1  /* Extract bright lines */
+#define MODE_DARK  2  /* Extract dark lines */
+#define PIXEL_BOUNDARY 0.6
+
+typedef enum {
+	cont_no_junc,    /* no end point is a junction */
+	cont_start_junc, /* only the start point of the line is a junction */
+	cont_end_junc,   /* only the end point of the line is a junction */
+	cont_both_junc,  /* both end points of the line are junctions */
+	cont_closed      /* the contour is closed */
+} contour_class;
+
+typedef struct {
+	long  num;                /* number of points */
+	float *row;               /* row coordinates of the line points */
+	float *col;               /* column coordinates of the line points */
+	float *angle;             /* angle of normal (measured from the row axis) */
+	float *response;          /* response of line point (second derivative) */
+	float *width_l;           /* width to the left of the line */
+	float *width_r;           /* width to the right of the line */
+	float *asymmetry;         /* asymmetry of the line point */
+	float *contrast;          /* contrast of the line point */
+	contour_class cont_class; /* contour class (e.g., closed, no_junc) */
+} contour;
+
+
+class Steger
+{
+private:
+	Steger();
+public:
+	Steger(double sigma,double low,double high);
+	~Steger();
+	bool detect(cv::Mat src, cv::Mat& out);
+	bool detect(cv::Mat src, std::vector<cv::Point2f>& points);
+protected:
+	void   run_gpu();
+	float* compute_gauss_mask_0(long* num,double sigma);
+	float* compute_gauss_mask_1(long   *num, double sigma);
+	float* compute_gauss_mask_2(long   *num, double sigma);
+	float  phi0(float x, float sigma);
+	float  phi1(float x, float sigma);
+	float  phi2(float x, float sigma);
+	float normal(float x);
+private:
+	double		m_sigma;
+	double		m_low;
+	double		m_high;
+
+	float*		m_k[5];
+
+	float*		m_mask_0_r;
+	float*		m_mask_0_c;
+	float*		m_mask_1_r;
+	float*		m_mask_1_c;
+	float*		m_mask_2_r;
+	float*		m_mask_2_c;
+	
+	long		m_l_0r;
+	long		m_l_0c;
+	long		m_l_1r;
+	long		m_l_1c;
+	long		m_l_2r;
+	long		m_l_2c;
+
+
+	float*		m_image;
+	float*		m_mid_data;
+	int			m_width;
+	int			m_height;
+};
+
+
+#endif

cuda_test/src/detectlines.h

@@ -0,0 +1,82 @@
+#ifndef  DETECT___LINES_H__
+#define  DETECT___LINES_H__
+#include "Steger.hpp"
+typedef struct {
+	long  cont1; /* Index of line that is already processed */
+	long  cont2; /* Index of line tnat runs into cont1 */
+	long  pos;   /* Index of the junction point in cont1 */
+	float x;     /* x-(row-)coordinate of the junction point */
+	float y;     /* y-(col-)coordinate of the junction point */
+} junction;
+
+typedef struct {
+	short  x;
+	short  y;
+	double value;
+	bool   done;
+} crossref;
+
+typedef struct {
+	long x;
+	long y;
+} offset;
+
+typedef struct {
+	short r;   /* row coordinate of the chord */
+	short cb;  /* column coordinate of the start of the chord */
+	short ce;  /* column coordinate of the end of the chord */
+} chord;
+
+/* Run-length encoded region of an image.  This type is returned by the
+threshold() function.  It provides the means to efficiently link line points
+into lines. */
+typedef struct {
+	long  num;      /* number of chords */
+	chord *rl;      /* array of chords */
+} region;
+
+#define INITIAL_SIZE   100  /* Initial size of dynamically realloc'ed arrays */
+#define REALLOC_FACTOR 2   
+#define FALSE false
+#define TRUE	true
+
+#define PI 3.14159265
+
+#define MAX_ANGLE_DIFFERENCE (PI/6.0)
+
+/* Maximum length by which a line is possibly extended in order to find a
+junction with another line. */
+#define MAX_LINE_EXTENSION (2.5*sigma)
+
+void *xmalloc(size_t size);
+
+void *xrealloc(
+	void   *old_ptr,
+	size_t size);
+
+void threshold(
+	unsigned char   *image,
+	long   min,
+	long   width,
+	long   height,
+	region *out);
+void compute_contours(
+	unsigned char    *ismax,
+	float   *eigval,
+	float   *normx,
+	float   *normy,
+	float   *posx,
+	float   *posy,
+	float   *gradx,
+	float   *grady,
+	contour ***result,
+	long    *num_result,
+	double  sigma,
+	bool    extend_lines,
+	int     mode,
+	double  low,
+	double  high,
+	long    width,
+	long    height);
+
+#endif

cuda_test/src/main.cpp

@@ -0,0 +1,98 @@
+#include "Steger.hpp"
+#include <cuda.h>
+#include <cuda_runtime.h>
+#include <cmath>
+#include <pcl/common/common.h>
+#include <pcl/filters//voxel_grid.h>
+//#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+
+
+
+bool init_cuda()
+{
+	static bool bInit = false;
+	if (bInit)
+		return true;
+	int count;
+
+	cudaGetDeviceCount(&count);
+	if (count == 0) {
+		fprintf(stderr, "There is no device./n");
+		return false;
+	}
+
+	int i;
+	for (i = 0; i < count; i++) {
+		cudaDeviceProp prop;
+		if (cudaGetDeviceProperties(&prop, i) == cudaSuccess) {
+			if (prop.major >= 1) {
+				break;
+			}
+		}
+	}
+
+	if (i == count) {
+		fprintf(stderr, "There is no device supporting CUDA 1.x./n");
+		return false;
+	}
+
+	cudaSetDevice(i);
+	bInit = true;
+	printf("CUDA Init success...........\n");
+	return true;
+}
+
+int main()
+{
+	if (!init_cuda())
+	{
+		return 0;
+	}
+	
+	cv::Mat src = cv::imread("D:\\code\\steger\\map.bmp", 0);
+	cv::Mat image=src.clone();
+	for (int i = 0; i < image.rows; i++)
+	{
+		for (int j = 0; j < image.cols; j++)
+		{
+			if (image.at<uchar>(i, j) < 10)
+				image.at<uchar>(i, j) = 0;
+		}
+	}
+
+
+	cv::Mat fimage, fout;
+	image.convertTo(fimage, CV_32FC1);
+	fout = cv::Mat::zeros(fimage.size(), CV_32FC1);
+	Steger steger(3.1, 0.5, 3.5);
+	// LARGE_INTEGER freq, t0, t1;
+	// QueryPerformanceFrequency(&freq);
+
+	// QueryPerformanceCounter(&t0);
+	std::vector<cv::Point2f> points;
+	steger.detect(fimage,points);
+
+	// QueryPerformanceCounter(&t1);
+
+	// printf("time=%lf\n", (((t1.QuadPart - t0.QuadPart) * 1000.0) / freq.QuadPart));
+	float resolution = 0.01;
+	pcl::PointCloud<pcl::PointXYZ> cloud;
+	for (int i = 0; i < points.size(); ++i)
+	{
+		pcl::PointXYZ point((points[i].x-fimage.cols/2)*resolution, 
+			(points[i].y-fimage.rows/2)*resolution, 0.0);
+		cloud.push_back(point);
+	}
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_upground_downsample(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
+	vgfilter.setInputCloud(cloud.makeShared());
+	vgfilter.setLeafSize(0.01f, 0.01f, 0.01f);
+	vgfilter.filter(*cloud_upground_downsample);
+
+	pcl::PCDWriter writer;
+	writer.write("./map.pcd", *cloud_upground_downsample);
+
+	getchar();
+}

cv_test/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
 )
 find_package(OpenCV REQUIRED)
+# MESSAGE( STATUS " ${OpenCV_DIR} ${OpenCV_INCLUDE_DIRS} ${OpenCV_LIBS}")
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 
@@ -137,6 +138,16 @@ target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${OpenCV_LIBS})
 
 add_executable(${PROJECT_NAME}_improc src/cv_improc.cpp)
 target_link_libraries(${PROJECT_NAME}_improc ${catkin_LIBRARIES} ${OpenCV_LIBS})
+
+add_executable(${PROJECT_NAME}_camera src/cv_camera.cpp)
+target_link_libraries(${PROJECT_NAME}_camera ${catkin_LIBRARIES} ${OpenCV_LIBS})
+
+add_executable(${PROJECT_NAME}_gui src/cv_gui.cpp)
+target_link_libraries(${PROJECT_NAME}_gui ${catkin_LIBRARIES} ${OpenCV_LIBS})
+
+add_executable(${PROJECT_NAME}_test src/cv_test.cpp)
+target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES} ${OpenCV_LIBS})
+
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use

cv_test/src/cv_camera.cpp

@@ -0,0 +1,134 @@
+#include <stdio.h>
+#include "opencv2/opencv.hpp"
+
+bool initCamera(cv::String cameraID, cv::VideoCapture &cap)
+{
+    // create handle and open camera
+    cv::VideoCapture camera_cap(cv::CAP_DSHOW);
+    cap = camera_cap;
+    cap.open(cameraID);
+    if (!cap.isOpened())
+    {
+        std::cerr << "Couldn't open capture." << std::endl;
+        return false;
+    }
+
+    // set params
+    // cap.set(cv::CAP_PROP_CONVERT_RGB,1);
+    cap.set(cv::CAP_PROP_FOURCC, CV_FOURCC('M', 'J', 'P', 'G'));
+    cap.set(cv::CAP_PROP_FRAME_WIDTH, 1280);
+    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 720);
+    std::cout << "props: \nwidth, height: " << cap.get(cv::CAP_PROP_FRAME_WIDTH) << " " << cap.get(cv::CAP_PROP_FRAME_HEIGHT)
+              << "\nfps: " << cap.get(cv::CAP_PROP_FPS) << std::endl;
+    return true;
+}
+
+void dispVideo(cv::VideoCapture &cap, bool disp_props = false, std::string img_addr = "./src/cv_test/capture.jpg")
+{
+    if (cap.isOpened())
+    {
+        cv::Mat frame;
+        for (;;)
+        {
+            // capture image and display
+            // cap.read(frame);
+            cap >> frame;
+            if (frame.empty())
+                break;
+            cv::imshow("camera 1", frame);
+
+            // 
+            char c = cv::waitKey(20);
+            // std::cout<<(int)c<<std::endl;
+            if (c == 'q')
+            {
+                // std::cout<<cv::getBuildInformation()<<std::endl;
+                std::cout << "leave" << std::endl;
+                // cap.release(); // release 后需断开并重新连接硬件
+                break;
+            }
+            else if (c == 'c')
+            {
+                cv::imwrite(img_addr, frame);
+                std::cout << "image saved to" + img_addr << std::endl;
+            }
+            else if (disp_props)
+            {
+                std::cout << "props: \nwidth, height: " << cap.get(cv::CAP_PROP_FRAME_WIDTH) << " " << cap.get(cv::CAP_PROP_FRAME_HEIGHT)
+                          << "\nfps: " << cap.get(cv::CAP_PROP_FPS) << std::endl;
+            }
+        }
+    }
+}
+
+void imgProc(cv::Mat dif){
+     cv::Mat img_opened, img_closed, img_gradient, img_hat, img_blackhat;
+    cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5,5), cv::Point(1,1));
+    cv::morphologyEx(dif, img_opened, cv::MORPH_OPEN, element);
+    cv::morphologyEx(dif, img_closed, cv::MORPH_CLOSE, element);
+    cv::morphologyEx(dif, img_gradient, cv::MORPH_GRADIENT, element);
+    cv::morphologyEx(dif, img_hat, cv::MORPH_TOPHAT, element);
+    cv::morphologyEx(dif, img_blackhat, cv::MORPH_BLACKHAT, element);
+    cv::namedWindow("origin",cv::WINDOW_FREERATIO);
+    cv::namedWindow("open",cv::WINDOW_FREERATIO);
+    cv::namedWindow("close",cv::WINDOW_FREERATIO);
+    cv::namedWindow("gradient",cv::WINDOW_FREERATIO);
+    cv::namedWindow("hat",cv::WINDOW_FREERATIO);
+    cv::namedWindow("blackhat",cv::WINDOW_FREERATIO);
+    cv::imshow("origin", dif);
+    cv::imshow("open", img_opened);
+    cv::imshow("close", img_closed);
+    cv::imshow("gradient", img_gradient);
+    cv::imshow("hat", img_hat);
+    cv::imshow("blackhat", img_blackhat);
+    for (;;)
+    {
+        if (cv::waitKey(10) == 27)
+            break;
+    }
+    cv::destroyAllWindows();
+}
+
+int main(int argc, char **argv)
+{
+    // cv::String cameraID = "/dev/video1";
+    // cv::namedWindow("camera 1", cv::WINDOW_FREERATIO);
+    // cv::VideoCapture cap;
+    // if (initCamera(cameraID, cap))
+    // {
+    //    dispVideo(cap, true);
+    // }
+
+    cv::Mat src1 = cv::imread("./src/cv_test/src1.jpg");
+    cv::imshow("origin", src1);
+    cv::Mat src2 = cv::imread("./src/cv_test/src2.jpg");
+    cv::Mat sobel_x, sobel_y, sobel_scharr_x, sobel_scharr_y, manual, blend, blend_scharr;
+    // cv::Mat kernel = (cv::Mat_<char>(3, 3) << -1, 0, 1,
+    //                                         -2, 0, 2,
+    //                                         -1, 0, 1);
+    // cv::filter2D(src1, manual, src1.depth(), kernel);
+    // cv::imshow("manual",manual);
+    cv::Sobel(src1, sobel_x, src1.depth(),1,0);
+    cv::imshow("sobel_x",sobel_x);
+    cv::Sobel(src1, sobel_y, src1.depth(),0,1);
+    // cv::imshow("sobel_y",sobel_y);
+    cv::Sobel(src1, sobel_scharr_x, src1.depth(),1,0, CV_SCHARR);
+    // cv::imshow("sobel_scharr_x",sobel_scharr_x);
+    cv::Sobel(src1, sobel_scharr_y, src1.depth(),0,1, CV_SCHARR);
+    // cv::imshow("sobel_scharr_y",sobel_scharr_y);
+
+    cv::addWeighted(sobel_x, 0.5, sobel_y, 0.5, 0.0, blend);//cv::cvtColor(blend, blend, cv::COLOR_BGR2GRAY);
+    cv::addWeighted(sobel_scharr_x, 0.5, sobel_scharr_y, 0.5, 0.0, blend_scharr);//cv::cvtColor(blend_scharr, blend_scharr, cv::COLOR_BGR2GRAY);
+    cv::threshold(blend, blend, 60, 255, cv::ThresholdTypes::THRESH_BINARY );
+    cv::threshold(blend_scharr, blend_scharr, 60, 255, cv::ThresholdTypes::THRESH_BINARY );
+    cv::imshow("blend", blend);
+    cv::imshow("blend_scharr", blend_scharr);
+    cv::waitKey();
+    // cv::Mat diff2;
+    // cv::absdiff(src1, src2, diff2);
+    // imgProc(diff2);
+
+    // std::cout<<cv::getBuildInformation()<<std::endl;
+    // std::cout<<"leave"<<std::endl;
+    return 0;
+}

cv_test/src/cv_gui.cpp

@@ -0,0 +1,116 @@
+#include <opencv2/opencv.hpp>
+
+
+cv::Rect box;
+bool drawing_box = false;
+int g_switch_value = 10;
+void switch_off_function() { std::cout << "Pause "<<g_switch_value<<"\n"; }; //YOU COULD DO MORE
+void switch_on_function() { std::cout << "Run "<<g_switch_value<<"\n"; };
+
+// A little subroutine to draw a box onto an image
+//
+void draw_box(cv::Mat &img, cv::Rect box)
+{
+    cv::rectangle(
+        img,
+        box.tl(),
+        box.br(),
+        cv::Scalar(0x00, 0x00, 0xff)
+        /* red */
+    );
+}
+
+// This is our mouse callback. If the user
+// presses the left button, we start a box.
+// When the user releases that button, then we
+// add the box to the current image. When the
+// mouse is dragged (with the button down) we
+// resize the box.
+//
+void my_mouse_callback(
+    int event, int x, int y, int flags, void *param)
+{
+    cv::Mat image = *(cv::Mat *)param;
+    switch (event)
+    {
+    case cv::EVENT_MOUSEMOVE:
+    {
+        if (drawing_box)
+        {
+            box.width = x - box.x;
+            box.height = y - box.y;
+        }
+    }
+    break;
+        case cv::EVENT_LBUTTONDOWN:
+        {
+            drawing_box = true;
+            box = cv::Rect(x, y, 0, 0);
+        }
+        break;
+    case cv::EVENT_LBUTTONUP:
+    {
+        drawing_box = false;
+        if (box.width < 0)
+        {
+            box.x += box.width;
+            box.width *= -1;
+        }
+        if (box.height < 0)
+        {
+            box.y += box.height;
+            box.height *= -1;
+        }
+        draw_box(image, box);
+    }
+    break;
+    }
+}
+
+void switch_callback(int position, void *)
+{
+    if (position <= 5)
+    {
+        switch_off_function();
+    }
+    else
+    {
+        switch_on_function();
+    }
+}
+
+
+int main(int argc, char **argv)
+{
+    box = cv::Rect(-1, -1, 0, 0);
+    cv::Mat origin, image, temp;
+    origin = cv::imread("./src/cv_test/src1.jpg");
+    origin.copyTo(image);
+    image.copyTo(temp);
+    // image = cv::Scalar::all(0);
+    cv::namedWindow("Box Example");
+
+    cv::setMouseCallback(
+        "Box Example",
+        my_mouse_callback,
+        (void *)&image);
+    cv::createTrackbar(
+        "Switch",
+        "Box Example",
+        &g_switch_value,
+        20,
+        switch_callback);
+
+    for (;;)
+    {
+        image.copyTo(temp);
+        if (drawing_box)
+            draw_box(temp, box);
+        if(!g_switch_value)
+            origin.copyTo(image);
+        cv::imshow("Box Example", temp);
+        if (cv::waitKey(15) == 27)
+            break;
+    }
+    return 0;
+}

cv_test/src/cv_improc.cpp

@@ -29,11 +29,14 @@ int display_caption( const char* caption )
 
 int main(){
     src = cv::imread("./src/cv_test/Gray_Image.jpg");
+    src.convertTo(src, CV_8UC1);
     if (src.empty())
     {
         std::cout << "empty image" << std::endl;
         return -1;
     }
+    cv::namedWindow("origin image", cv::WINDOW_KEEPRATIO);
+    cv::imshow("origin image",src);
 
     { //smoothing
         // if (display_caption("Original Image") != 0)
@@ -96,9 +99,47 @@ int main(){
         // display_caption("Done!");
     }
 
-    {
-        
+    { //erosion(min) and dilation(max), createTrackBar(...)
+        // cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3), cv::Point(1,1));
+        // cv::Mat eroMat, dilMat, openMat, closeMat, gradiMat, tHatMat, bHatMat;
+        // cv::erode(src, eroMat, element);
+        // cv::dilate(src, dilMat, element);
+        // // cv::imshow("erosion",eroMat);
+        // // cv::imshow("dilation",dilMat);
+
+        // //open(erode->dilate, remove lights), close(dialate->erode, remove holes), Morphological Gradient(dilate-erode), tophat(src-open), blackhat(close-src)
+        // cv::morphologyEx(src, openMat, 2, element);
+        // cv::morphologyEx(src, closeMat, 3, element);
+        // cv::morphologyEx(src, gradiMat, 4, element);
+        // cv::morphologyEx(src, tHatMat, 5, element);
+        // cv::morphologyEx(src, bHatMat, 6, element);
+        // imshow("open",openMat);
+        // imshow("close",closeMat);
+        // imshow("morph gradient",gradiMat);
+        // imshow("top hat",tHatMat);
+        // imshow("black hat",bHatMat);
+
+
+    }
+
+    { //pyramid
+        // cv::Mat origin = src.clone();
+        // cv::pyrDown(origin, origin, cv::Size(origin.cols/2, origin.rows/2));
+        // cv::imshow("down sampling",origin);
+        // cv::pyrUp(origin, origin, cv::Size(origin.cols*2, origin.rows*2));
+        // cv::imshow("up sampling", origin);
+    }
+
+    { //threshold, binary(threshold), binary_inv, truncate(set max to threshold), tozero(delete those less than threshold), tozero_inv
+        // cv::Mat output1;
+        // cv::threshold(src, output1, 100, 255, cv::ThresholdTypes::THRESH_TOZERO_INV );
+        // cv::imshow("threshold",output1);
+    }
+
+    { //
+
     }
 
+    cv::waitKey();
     return 0;
 }

cv_test/src/cv_test.cpp

@@ -0,0 +1,38 @@
+#include <stdio.h>
+#include <opencv2/opencv.hpp>
+
+int main(int argc, char** argv )
+{
+    char * addr = "/home/youchen/Downloads/webwxgetmsg.jpg";
+    cv::String imageName(addr);
+    //创建Mat
+    cv::Mat image;
+    //读取图片
+    image = cv::imread(imageName, 1 );
+    //判断图片是否存在
+    if(!image.empty()){
+        // 三通道
+        assert(image.channels() == 3);
+        // 列反序
+        for (int j = 0; j < image.rows; j++)
+        {
+            for (int i = 0; i < image.cols / 2; i++)
+            {
+                cv::Vec3b left = image.at<cv::Vec3b>(j, i);
+                cv::Vec3b right = image.at<cv::Vec3b>(j, image.cols - i);
+                // cv::Vec3b temp = left;
+                // left[0] = right[0];
+                // left[1] = right[1];
+                // left[2] = right[2];
+                // right[0] = temp[0];
+                // right[1] = temp[1];
+                // right[2] = temp[2];
+                image.at<cv::Vec3b>(j, i) = right;
+                image.at<cv::Vec3b>(j, image.cols - i) = left;
+            }
+        }
+        cv::namedWindow("Display Image", cv::WINDOW_KEEPRATIO);
+        cv::imshow("Display Image", image);
+        cv::waitKey(0);
+    }
+}

fuse_wire_detection/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>fuse_wire_detection</name>
+  <version>0.0.0</version>
+  <description>The fuse_wire_detection package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="youchen@todo.todo">youchen</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/cpp_pkgs</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

fuse_wire_detection/src/FuseWireDetector.cpp

@@ -0,0 +1,23 @@
+#include "FuseWireDetector.h"
+
+int main()
+{
+	FuseWireDetector fwd;
+	//fwd.get_total_points_from_txt("points1Fuse.txt");
+
+	std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();
+
+	// fwd.my_process();
+	// fwd.dealWithTop("segDataPart3.txt");
+	for (size_t i = 1; i < 101; i++)
+	{
+		std::string filename = "clouds/filter/points"+boost::to_string(i)+".txt";
+		fwd.wholeProcess(filename);
+	}
+
+	std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
+	std::cout << " total seconds: " << std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count() / 1000.0 << std::endl;
+	std::cout << "finished" << std::endl;
+	// std::getchar();
+	return 0;
+}