00001
00007 #include <youbot_overhead_vision/create_calibs.h>
00008
00009 using namespace std;
00010
00011 createCalibs::createCalibs()
00012 {
00013
00014
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
00025
00026
00027
00028 calibToggled = 1;
00029 cameraShown = NORTH_CAMERA;
00030
00031
00032 roomBoundaryModeFlag = false;
00033 roomBoundaryPoints = *(new vector<cv::Point>());
00034
00035
00036 calibratingColor = FLOOR;
00037
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
00047 nh.getParam("/create_calibs/runFromSimulation", useSim);
00048
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
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
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
00106
00107
00108 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
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
00129 if (zoom)
00130 {
00131
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
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)
00150 {
00151 if (imagePaused)
00152 imagePaused = false;
00153 else
00154 imagePaused = true;
00155 }
00156 if (key == 115)
00157 {
00158 ROS_INFO("Saving Files");
00159 saveCalibsToFiles();
00160 }
00161 else if (key == 100)
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)
00170 {
00171 calibToggled = (calibToggled + 1) % 3;
00172 }
00173 else if (key == 49)
00174 {
00175 cameraShown = SOUTH_CAMERA;
00176 }
00177 else if (key == 50)
00178 {
00179 cameraShown = NORTH_CAMERA;
00180 }
00181 else if (key == 27)
00182 {
00183 escPressed = true;
00184 }
00185 else if (key == 97)
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)
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)
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)
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)
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)
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)
00272 {
00273 if (zoom)
00274 {
00275 zoom = false;
00276 }
00277 else
00278 {
00279 zoom = true;
00280
00281
00282 zoomQuadX = mouseX / (out1.cols / (ZOOM_CONSTANT) * SCREEN_RESIZE);
00283 zoomQuadY = mouseY / (out1.rows / (ZOOM_CONSTANT) * SCREEN_RESIZE);
00284 }
00285 }
00286 }
00287
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
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)
00313
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
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
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
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
00431 if (!imagePaused)
00432 {
00433 ros::Time time = image_msg->header.stamp;
00434
00435 raw_Img1 = cv_bridge::toCvCopy(image_msg, ENCODING);
00436
00437
00438 raw_Img1->encoding = ENCODING;
00439
00440 raw_Img1->header.seq = seq_num++;
00441
00442 raw_Img1->header.stamp = ros::Time::now();
00443
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
00452 if (!imagePaused)
00453 {
00454 ros::Time time = image_msg->header.stamp;
00455 raw_Img2 = cv_bridge::toCvCopy(image_msg, ENCODING);
00456
00457
00458 raw_Img2->encoding = ENCODING;
00459
00460 raw_Img2->header.seq = seq_num++;
00461
00462 raw_Img2->header.stamp = ros::Time::now();
00463
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
00484 for (int index = 0; index < numBytes; index += 3)
00485 {
00486
00487
00488 int calibType = calTree->parseCalibTree((int)image.data[index], (int)image.data[index + 1],
00489 (int)image.data[index + 2], camera, calibAccuracy);
00490
00491
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)
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
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
00537 int pointIndex = 1;
00538 if (fileType == FLOOR)
00539 {
00540
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
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
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;
00590 ss << number;
00591 return ss.str();
00592 }
00593
00594 int main(int argc, char** argv)
00595 {
00596
00597 ros::init(argc, argv, "createCalibs");
00598
00599 new createCalibs();
00600
00601 return (0);
00602 }