void locator()
{
namedWindow("Tracking");
int hMin, hMax, sMin, sMax, vMin, vMax,area_min;
hMin = 0;
//hMax = 124; // night values/???
hMax = 255;
//sMin = 95;
sMin = 126;
sMax = 255;
//vMin = 139;
vMin = 173;
vMax = 255;
area_min = 100;
Mat smoothed, hsvImg, t_img;
createTrackbar("blob min area","Tracking" ,&area_min ,1000);
createTrackbar("Hue Min", "Tracking", &hMin, 255);
createTrackbar("Hue Max", "Tracking", &hMax, 255);
createTrackbar("Sat Min", "Tracking", &sMin, 255);
createTrackbar("Sat Max", "Tracking", &sMax, 255);
createTrackbar("Val Min", "Tracking", &vMin, 255);
createTrackbar("Val MaX", "Tracking", &vMax, 255);
while(ros::ok())
{
Mat source = imageB;
Mat copy = imageB.clone();
GaussianBlur(source, smoothed, Size(9,9), 4);
cvtColor(smoothed, hsvImg, CV_BGR2HSV);
inRange(hsvImg, Scalar(hMin, sMin, vMin), Scalar(hMax, sMax, vMax), t_img);
CBlobResult blob;
IplImage i_img = t_img;
blob = CBlobResult(&i_img,NULL,0);
int num_blobs = blob.GetNumBlobs();
blob.Filter(blob, B_INCLUDE, CBlobGetArea(), B_INSIDE, area_min, blob_area_absolute_max_);
num_blobs = blob.GetNumBlobs();
std::string reference_frame = "/virtual_table"; // Table frame at ball_radius above the actual table plane
tf::StampedTransform transform;
tf_.waitForTransform(reference_frame, model.tfFrame(), ros::Time(0), ros::Duration(0.5));
tf_.lookupTransform(reference_frame, model.tfFrame(), ros::Time(0), transform);
for(int i =0;i<num_blobs;i++)
{
CBlob* bl = blob.GetBlob(i);
Point2d uv(CBlobGetXCenter()(*bl), CBlobGetYCenter()(*bl));
//Use the width as the height
uv.y = bl->MinY() + (bl->MaxX() - bl->MinX()) * 0.5;
circle(copy,uv,50,Scalar(255,0,0),5);
cv::Point3d xyz;
model.projectPixelTo3dRay(uv, xyz);
// Intersect ray with plane in virtual table frame
//Origin of camera frame wrt virtual table frame
tf::Point P0 = transform.getOrigin();
//Point at end of unit ray wrt virtual table frame
tf::Point P1 = transform * tf::Point(xyz.x, xyz.y, xyz.z);
// Origin of virtual table frame
tf::Point V0 = tf::Point(0.0,0.0,0.0);
// normal to the table plane
tf::Vector3 n(0, 0, 1);
// finding scaling value
double scale = (n.dot(V0-P0))/(n.dot(P1-P0));
tf::Point ball_pos = P0 + (P1-P0)*scale;
cout <<ball_pos.x() << " " << ball_pos.y() << " " << ball_pos.z() <<endl;
}
imshow(WINDOW, copy);
waitKey(3);
imshow("edited", t_img);
waitKey(3);
ros::spinOnce();
}
}
