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(); } }