Polygon::Polygon(char* filename)
{
printf("Analyzing %s\n", filename);
//Temporary image pointer
Image* tempImage;
//OpenCV image matrix
Mat imageCV;
//read the file
imageCV = imread(filename,0);
//check for existance of image
if(!imageCV.dims)
{
//printf("Unable to load iamge for %s\n", filename);
image = NULL;
return;
}
//Convert to local class
image = new Image(imageCV);
//Generate Entropy image
printf("\tGenerate Entropy image ... \n");
entropyImage = ImageFeatures::Entropy(image, 9);
//Image processing class
ImageProcessing ip = ImageProcessing(entropyImage);
//Threshold and open the image
mask = ip.doMorphologicalOperation(MORPHO_OPEN,20);
//extract largest region
tempImage = ip.label(mask);
delete mask;
mask = tempImage;
tempImage = mask->zeropad();
delete mask;
mask = tempImage;
tempImage = image->zeropad();
delete image;
image = tempImage;
//refine the mask
printf("\tRefining the mask ... \n");
maskedImage = ip.RefineForeground(image, &mask);
//Approxiame image as mask
printf("\tApproximate image as mask ... \n");
Link* polygonCorners = ip.polyApprox(mask, 20);
//fill the vertices
Link* currentCorner = polygonCorners;
nVertices = 0;
while(currentCorner != NULL)
{
nVertices++;
currentCorner = currentCorner->next;
}
vertex = (Pair*)calloc(nVertices, sizeof(Pair));
currentCorner = polygonCorners;
for(int i = 0; i < nVertices; i++)
{
vertex[i].x = currentCorner->data->x;
vertex[i].y = currentCorner->data->y;
currentCorner = currentCorner->next;
}
angle = NULL;
edgeLength = NULL;
//compute geometric features
updateGeometry();
}
/**
@brief Main function of the camera node
@param argc
@param argv
@return int
*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "Point_Grey");
ros::NodeHandle n("~");
string node_ns = ros::this_node::getNamespace();
node_ns.erase(0, 2);
n.getParam("ballDiameter", BALL_DIAMETER);
cout << "Node namespace:" << node_ns << endl;
cout << "Ball diameter:" << BALL_DIAMETER << endl;
//read calibration paraneters
string a="/intrinsic_calibrations/ros_calib.yaml";
string path = ros::package::getPath("calibration_gui");
path += a;
FileStorage fs(path, FileStorage::READ);
if(!fs.isOpened())
{
cout<<"failed to open document"<<endl;
return -1;
}
fs["CM1"] >> CameraMatrix1;
fs["D1"] >> disCoeffs1;
std::cout << CameraMatrix1 << std::endl;
std::cout << disCoeffs1 << std::endl;
image_transport::ImageTransport it(n);
string raw_data_topic = "/" + node_ns;
string ballDetection_topic = raw_data_topic + "/BD_" + node_ns;
ballCentroidImage_pub = it.advertise(ballDetection_topic + "/BallDetection", 1);
ballCentroidCam_pub = n.advertise<geometry_msgs::PointStamped>( ballDetection_topic + "/SphereCentroid", 1);
ballCentroidCamPnP_pub = n.advertise<geometry_msgs::PointStamped>( ballDetection_topic + "/SphereCentroidPnP", 1);
CameraRaw cameraRaw(node_ns);
CreateTrackbarsAndWindows ();
ros::Rate loop_rate(15);
std::cout << "test" << std::endl;
while (ros::ok())
{
std::cout << "test2" << std::endl;
if(!cameraRaw.camImage.empty()) {
ImageProcessing(cameraRaw.camImage);
}
std::cout << "test3" << std::endl;
ros::spinOnce();
loop_rate.sleep();
}
//destroy the windows
destroyWindow("Camera");
destroyWindow("Binarized Image");
destroyWindow("Control");
destroyWindow("Circle");
//destroyWindow("Canny");
return 0;
}
