Commit 7f824ceb authored by muskula2's avatar muskula2
Browse files

TA's suggestions

parent 5f68ac75
......@@ -22,6 +22,7 @@ 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;
......@@ -294,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;
......@@ -430,20 +431,21 @@ Mat ImageConverter::associateObjects(Mat bw_img)
obj_centroids_y.push_back(temp.second);
}
double base = 1.0;
double hypotenuse = 1.0;
// 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){
base = abs(base - obj_centroids_y[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);
// theta = acos(base/hypotenuse);
// Vec3b color;
......@@ -543,17 +545,19 @@ 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 = y;
double c = x;
double x_c = (x - O_r)/beta;
double y_c =(c - O_c)/beta;
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
......
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