Merge branch 'master' of https://gitlab.engr.illinois.edu/rmaksi2/ECE470

Lab5/lab56func.cpp

@@ -18,6 +18,15 @@ vector<int> *ptr_to_obj_labels;
 vector<int> *ptr_to_obj_centroids_x;
 vector<int> *ptr_to_obj_centroids_y;
 
+double O_r = 1.0;
+double O_c = 1.0;
+double beta = 784.57;
+double theta = 1.0;
+double hypotenuse = 1.0;
+double Tx = 337.0/beta;
+double Ty = 461.0/beta;
+double zw =  0.032;
+
 using namespace std;
 
 /*****************************************************
@@ -107,7 +116,8 @@ void ImageConverter::imageCb(const sensor_msgs::ImageConstPtr& msg)
     // convert to black and white img, then associate objects:
 
 	Mat bw_image;
-	adaptiveThreshold(gray_image,bw_image,255,0,0,151,5);
+	// adaptiveThreshold(gray_image,bw_image,255,0,0,151,5);
+    bw_image = thresholdImage(gray_image);
 	//adaptiveThreshold(scr,dst,MAXVALUE,adaptiveMethod,thresholdType,blocksize,C);
 	//adaptiveMethod = 0, ADAPTIVE_THRESH_MEAN_C
 	//thresholdType = 0, BINARY
@@ -171,6 +181,10 @@ Mat ImageConverter::associateObjects(Mat bw_img)
 	height = bw_img.rows;
 	width = bw_img.cols;
 	int num = 0;
+
+    O_r = 1/2*height;
+    O_c = 1/2*width;
+
 	// initialize an array of labels, assigning a label number to each pixel in the image
 	// this create a 2 dimensional array pixellabel[row][col]
 	int ** pixellabel = new int*[height];
@@ -281,7 +295,7 @@ Mat ImageConverter::associateObjects(Mat bw_img)
 
     // noise boundaries
     // the window should only display the blocks
-    int lower_boundary = 650;
+    int lower_boundary = 550;
     int upper_boundary = 1000;
     int difference = upper_boundary - lower_boundary;
 
@@ -417,6 +431,23 @@ Mat ImageConverter::associateObjects(Mat bw_img)
         obj_centroids_y.push_back(temp.second);
     }
 
+    // double base = 1.0;
+    // double hypotenuse = 1.0;
+
+    for (size_t i = 0; i < obj_centroids_y.size() && i < 2; i++) {
+        // if(i==0){
+            // base = obj_centroids_y[i];
+        // }
+        // else if(i == 1){
+        if(i == 1){
+            // base = abs(base - obj_centroids_y[i]);
+            hypotenuse = sqrt(pow(obj_centroids_x[1] - obj_centroids_x[0],2) + pow(obj_centroids_y[1] - obj_centroids_y[0],2));
+        }
+    }
+
+    // theta = acos(base/hypotenuse);
+
+
     // Vec3b color;
     // color[0] = 255;
     // color[1] = 255;
@@ -424,34 +455,34 @@ Mat ImageConverter::associateObjects(Mat bw_img)
     color[0] = 0;
     color[1] = 0;
     color[2] = 0;
-    std::cout << "There are "<< obj_labels.size() << "objects" << std::endl;
+    // std::cout << "There are "<< obj_labels.size() << "objects" << std::endl;
     for (int i = 0; i < obj_labels.size(); i++) {
-        std::cout << "Object " << i << ": " << '\n';
-        std::cout << "x centroid :" << obj_centroids_x[i] << '\n';
-        std::cout << "y centroid :" << obj_centroids_y[i] << '\n';
+        // std::cout << "Object " << i << ": " << '\n';
+        // std::cout << "x centroid :" << obj_centroids_x[i] << '\n';
+        // std::cout << "y centroid :" << obj_centroids_y[i] << '\n';
 
         for (int j = 0; j < 4; j++) {
             int x = obj_centroids_x[i];
             int y = obj_centroids_y[i];
 
             if(x+j < width && x+j>=0){
-                cout << "1" <<endl;
+                // cout << "1" <<endl;
                 associate_img.at<Vec3b>(Point(x+j,y)) = color;
             }
 
             if(x-j > 0 && x-j >=0){
-                cout << "2" <<endl;
+                // cout << "2" <<endl;
                 associate_img.at<Vec3b>(Point(x-j,y)) = color;
             }
 
             if(y+j < height && y+j >=0){
-                cout << "3" <<endl;
+                // cout << "3" <<endl;
                 associate_img.at<Vec3b>(Point(x,y+j)) = color;
             }
 
             if(y-j > 0 && y-j >=0){
-                cout << "4" <<endl;
-                cout << (y-j) <<endl;
+                // cout << "4" <<endl;
+                // cout << (y-j) <<endl;
                 associate_img.at<Vec3b>(Point(x,y-j)) = color;
             }
 
@@ -461,8 +492,8 @@ Mat ImageConverter::associateObjects(Mat bw_img)
             // associate_img.at<Vec3b>(Point(x,y)) = color;
 
     }
-    std::cout << "x: " << width << '\n';
-    std::cout << "y: " << height << '\n';
+    // std::cout << "x: " << width << '\n';
+    // std::cout << "y: " << height << '\n';
 
 
 
@@ -514,16 +545,20 @@ void ImageConverter::onClick(int event,int x, int y, int flags, void* userdata)
 	{
 		if (rightclickdone == 1) {  // if previous right click not finished ignore
 			rightclickdone = 0;  // starting code
-			ROS_INFO_STREAM("right click:  (" << x << ", " << y << ")");  //the point you clicked
-
-			// // put your right click code here
-            // //Store the parameters transfer btw Camera frame and world frame
-            // double r =
-            // double c =
-            // double x_c = (x - O_r)/beta
-            // double y_c =(c - o_c)/beta
-            // double x_w = x_c*cos(theta) -y_c*sin(theta) - Tx*cos(theta) + Ty*sin(theta)
-            // double y_w = x_c*sin(theta) + y_c*cos(theta) - Tx*sin(theta) -Ty*cos(theta)
+
+			// put your right click code here
+            //Store the parameters transfer btw Camera frame and world frame
+            double r = y;
+            double c = x;
+            double x_c = (O_r - x)/beta;
+            double y_c =(O_c - c)/beta;
+            theta = acos(y_c/hypotenuse);
+            double x_w = x_c*cos(theta) -y_c*sin(theta) - Tx*cos(theta) + Ty*sin(theta);
+            double y_w = x_c*sin(theta) + y_c*cos(theta) - Tx*sin(theta) -Ty*cos(theta);
+
+            ROS_INFO_STREAM("right click:  (" << x_w << ", " << y_w << ")");  //the point you clicked
+
+
 
 			rightclickdone = 1; // code finished
 		} else {