create_calibs.cpp
Go to the documentation of this file.
00001 
00007 #include <youbot_overhead_vision/create_calibs.h>
00008 
00009 using namespace std;
00010 
00011 createCalibs::createCalibs()
00012 {
00013   //These variable is red from a file.  IF the file is empty, zeroes will break the program.  As such, they are preset
00014   //As such, it is initialized to 1.
00015   blurringFactor = 1;
00016   calibAccuracy = 5;
00017 
00018   seq_num = 0;
00019   image1LoadedFlag = false;
00020   image2LoadedFlag = false;
00021   mouseEvent = false;
00022   savePointToBothCameraFlag = false;
00023   imagePaused = false;
00024   //These variable is red from a file.  IF the file is empty, zeroes will break the program.  As such, they are preset
00025   //As such, it is initialized to 1.  calibAccuracy = 5;
00026 
00027   //Initialized to colors calib (0:no calib shown, 2:black and white calib)
00028   calibToggled = 1;
00029   cameraShown = NORTH_CAMERA;
00030 
00031   //Initializes boundarybox being drawn to false
00032   roomBoundaryModeFlag = false;
00033   roomBoundaryPoints = *(new vector<cv::Point>());
00034 
00035   //Initializes the calibrated color to the floor.
00036   calibratingColor = FLOOR;
00037   //Initializes the zoom to false.
00038   zoom = false;
00039   escPressed = false;
00040 
00041   ros::NodeHandle nh;
00042 
00043   imgSub1 = nh.subscribe<sensor_msgs::Image>(OVERHEAD_NORTH, 0, &createCalibs::imageCallback1, this);
00044   imgSub2 = nh.subscribe<sensor_msgs::Image>(OVERHEAD_SOUTH, 0, &createCalibs::imageCallback2, this);
00045 
00046   //Gets the boolean marking which camera feed will be used from the param server.  Value is loaded from the youbot_overhead_vision_create_calibs.launch
00047   nh.getParam("/create_calibs/runFromSimulation", useSim);
00048   //Gets the file locations of the calibration files from the param server
00049 
00050   nh.getParam("/create_calibs/floor_cal", filenames[FLOOR]);
00051   nh.getParam("/create_calibs/robot_lb_file", filenames[ROBOT_LEFT_BACK]);
00052   nh.getParam("/create_calibs/robot_rb_file", filenames[ROBOT_RIGHT_BACK]);
00053   nh.getParam("/create_calibs/robot_lf_file", filenames[ROBOT_LEFT_FRONT]);
00054   nh.getParam("/create_calibs/robot_rf_file", filenames[ROBOT_RIGHT_FRONT]);
00055   nh.getParam("/create_calibs/room_boundary_map", boundaryImgFilename);
00056 
00057   //Initializing Tree creator/parser class.
00058   calTree = new calibTree();
00059 
00060   getCalibration(FLOOR, 255, 255, 255);
00061   getCalibration(ROBOT_RIGHT_BACK, 255, 0, 0);
00062   getCalibration(ROBOT_LEFT_BACK, 0, 255, 0);
00063   getCalibration(ROBOT_LEFT_FRONT, 0, 0, 255);
00064   getCalibration(ROBOT_RIGHT_FRONT, 0, 255, 255);
00065 
00066   ROS_INFO(
00067       "\n\t\t\t\tControls: \n 's': Save calib to files \t\t 'c': Change calibration color \n 'x': Remove most recent point  \t 'z': Zoom/unzoom\n 'b': Decrease Blur\t\t\t 'B': Increase blur\n 'a': Decrease pixel accuracy\t\t 'A': Decrease pixel accuracy\n '1': Switch to North Camera \t\t '2': Switch to South Camera\n 'd': To toggle room boundary box drawing mode.\n 'q': To switch between calib display types\n'SPACE': to pause the image \n 'Control + Click' : Add point to both camera\n'ESC': To quit \n");
00068 
00072   ros::Rate loop_rate(10);
00073   while (ros::ok() && !escPressed)
00074   {
00075     ros::spinOnce();
00076     loop_rate.sleep();
00077 
00078     if (image1LoadedFlag && image2LoadedFlag)
00079     {
00080       showImage();
00081     }
00082   }
00083 }
00084 
00085 createCalibs::~createCalibs()
00086 {
00087 
00088 }
00089 
00090 void createCalibs::showImage()
00091 {
00092   cv::namedWindow("Calibration Tool");
00093   cv_bridge::CvImagePtr * tempImg;
00094   if (cameraShown == NORTH_CAMERA)
00095     tempImg = (new cv_bridge::CvImagePtr(new cv_bridge::CvImage(*(raw_Img1))));
00096   else
00097     tempImg = (new cv_bridge::CvImagePtr(new cv_bridge::CvImage(*(raw_Img2))));
00098   output_Img1 = *tempImg;
00099   output_Img1->image = output_Img1->image.clone();
00100 
00101   //cv::Mat out1 = output_Img1->image;
00102   cv::Mat out1(output_Img1->image.rows / 2, output_Img1->image.cols / 2, output_Img1->image.type());
00103   cv::resize(output_Img1->image, out1, out1.size(), cv::INTER_NEAREST);
00104 
00105   //if(useSim && calibToggled > 0)
00106   //{
00107   //  applyCalib(&out1, BOTH_CAMERAS);
00108   /* }else*/if (calibToggled > 0)
00109   {
00110 
00111     if (cameraShown == NORTH_CAMERA)
00112       applyCalib(&out1, NORTH_CAMERA);
00113     else if (cameraShown == SOUTH_CAMERA)
00114       applyCalib(&out1, SOUTH_CAMERA);
00115 
00116   }
00117   uint8_t temp;
00118   //imshow assumes bgr.  As such, image is switch to match that.
00119   for (int i = 0; i < 3 * (out1.cols * out1.rows); i += 3)
00120   {
00121     temp = out1.data[i];
00122     out1.data[i] = out1.data[i + 2];
00123     out1.data[i + 2] = temp;
00124   }
00125 
00126   cv::Mat dest(out1.rows * 2, out1.cols * 2, out1.type());
00127 
00128   //If the zoom flag is true, the image has to be zoomed in.
00129   if (zoom)
00130   {
00131     // declare a destination Mat object with correct size and type
00132     cv::Mat roiOut1 = out1(
00133         cv::Rect(zoomQuadX * out1.cols / ZOOM_CONSTANT, zoomQuadY * out1.rows / ZOOM_CONSTANT,
00134                  out1.cols / ZOOM_CONSTANT, out1.rows / ZOOM_CONSTANT));
00135     //use cvResize to resize source to a destination image
00136     roiOut1.copyTo(out1);
00137     cv::resize(out1, dest, dest.size(), cv::INTER_NEAREST);
00138     imshow("Calibration Tool", dest);
00139   }
00140   else
00141   {
00142     cv::resize(out1, dest, dest.size(), cv::INTER_NEAREST);
00143     imshow("Calibration Tool", dest);
00144   }
00145   cv::setMouseCallback("Calibration Tool", myMouseCallback, (void *)&out1);
00146   char key = cv::waitKey(50);
00147   if (key > 0)
00148   {
00149     if (key == 32) //Pause the image if space is pressed
00150     {
00151       if (imagePaused)
00152         imagePaused = false;
00153       else
00154         imagePaused = true;
00155     }
00156     if (key == 115) //Save to file when 's' is pressed
00157     {
00158       ROS_INFO("Saving Files");
00159       saveCalibsToFiles();
00160     }
00161     else if (key == 100) //Go to room boundary drawing mode when 'd' is pressed
00162     {
00163       roomBoundaryModeFlag = !roomBoundaryModeFlag;
00164       if (roomBoundaryModeFlag)
00165         ROS_INFO("Room boundary mode: ON. Click points to define room boundaries");
00166       else
00167         ROS_INFO("Room boundary mode: OFF. Click points to add them to calibrations");
00168     }
00169     else if (key == 113) //Go through calibration display modes when 'q' is pressed
00170     {
00171       calibToggled = (calibToggled + 1) % 3;
00172     }
00173     else if (key == 49) //Switches to south camera when '1' is pressed
00174     {
00175       cameraShown = SOUTH_CAMERA;
00176     }
00177     else if (key == 50) //Switches to north camera when '2' is pressed
00178     {
00179       cameraShown = NORTH_CAMERA;
00180     }
00181     else if (key == 27) //Escapes when esc is pressed
00182     {
00183       escPressed = true;
00184     }
00185     else if (key == 97) //Decrease the pixel color accuracy required when 'a' is pressed
00186     {
00187       if (calibAccuracy > 1)
00188       {
00189         calibAccuracy--;
00190         ROS_INFO("Accuracy required reduced to %d", calibAccuracy);
00191       }
00192       else
00193       {
00194         ROS_INFO("Pixel accuracy required already at minimum");
00195       }
00196     }
00197     else if (key == 65) //Increase the pixel color accuracy required when 'A' is pressed
00198     {
00199       if (calibAccuracy < 40)
00200       {
00201         calibAccuracy++;
00202         ROS_INFO("Accuracy required increased to %d", calibAccuracy);
00203       }
00204       else
00205       {
00206         ROS_INFO("Pixel accuracy required already at maximum");
00207       }
00208     }
00209     else if (key == 98) //Decrease the blur used when 'b' is pressed
00210     {
00211       if (blurringFactor > 1)
00212       {
00213         blurringFactor--;
00214         ROS_INFO("Blur reduced to %d", blurringFactor);
00215       }
00216       else
00217       {
00218         ROS_INFO("Blur is already at minimum");
00219       }
00220     }
00221     else if (key == 66) //Increase the blur used when 'B' is pressed
00222     {
00223       if (blurringFactor < 25)
00224       {
00225         blurringFactor++;
00226         ROS_INFO("Blur increased to %d", blurringFactor);
00227       }
00228       else
00229       {
00230         ROS_INFO("Blur is already at maximum");
00231       }
00232     }
00233     else if (key == 99) //Change the image when 'c' is pressed
00234     {
00235       calibratingColor = (calibratingColor + 1) % 5;
00236       if (calibratingColor == 0)
00237         ROS_INFO("Current Calib: Robot Left Front Marker     Color: Blue ");
00238       if (calibratingColor == 1)
00239         ROS_INFO("Current Calib: Robot Right Front Marker    Color: Cyan");
00240       if (calibratingColor == 2)
00241         ROS_INFO("Current Calib: Robot Left Back Marker      Color: Green");
00242       if (calibratingColor == 3)
00243         ROS_INFO("Current Calib: Robot Right Back Marker     Color: Red");
00244       if (calibratingColor == 4)
00245         ROS_INFO("Current Calib: Floor                       Color: White");
00246     }
00247     else if (key == 120) //Delete the last added color when 'x' is pressed
00248     {
00249 
00250       if (roomBoundaryModeFlag)
00251       {
00252         if (roomBoundaryPoints.size() > 0)
00253         {
00254           ROS_INFO("Last room boundary point removed");
00255           roomBoundaryPoints.pop_back();
00256         }
00257         else
00258           ROS_INFO("No room boundary points left to remove");
00259       }
00260       else
00261       {
00262         bool success = calTree->removeLastNodeFromTree(cameraShown);
00263         if (success)
00264         {
00265           ROS_INFO("Removing last point.");
00266         }
00267         else
00268           ROS_INFO("No point left to remove ");
00269       }
00270     }
00271     else if (key == 122) //Zoom if 'z' is pressed.  If the image is already zoomed in,it will dezoom.
00272     {
00273       if (zoom)
00274       {
00275         zoom = false;
00276       }
00277       else
00278       {
00279         zoom = true;
00280         //Zoom does a x4 zoom and figures out what quadrant the mouse was in at zoom time.
00281 
00282         zoomQuadX = mouseX / (out1.cols / (ZOOM_CONSTANT) * SCREEN_RESIZE);
00283         zoomQuadY = mouseY / (out1.rows / (ZOOM_CONSTANT) * SCREEN_RESIZE);
00284       }
00285     }
00286   }
00287   //Processes mouse clicks.
00288   if (mouseEvent)
00289   {
00290     if (roomBoundaryModeFlag)
00291     {
00292       roomBoundaryPoints.push_back(*(new cv::Point()));
00293 
00294       ROS_INFO("x %d y %d", mouseX, mouseY);
00295       //Points values are divided by two because the image is compressed before application in image_calibration.cpp
00296       roomBoundaryPoints[roomBoundaryPoints.size() - 1].x = mouseX / 2;
00297       if (cameraShown == SOUTH_CAMERA && !useSim)
00298       {
00299         roomBoundaryPoints[roomBoundaryPoints.size() - 1].y = (mouseY + out1.rows - REAL_OVERLAP) / 2;
00300       }
00301       else if (cameraShown == SOUTH_CAMERA)
00302       {
00303         roomBoundaryPoints[roomBoundaryPoints.size() - 1].y = (mouseY + out1.rows - SIM_OVERLAP) / 2;
00304       }
00305       else
00306         roomBoundaryPoints[roomBoundaryPoints.size() - 1].y = mouseY / 2;
00307 
00308       ROS_INFO(
00309           "Boundary Point added.  x: %d, y: %d", roomBoundaryPoints[roomBoundaryPoints.size() -1].x, roomBoundaryPoints[roomBoundaryPoints.size() -1].y);
00310 
00311     }
00312     else if (calibToggled == 2) //If the image is in black and white, dont allow point clicks and give warning.
00313     //otherwise, a black point is added instead of the proper value.
00314     {
00315       ROS_INFO("Points cannot be picked in black and white mode");
00316 
00317     }
00318     else
00319     {
00320       ROS_INFO("Added Color r: %d g: %d b: %d", clickedColorR, clickedColorG, clickedColorB);
00321       if (useSim || savePointToBothCameraFlag)
00322         calTree->newCalibPoint(clickedColorR, clickedColorG, clickedColorB, BOTH_CAMERAS, calibratingColor);
00323       else
00324         calTree->newCalibPoint(clickedColorR, clickedColorG, clickedColorB, cameraShown, calibratingColor);
00325 
00326       savePointToBothCameraFlag = false;
00327       //Order Calib Points by red using insertion sort:
00328       calTree->orderCalibsByRed();
00329       calTree->freeTree();
00330       calTree->createTree();
00331     }
00332     mouseEvent = false;
00333   }
00334   delete (tempImg);
00335 
00336 }
00337 
00338 void myMouseCallback(int event, int x, int y, int flags, void *param)
00339 {
00340   int tempX, tempY;
00341   tempX = x / SCREEN_RESIZE;
00342   tempY = y / SCREEN_RESIZE;
00343 
00344   if (zoom)
00345   {
00346     tempY /= ZOOM_CONSTANT;
00347     tempX /= ZOOM_CONSTANT;
00348   }
00349 
00350   if (event == CV_EVENT_LBUTTONDOWN)
00351   {
00352     int index;
00353     //If the image is zoomed, the x and y values of the pixel have to be modified
00354 
00355     if (flags & 1 << 3)
00356     {
00357       savePointToBothCameraFlag = true;
00358     }
00359     index = tempY * ((cv::Mat *)param)->step + tempX * ((cv::Mat *)param)->channels();
00360     clickedColorB = ((cv::Mat *)param)->data[index];
00361     clickedColorG = ((cv::Mat *)param)->data[index + 1];
00362     clickedColorR = ((cv::Mat *)param)->data[index + 2];
00363 
00364     mouseEvent = true;
00365 
00366   }
00367 
00368   mouseX = tempX * SCREEN_RESIZE;
00369   ;
00370   mouseY = tempY * SCREEN_RESIZE;
00371   ;
00372 }
00373 
00374 void createCalibs::getCalibration(int calibType, int calibR, int calibG, int calibB)
00375 {
00376   string filename = filenames[calibType];
00377   colors[calibType][0] = calibR;
00378   colors[calibType][1] = calibG;
00379   colors[calibType][2] = calibB;
00380 
00381   ifstream calFile(filename.c_str());
00382   YAML::Parser parser(calFile);
00383   YAML::Node doc;
00384   parser.GetNextDocument(doc);
00385 
00386   if ((doc.size() > 0 && calibType != FLOOR) || doc.size() > 1)
00387   {
00388     int max = doc.size();
00389 
00390     //Getting Blurring factor and calibAccuracy parameters in the floor file
00391     if (calibType == FLOOR)
00392     {
00393       max -= 2;
00394       try
00395       {
00396         doc["blur"] >> blurringFactor;
00397         doc["calib_accuracy"] >> calibAccuracy;
00398       }
00399       catch (YAML::InvalidScalar &)
00400       {
00401         ROS_WARN("The floor file does not have a blurring or accuracy value.  Values are set to defaults");
00402       }
00403     }
00404     for (int i = 1; i <= max; i++)
00405     {
00406       string aPoint = "Point " + convertInt(i);
00407 
00408       int red, green, blue, camera;
00409       (*(doc.FindValue(aPoint.c_str()))->FindValue("rgb"))[0] >> red;
00410       (*(doc.FindValue(aPoint.c_str()))->FindValue("rgb"))[1] >> green;
00411       (*(doc.FindValue(aPoint.c_str()))->FindValue("rgb"))[2] >> blue;
00412       (*(doc.FindValue(aPoint.c_str()))->FindValue("camera")) >> camera;
00413       calTree->newCalibPoint((uint8_t)red, (uint8_t)green, (uint8_t)blue, camera, calibType);
00414 
00415     }
00416     calTree->orderCalibsByRed();
00417     calTree->createTree();
00418 
00419   }
00420   else if (calibType == FLOOR)
00421     ROS_INFO("Floor file did not have proper format. No blur/calibration variables found.");
00422   else
00423   {
00424     ROS_INFO("File not found");
00425   }
00426 }
00427 
00428 void createCalibs::imageCallback1(const sensor_msgs::ImageConstPtr& image_msg)
00429 {
00430   //If the image is paused, the raw image isnt updated.
00431   if (!imagePaused)
00432   {
00433     ros::Time time = image_msg->header.stamp;
00434 
00435     raw_Img1 = cv_bridge::toCvCopy(image_msg, ENCODING);
00436 
00437     // set the encoding
00438     raw_Img1->encoding = ENCODING;
00439     // set and update the sequence number
00440     raw_Img1->header.seq = seq_num++;
00441     // get the current timestamp
00442     raw_Img1->header.stamp = ros::Time::now();
00443     // set the frame ID
00444     raw_Img1->header.frame_id = FRAME_ID;
00445   }
00446   image1LoadedFlag = true;
00447 }
00448 
00449 void createCalibs::imageCallback2(const sensor_msgs::ImageConstPtr& image_msg)
00450 {
00451   //If the image is paused, the raw image isnt updated.
00452   if (!imagePaused)
00453   {
00454     ros::Time time = image_msg->header.stamp;
00455     raw_Img2 = cv_bridge::toCvCopy(image_msg, ENCODING);
00456 
00457     // set the encoding
00458     raw_Img2->encoding = ENCODING;
00459     // set and update the sequence number
00460     raw_Img2->header.seq = seq_num++;
00461     // get the current timestamp
00462     raw_Img2->header.stamp = ros::Time::now();
00463     // set the frame ID
00464     raw_Img2->header.frame_id = FRAME_ID;
00465   }
00466 
00467   image2LoadedFlag = true;
00468 
00469 }
00470 
00471 void createCalibs::applyCalib(cv::Mat * imagePtr, int camera)
00472 {
00473   cv::Mat boundaryImg = cv::imread(boundaryImgFilename);
00474   cv::Mat lrgBoundaryImg(boundaryImg.rows * 2, boundaryImg.cols * 2, boundaryImg.type());
00475   if (boundaryImg.data != NULL)
00476   {
00477     cv::resize(boundaryImg, lrgBoundaryImg, lrgBoundaryImg.size(), cv::INTER_NEAREST);
00478   }
00479   const cv::Mat& image = (*imagePtr);
00480 
00481   cv::blur(image, image, cv::Size(blurringFactor, blurringFactor), cv::Point(-1, -1), cv::BORDER_DEFAULT);
00482   long numBytes = image.step * image.rows;
00483   // applies calibration file on each pixel
00484   for (int index = 0; index < numBytes; index += 3)
00485   {
00486     //If any of the colors in the array is equal to NOT_SET, there is no calibration file for that type, and no calibration
00487     //Needs to be applied 
00488     int calibType = calTree->parseCalibTree((int)image.data[index], (int)image.data[index + 1],
00489                                             (int)image.data[index + 2], camera, calibAccuracy);
00490 
00491     //If the pixel is outside the bounded area, it is turned black.
00492     int fullImageIndex = index;
00493     if (useSim && camera == SOUTH_CAMERA)
00494       fullImageIndex = index + 3 * image.cols * (image.rows - SIM_OVERLAP);
00495     else if (camera == SOUTH_CAMERA)
00496       fullImageIndex = index + 3 * image.cols * (image.rows - REAL_OVERLAP);
00497 
00498     if (boundaryImg.data != NULL && lrgBoundaryImg.data[fullImageIndex] == 0
00499         && lrgBoundaryImg.data[fullImageIndex + 1] == 0 && lrgBoundaryImg.data[fullImageIndex + 2] == 0)
00500     {
00501       image.data[index] = (uint8_t)0;
00502       image.data[index + 1] = (uint8_t)0;
00503       image.data[index + 2] = (uint8_t)0;
00504     }
00505     else if (calibType >= ROBOT_LEFT_FRONT && calibType <= FLOOR)
00506     {
00507       image.data[index] = (uint8_t)(colors[calibType][0]);
00508       image.data[index + 1] = (uint8_t)(colors[calibType][1]);
00509       image.data[index + 2] = (uint8_t)(colors[calibType][2]);
00510     }
00511     else if (calibToggled == 2) //if black and white mode is on, pixels with unfound calibrations are turned black.
00512     {
00513       image.data[index] = (uint8_t)0;
00514       image.data[index + 1] = (uint8_t)0;
00515       image.data[index + 2] = (uint8_t)0;
00516     }
00517   }
00518 
00519   //The dilate +erode eliminates pixels that are by themselves.  dilate reduces black zones by 1 pixel in each direction, erode increases them back their original position.  Large obstacles remain unchanged, small obstacles disappear
00520   if (calibToggled == 2)
00521   {
00522     int erosion_size = 1;
00523     cv::Mat element = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(2 * erosion_size + 1, 2 * erosion_size + 1),
00524                                                 cv::Point(erosion_size, erosion_size));
00525     morphologyEx(image, image, cv::MORPH_CLOSE, element);
00526   }
00527 }
00528 
00529 void createCalibs::saveCalibsToFiles()
00530 {
00531   for (int fileType = 0; fileType < 5; fileType++)
00532   {
00533     ofstream calibFile;
00534     calibFile.open(filenames[fileType].c_str());
00535 
00536     //Keeps track of how many points were saved for each file.
00537     int pointIndex = 1;
00538     if (fileType == FLOOR)
00539     {
00540       //The floor file begin with the blur and calibration variables
00541       calibFile << "blur: " << blurringFactor << endl;
00542       calibFile << "calib_accuracy: " << calibAccuracy << endl;
00543     }
00544 
00545     for (int i = 0; i < calTree->getCalibPointsSize(); i++)
00546     {
00547       int calibType = calTree->getCalibPointType(i);
00548       //If the color is white, it means a point as clicked that was already calibrated.  That point should not be added.
00549       if ((calibType == fileType)
00550           && (calTree->getCalibPointColor(i, RED) != 255 || calTree->getCalibPointColor(i, GREEN) != 255
00551               || calTree->getCalibPointColor(i, BLUE) != 255 || calibratingColor != FLOOR))
00552       {
00553         calibFile << "Point " << (pointIndex) << ":" << endl;
00554         calibFile << "  rgb: [" << convertInt(calTree->getCalibPointColor(i, RED)) << ", "
00555             << convertInt(calTree->getCalibPointColor(i, GREEN)) << ", "
00556             << convertInt(calTree->getCalibPointColor(i, BLUE)) << "]" << endl;
00557         calibFile << "  camera: " << calTree->getCalibPointCamera(i) << endl;
00558         pointIndex++;
00559       }
00560     }
00561     //Close the file
00562     calibFile.close();
00563   }
00564 
00565   if (roomBoundaryPoints.size() > 2)
00566   {
00567     cv::Mat * boundImg;
00568     if (useSim)
00569       boundImg = new cv::Mat((output_Img1->image.rows - SIM_OVERLAP) / 2, output_Img1->image.cols / 4,
00570                              output_Img1->image.type());
00571     else
00572       boundImg = new cv::Mat((output_Img1->image.rows - REAL_OVERLAP) / 2, output_Img1->image.cols / 4,
00573                              output_Img1->image.type());
00574     cv::rectangle(*boundImg, cv::Point(0, 0), cv::Point(boundImg->cols, boundImg->rows), CV_RGB(0,0,0), CV_FILLED);
00575 
00576     const cv::Point * points[1];
00577     points[0] = &(roomBoundaryPoints[0]);
00578     int sizes[1];
00579     sizes[0] = roomBoundaryPoints.size();
00580 
00581     cv::fillPoly(*boundImg, points, sizes, 1, CV_RGB(255,255,255));
00582 
00583     imwrite(boundaryImgFilename, *boundImg);
00584   }
00585 }
00586 
00587 string createCalibs::convertInt(int number)
00588 {
00589   stringstream ss; //create a stringstream
00590   ss << number; //add number to the stream
00591   return ss.str(); //return a string with the contents of the stream
00592 }
00593 
00594 int main(int argc, char** argv)
00595 {
00596   // Initialize ROS
00597   ros::init(argc, argv, "createCalibs");
00598 
00599   new createCalibs();
00600 
00601   return (0);
00602 }


youbot_overhead_vision
Author(s): Fred Clinckemaillie, Maintained by David Kent
autogenerated on Thu Jan 2 2014 12:14:12