Commit f99d35b0 authored by rmaksi2's avatar rmaksi2
Browse files
parents d7105704 7f824ceb
......@@ -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 {
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment