00001
00005 #include "calibrator.hpp"
00006
00007 int main(int argc, char** argv) {
00008
00009 ROS_INFO("<%s> node launched.", __PROGRAM__);
00010
00011 ros::init(argc, argv, "mm_calibrator");
00012
00013 ros::NodeHandle private_node_handle("~");
00014
00015 calibratorData startupData;
00016
00017 startupData.read_addr = argv[0];
00018 startupData.read_addr = startupData.read_addr.substr(0, startupData.read_addr.size()-11);
00019
00020 bool inputIsValid = startupData.obtainStartingData(private_node_handle);
00021
00022 if (!inputIsValid) {
00023 ROS_ERROR("Configuration invalid.");
00024 }
00025
00026 if (startupData.verboseMode) { ROS_INFO("Startup data processed."); }
00027
00028
00029 ros::NodeHandle nh;
00030 calibratorNode calib_node(nh, startupData);
00031 signal(SIGINT, mySigintHandler);
00032
00033 if (startupData.verboseMode) { ROS_INFO("Node configured."); }
00034
00035 while (calib_node.isStillCollecting()) { ros::spinOnce(); }
00036
00037 ROS_INFO("Patterns captured.");
00038
00039 calib_node.getFrameTrackingTime();
00040
00041 if (calib_node.wantsIntrinsics()) {
00042
00043 ROS_INFO("Performing intrinsic calibration...");
00044
00045 calib_node.preparePatternSubsets();
00046 calib_node.performIntrinsicCalibration();
00047
00048 ROS_INFO("Intrinsic calibration completed.");
00049
00050 } else {
00051 calib_node.assignIntrinsics();
00052 }
00053
00054 if (calib_node.wantsExtrinsics()) {
00055
00056 ROS_INFO("Performing extrinsic calibration...");
00057
00058 calib_node.create_virtual_pairs();
00059 calib_node.prepareExtrinsicPatternSubsets();
00060 calib_node.performExtrinsicCalibration();
00061
00062 }
00063
00064 calib_node.writeResults();
00065
00066 calib_node.set_ready_for_output();
00067
00068 if (calib_node.wantsToUndistort() || (calib_node.wantsToRectify())) {
00069 calib_node.getAverageTime();
00070 }
00071
00072 if (calib_node.wantsToUndistort()) {
00073
00074 if (calib_node.wantsIntrinsics()) {
00075 ROS_INFO("Publishing undistorted images...");
00076
00077 calib_node.startUndistortionPublishing();
00078
00079 while (calib_node.wantsToUndistort()) {
00080 ros::spinOnce();
00081 }
00082
00083 ROS_INFO("Publishing complete.");
00084 }
00085 }
00086
00087 if (calib_node.wantsToRectify()) {
00088 if (calib_node.wantsExtrinsics()) {
00089
00090 ROS_INFO("Publishing rectified images...");
00091
00092 calib_node.startRectificationPublishing();
00093
00094 while (calib_node.wantsToRectify()) {
00095 ros::spinOnce();
00096 }
00097
00098 ROS_INFO("Publishing complete.");
00099
00100 }
00101
00102 }
00103
00104 ROS_WARN("Operations complete. Node terminating.");
00105
00106 return 0;
00107
00108 }
00109
00110 void calibratorNode::prepareExtrinsicPatternSubsets() {
00111
00112 ROS_WARN("prepareExtrinsicPatternSubsets...");
00113
00114 srand((unsigned)time(0));
00115
00116 ROS_INFO("extrinsicsPointSets[0].size() = (%d)", extrinsicsPointSets[0].size());
00117
00118 vector<vector<Point2f> > tmpSet[MAX_ALLOWABLE_CAMS];
00119
00120 for (unsigned int xxx = 0; xxx < configData.numCams; xxx++) {
00121
00122 for (unsigned int iii = 0; iii < extrinsicsPointSets[xxx].size(); iii++) {
00123 tmpSet[xxx].push_back(extrinsicsPointSets[xxx].at(iii));
00124 }
00125
00126 }
00127
00128
00129 while ( (((int)extrinsicCandidateSets[0].size()) < configData.maxCandidates) && (configData.maxCandidates > 0) && (tmpSet[0].size() > 0) ) {
00130
00131 unsigned int randomIndex = rand() % tmpSet[0].size();
00132
00133 for (unsigned int xxx = 0; xxx < configData.numCams; xxx++) {
00134 extrinsicCandidateSets[xxx].push_back(tmpSet[xxx].at(randomIndex));
00135 tmpSet[xxx].erase(tmpSet[xxx].begin() + randomIndex);
00136 }
00137
00138 }
00139
00140 ROS_INFO("extrinsicCandidateSets[0].size() = (%d)", extrinsicCandidateSets[0].size());
00141
00142 for (unsigned int xxx = 0; xxx < configData.numCams; xxx++) {
00143 tmpSet[xxx].clear();
00144 }
00145
00146 for (unsigned int xxx = 0; xxx < configData.numCams; xxx++) {
00147
00148 for (unsigned int iii = 0; iii < extrinsicsPointSets[xxx].size(); iii++) {
00149 tmpSet[xxx].push_back(extrinsicsPointSets[xxx].at(iii));
00150 }
00151
00152 }
00153
00154 ROS_INFO("Here.");
00155
00156
00157 while ( (((int)extrinsicTestingSets[0].size()) < configData.maxTests) && (configData.maxTests > 0) && (tmpSet[0].size() > 0) ) {
00158
00159 unsigned int randomIndex = rand() % tmpSet[0].size();
00160
00161 for (unsigned int xxx = 0; xxx < configData.numCams; xxx++) {
00162 extrinsicTestingSets[xxx].push_back(tmpSet[xxx].at(randomIndex));
00163 tmpSet[xxx].erase(tmpSet[xxx].begin() + randomIndex);
00164 }
00165
00166 }
00167
00168 ROS_INFO("extrinsicTestingSets[0].size() = (%d)", extrinsicTestingSets[0].size());
00169
00170 }
00171
00172 void calibratorNode::create_virtual_pairs() {
00173
00174 ROS_INFO("Extracted patterns for camera (1) : (%d/%d) & (2) : (%d/%d)", pointSets[0].size(), frameCount[0], pointSets[1].size(), frameCount[1]);
00175
00176
00177
00178 for (unsigned int aaa = 0; aaa < configData.numCams; aaa++) {
00179
00180 for (unsigned int bbb = 0; bbb < configData.numCams; bbb++) {
00181
00182 if (aaa == bbb) {
00183 continue;
00184 }
00185
00186 if (configData.verboseMode) { ROS_INFO("Finding virtual frames for camera relationship (%d) -> (%d).", aaa, bbb); }
00187
00188 for (unsigned int iii = 0; iii < patternTimestamps[aaa].size(); iii++) {
00189
00190 unsigned int matchingIndex = 0;
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200 while (patternTimestamps[aaa].at(iii).toSec() > patternTimestamps[bbb].at(matchingIndex).toSec()) {
00201
00202 matchingIndex++;
00203
00204
00205 if (matchingIndex >= patternTimestamps[bbb].size()) {
00206 break;
00207 }
00208
00209 }
00210
00211 if (matchingIndex >= patternTimestamps[bbb].size()) {
00212 ROS_INFO("No matches found, breaking...");
00213 break;
00214 }
00215
00216
00217
00218 if (matchingIndex == 0) {
00219 ROS_INFO("No matches found, continuing...");
00220 continue;
00221 }
00222
00223 if (configData.verboseMode) { ROS_INFO("Best index matches for (%d/%d) = (%d-%d/%d)", iii, patternTimestamps[aaa].size(), matchingIndex-1, matchingIndex, patternTimestamps[bbb].size()); }
00224 if (configData.verboseMode) { ROS_INFO("Best index matches for (%d)[%f] = (%d)[%f] and (%d)[%f]", iii, patternTimestamps[aaa].at(iii).toSec(), matchingIndex-1, patternTimestamps[bbb].at(matchingIndex-1).toSec(), matchingIndex, patternTimestamps[bbb].at(matchingIndex).toSec()); }
00225
00226 double interp_distance_1 = abs(patternTimestamps[aaa].at(iii).toSec() - patternTimestamps[bbb].at(matchingIndex-1).toSec());
00227 double interp_distance_2 = abs(patternTimestamps[aaa].at(iii).toSec() - patternTimestamps[bbb].at(matchingIndex).toSec());
00228 double interp_distance = max(interp_distance_1, interp_distance_2);
00229
00230 if (configData.verboseMode) { ROS_INFO("Interpolation distance = (%f) {max(%f, %f)}", interp_distance, interp_distance_1, interp_distance_2); }
00231
00232
00233
00234 if (interp_distance < configData.maxInterpolationTimegap) {
00235
00236
00237
00238 vector<Point2f> virtualPointset;
00239
00240 double biasFraction = (patternTimestamps[aaa].at(iii).toSec() - patternTimestamps[bbb].at(matchingIndex-1).toSec()) / (patternTimestamps[bbb].at(matchingIndex).toSec() - patternTimestamps[bbb].at(matchingIndex-1).toSec());
00241
00242 double motion = generateVirtualPointset(pointSets[bbb].at(matchingIndex-1), pointSets[bbb].at(matchingIndex), virtualPointset, biasFraction);
00243
00244 if (motion < configData.maxInterpolationMotion) {
00245 extrinsicsPointSets[aaa].push_back(pointSets[aaa].at(iii));
00246 extrinsicsPointSets[bbb].push_back(virtualPointset);
00247 }
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268 }
00269
00270 }
00271
00272 }
00273
00274 }
00275
00276 if (configData.verboseMode) { ROS_INFO("Found (%d // %d) virtual pairs", extrinsicsPointSets[0].size(), extrinsicsPointSets[1].size()); }
00277
00278 if (configData.verboseMode) { ROS_INFO("Completed generation of virtual frame pairs."); }
00279
00280 }
00281
00282 void calibratorNode::preparePatternSubsets() {
00283
00284 ROS_WARN("Preparing pattern subsets...");
00285
00286 srand((unsigned)time(0));
00287
00288
00289
00290 for (unsigned int xxx = 0; xxx < configData.numCams; xxx++) {
00291 vector<vector<Point2f> > tmpSet;
00292
00293 if (configData.verboseMode) { ROS_INFO("Looping for cam (%d)", xxx); }
00294
00295 for (unsigned int iii = 0; iii < pointSets[xxx].size(); iii++) {
00296 tmpSet.push_back(pointSets[xxx].at(iii));
00297 if (pointSets[xxx].at(iii).size() == 0) {
00298 ROS_ERROR("FOR SOME REASON A POINT SET HAS NO POINTS IN IT!!");
00299 imshow("tmp", prevMat[xxx]);
00300 waitKey(0);
00301 }
00302 }
00303
00304 if (configData.verboseMode) { ROS_INFO("Loop (%d) completed", xxx); }
00305
00306 int actualMaxCandidates;
00307 actualMaxCandidates = min(configData.maxCandidates, int(pointSets[xxx].size()));
00308
00309
00310 while ( (((double) candidateSets[xxx].size()) < actualMaxCandidates) && (configData.maxCandidates > 0) && (tmpSet.size() > 0) ) {
00311
00312
00313
00314 unsigned int randomIndex = rand() % tmpSet.size();
00315
00316 candidateSets[xxx].push_back(tmpSet.at(randomIndex));
00317
00318 tmpSet.erase(tmpSet.begin() + randomIndex);
00319
00320 }
00321
00322 tmpSet.clear();
00323
00324 for (unsigned int iii = 0; iii < pointSets[xxx].size(); iii++) {
00325 tmpSet.push_back(pointSets[xxx].at(iii));
00326 }
00327
00328 int actualMaxTests;
00329 actualMaxTests = min(configData.maxTests, int(pointSets[xxx].size()));
00330
00331
00332 while ( (((double) testingSets[xxx].size()) < actualMaxTests) && (configData.maxTests > 0) && (tmpSet.size() > 0) ) {
00333
00334 unsigned int randomIndex = rand() % tmpSet.size();
00335
00336 testingSets[xxx].push_back(tmpSet.at(randomIndex));
00337
00338 tmpSet.erase(tmpSet.begin() + randomIndex);
00339
00340 }
00341
00342 }
00343
00344 }
00345
00346 void calibratorNode::prepareForTermination() {
00347 return;
00348 }
00349
00350 void mySigintHandler(int sig) {
00351 wantsToShutdown = true;
00352 ROS_WARN("Requested shutdown... terminating feeds...");
00353
00354 (*globalNodePtr)->prepareForTermination();
00355 }
00356
00357 void calibratorNode::assignIntrinsics() {
00358
00359 configData.K[0].copyTo(cameraMatrices[0]);
00360 configData.K[1].copyTo(cameraMatrices[1]);
00361 configData.distCoeffs[0].copyTo(distCoeffVecs[0]);
00362 configData.distCoeffs[1].copyTo(distCoeffVecs[1]);
00363
00364 }
00365
00366 bool calibratorNode::wantsIntrinsics() {
00367 if (configData.calibType == "single") {
00368 return true;
00369 } else {
00370 if (configData.wantsIntrinsics) {
00371 return true;
00372 } else {
00373 return false;
00374 }
00375 }
00376 }
00377
00378 bool calibratorNode::wantsExtrinsics() {
00379 if (configData.calibType == "stereo") {
00380 return true;
00381 } else {
00382 return false;
00383 }
00384 }
00385
00386 void calibratorNode::writeResults() {
00387
00388
00389 if (wantsIntrinsics()) {
00390 for (unsigned int xxx = 0; xxx < configData.numCams; xxx++) {
00391
00392 updateIntrinsicMap(xxx);
00393
00394 stringstream ss;
00395 ss << (xxx+1);
00396 string outputName = configData.outputFolder + "/" + "intrinsics-" + ss.str() + ".yml";
00397
00398 if (configData.verboseMode) { ROS_INFO("outputName = (%s)", outputName.c_str()); }
00399
00400 FileStorage fs(outputName, FileStorage::WRITE);
00401
00402 Mat imageSize(1, 2, CV_16UC1);
00403 imageSize.at<unsigned short>(0,0) = imSize[xxx].width;
00404 imageSize.at<unsigned short>(0,1) = imSize[xxx].height;
00405 fs << "imageSize" << imageSize;
00406 fs << "cameraMatrix" << cameraMatrices[xxx];
00407 fs << "distCoeffs" << distCoeffVecs[xxx];
00408 fs << "newCamMat" << newCamMat[xxx];
00409 fs << "alpha" << configData.alpha;
00410
00411 fs << "reprojectionError" << reprojectionError_intrinsics[xxx];
00412 fs << "generalisedError" << extendedReprojectionError_intrinsics[xxx];
00413
00414 fs << "patternsUsed" << ((int) subselectedTags_intrinsics[xxx].size());
00415
00416 fs.release();
00417
00418 ROS_WARN("Wrote intrinsics calibration data to:\n %s", outputName.c_str());
00419
00420 }
00421
00422 }
00423
00424 if (wantsExtrinsics()) {
00425
00426 string outputName = configData.outputFolder + "/" + "extrinsics" + ".yml";
00427
00428 if (configData.verboseMode) { ROS_INFO("outputName = (%s)", outputName.c_str()); }
00429
00430 FileStorage fs(outputName, FileStorage::WRITE);
00431
00432
00433
00434 string tmpString;
00435 char tmp[64];
00436
00437 sprintf(tmp, "reprojectionErr");
00438 tmpString = string(tmp);
00439 fs << tmpString << stereoError;
00440
00441
00442
00443 sprintf(tmp, "generalisedMRE");
00444 tmpString = string(tmp);
00445 fs << tmpString << extendedExtrinsicReprojectionError;
00446
00447
00448
00449 for (unsigned int i = 0; i < configData.numCams; i++) {
00450
00451 sprintf(tmp, "R%d", i);
00452
00453 tmpString = string(tmp);
00454 fs << tmpString << R[i];
00455
00456 sprintf(tmp, "Rvec%d", i);
00457 tmpString = string(tmp);
00458 fs << tmpString << Rv[i];
00459
00460 sprintf(tmp, "T%d", i);
00461
00462 tmpString = string(tmp);
00463 fs << tmpString << T[i];
00464
00465 sprintf(tmp, "R_%d", i);
00466
00467 tmpString = string(tmp);
00468 fs << tmpString << R_[i];
00469
00470 sprintf(tmp, "P_%d", i);
00471
00472 tmpString = string(tmp);
00473 fs << tmpString << P_[i];
00474
00475 sprintf(tmp, "cameraMatrix%d", i);
00476
00477 tmpString = string(tmp);
00478 fs << tmpString << cameraMatrices[i];
00479
00480 sprintf(tmp, "distCoeffs%d", i);
00481
00482 tmpString = string(tmp);
00483 fs << tmpString << distCoeffVecs[i];
00484 }
00485
00486 fs << "E" << E[1];
00487 fs << "F" << F[1];
00488
00489
00490 fs << "R" << R[1];
00491 fs << "T" << T[1];
00492 fs << "alpha" << configData.alpha;
00493
00494 fs << "Q0" << Q[0];
00495 fs << "Q1" << Q[1];
00496
00497
00498
00499 fs.release();
00500
00501 ROS_WARN("Wrote extrinsic calibration data to:\n %s", outputName.c_str());
00502
00503 if (!configData.fixIntrinsics) {
00504
00505 for (unsigned int xxx = 0; xxx < configData.numCams; xxx++) {
00506
00507 stringstream ss;
00508 ss << (xxx+1);
00509 string outputName = configData.outputFolder + "/" + "intrinsics-modified-" + ss.str() + ".yml";
00510
00511 if (configData.verboseMode) { ROS_INFO("outputName = (%s)", outputName.c_str()); }
00512
00513 FileStorage fs(outputName, FileStorage::WRITE);
00514
00515 Mat imageSize(1, 2, CV_16UC1);
00516 imageSize.at<unsigned short>(0,0) = imSize[xxx].width;
00517 imageSize.at<unsigned short>(0,1) = imSize[xxx].height;
00518 fs << "imageSize" << imageSize;
00519 fs << "cameraMatrix" << cameraMatrices[xxx];
00520 fs << "distCoeffs" << distCoeffVecs[xxx];
00521
00522 Rect* validPixROI = 0;
00523 bool centerPrincipalPoint = true;
00524
00525 newCamMat[xxx] = getOptimalNewCameraMatrix(cameraMatrices[xxx], distCoeffVecs[xxx], imSize[xxx], configData.alpha, imSize[xxx], validPixROI, centerPrincipalPoint);
00526
00527 fs << "newCamMat" << newCamMat[xxx];
00528
00529
00530
00531
00532
00533
00534
00535 fs.release();
00536
00537 ROS_WARN("Wrote modified intrinsics calibration data to:\n %s", outputName.c_str());
00538
00539 }
00540 }
00541 }
00542
00543
00544
00545
00546 }
00547
00548 bool calibratorNode::wantsToUndistort() {
00549 return configData.undistortImages;
00550 }
00551
00552 bool calibratorNode::wantsToRectify() {
00553 return configData.rectifyImages;
00554 }
00555
00556 void calibratorNode::getAverageTime() {
00557
00558 unsigned int divisorFrames = 0;
00559
00560 for (unsigned int iii = 0; iii < configData.numCams; iii++) {
00561 divisorFrames = max(divisorFrames, frameCount[iii]);
00562 }
00563
00564 avgTime /= ((double) divisorFrames - 1);
00565 ROS_INFO("?Average time per frame: (%f)", avgTime);
00566
00567 }
00568
00569 void calibratorNode::getFrameTrackingTime() {
00570
00571 unsigned int divisorFrames = 0;
00572
00573 for (unsigned int iii = 0; iii < configData.numCams; iii++) {
00574 divisorFrames = max(divisorFrames, frameCount[iii]);
00575 }
00576
00577 frameTrackingTime /= ((double) divisorFrames - 1);
00578 ROS_INFO("Average tracking time per frame: (%f)", frameTrackingTime);
00579
00580 }
00581
00582
00583
00584 void calibratorNode::startUndistortionPublishing() {
00585
00586
00587
00588
00589
00590
00591 for (unsigned int xxx = 0; xxx < configData.numCams; xxx++) {
00592 updateIntrinsicMap(xxx);
00593
00594
00595 }
00596
00597
00598
00599 timer = ref->createTimer(ros::Duration(avgTime / 1000.0), &calibratorNode::publishUndistorted, this);
00600
00601 }
00602
00603 void calibratorNode::startRectificationPublishing() {
00604
00605
00606
00607
00608
00609 Point pt1, pt2;
00610 vector<Point2f> rectangleBounds, newRecBounds;
00611
00612
00613
00614 Mat blankCoeffs(1, 8, CV_64F);
00615
00616
00617
00618
00619
00620
00621
00622
00623 for (int i = 0; i < 2; i++) {
00624
00625 ROS_INFO("Initializing rectification map (%d)...", i);
00626
00627 initUndistortRectifyMap(cameraMatrices[i],
00628 distCoeffVecs[i],
00629 R_[i],
00630 P_[i],
00631 imSize[i],
00632 CV_32F,
00633 map1[i],
00634 map2[i]);
00635
00636
00637 ROS_INFO("Map (%d) initialized.", i);
00638
00639 pt1 = Point(roi[i].x, roi[i].y);
00640 pt2 = Point(roi[i].x + roi[i].width, roi[i].y + roi[i].height);
00641
00642 if ((pt1.x == 0) && (pt1.y == 0) && (pt2.x == 0) && (pt2.y == 0)) {
00643 ROS_WARN("ROI is defined as having zero area...");
00644 } else {
00645 printf("%s << (und) pt1 = (%d, %d); pt2 = (%d, %d)\n", __FUNCTION__, pt1.x, pt1.y, pt2.x, pt2.y);
00646
00647 rectangleBounds.push_back(Point2f(pt1.x, pt1.y));
00648 rectangleBounds.push_back(Point2f(pt2.x, pt2.y));
00649
00650 Rect validROI_x;
00651
00652
00653 rectCamMat[i] = getOptimalNewCameraMatrix(cameraMatrices[i], distCoeffVecs[i], imSize[i], configData.alpha, imSize[i], &validROI_x);
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666
00667
00668
00669
00670
00671
00672
00673
00674
00675
00676
00677
00678
00679
00680
00681 }
00682
00683
00684 }
00685
00686 printf("%s << topValidHeight = %d; botValidHeight = %d\n", __FUNCTION__, topValidHeight, botValidHeight);
00687
00688
00689
00690
00691 for (int k = 1; k < 32; k++)
00692 {
00693
00694 leftLinePoints.push_back(Point2f(0, k*(botValidHeight - topValidHeight)/32 - 1));
00695 rightLinePoints.push_back(Point2f(imSize[0].width, k*(botValidHeight - topValidHeight)/32 - 1));
00696 }
00697
00698
00699
00700
00701
00702 timer = ref->createTimer(ros::Duration(avgTime / 1000.0), &calibratorNode::publishRectified, this);
00703
00704 }
00705
00706 void calibratorNode::publishUndistorted(const ros::TimerEvent& event) {
00707
00708
00709
00710 Mat dispMat;
00711
00712 if (configData.calibType == "single") {
00713 if (undistortionCount >= frameCount[0]) {
00714 configData.undistortImages = false;
00715 return;
00716 }
00717
00718
00719
00720 Mat colorMat, preMat, postMat;
00721
00722 cvtColor(displayImages[0].at(undistortionCount), colorMat, CV_GRAY2RGB);
00723
00724
00725 if (configData.drawGrids) {
00726 drawGrid(colorMat, preMat, 0);
00727 } else {
00728 colorMat.copyTo(preMat);
00729 }
00730
00731
00732 remap(preMat, postMat, map1[0], map2[0], INTER_LINEAR, BORDER_TRANSPARENT);
00733
00734
00735 if (configData.drawGrids) {
00736 drawGrid(postMat, dispMat, 1);
00737 } else {
00738 postMat.copyTo(dispMat);
00739 }
00740
00741 std::copy(&(dispMat.at<Vec3b>(0,0)[0]), &(dispMat.at<Vec3b>(0,0)[0])+(dispMat.cols*dispMat.rows*dispMat.channels()), msg_debug[0].data.begin());
00742 debug_pub[0].publish(msg_debug[0], debug_camera_info[0]);
00743
00744
00745
00746 } else {
00747 if (undistortionCount >= validPairs[0].size()) {
00748 configData.undistortImages = false;
00749 return;
00750 }
00751
00752 Mat preMat, postMat;
00753
00754
00755 drawGrid(displayImages[0].at(validPairs[0].at(undistortionCount)), preMat, 0);
00756 remap(preMat, postMat, map1[0], map2[0], INTER_LINEAR, BORDER_TRANSPARENT);
00757 drawGrid(postMat, dispMat, 1);
00758 std::copy(&(dispMat.at<Vec3b>(0,0)[0]), &(dispMat.at<Vec3b>(0,0)[0])+(dispMat.cols*dispMat.rows*dispMat.channels()), msg_debug[0].data.begin());
00759 debug_pub[0].publish(msg_debug[0], debug_camera_info[0]);
00760
00761
00762 if (configData.numCams > 1) {
00763 drawGrid(displayImages[1].at(validPairs[1].at(undistortionCount)), preMat, 0);
00764 remap(preMat, postMat, map1[1], map2[1], INTER_LINEAR, BORDER_TRANSPARENT);
00765 drawGrid(postMat, dispMat, 1);
00766 std::copy(&(dispMat.at<Vec3b>(0,0)[0]), &(dispMat.at<Vec3b>(0,0)[0])+(dispMat.cols*dispMat.rows*dispMat.channels()), msg_debug[1].data.begin());
00767 debug_pub[1].publish(msg_debug[1], debug_camera_info[1]);
00768 }
00769 }
00770
00771 undistortionCount++;
00772 }
00773
00774 void calibratorNode::publishRectified(const ros::TimerEvent& event) {
00775
00776
00777
00778 Mat dispMat;
00779
00780
00781 if (rectificationCount >= validPairs[0].size()) {
00782 configData.rectifyImages = false;
00783 return;
00784 }
00785
00786 Mat und1, und2, disp1, disp2;
00787
00788
00789 remap(displayImages[0].at(validPairs[0].at(rectificationCount)), und1, map1[0], map2[0], INTER_LINEAR);
00790
00791
00792
00793 drawGrid(und1, disp1, 1);
00794
00795
00796 rectangle(disp1, Point2f(roi[0].x, roi[0].y), Point2f(roi[0].x + roi[0].width , roi[0].y + roi[0].height), CV_RGB(127, 255, 0));
00797
00798
00799 std::copy(&(disp1.at<Vec3b>(0,0)[0]), &(disp1.at<Vec3b>(0,0)[0])+(disp1.cols*disp1.rows*disp1.channels()), msg_debug[0].data.begin());
00800 debug_pub[0].publish(msg_debug[0], debug_camera_info[0]);
00801
00802 remap(displayImages[1].at(validPairs[1].at(rectificationCount)), und2, map1[1], map2[1], INTER_LINEAR);
00803 drawGrid(und2, disp2, 1);
00804
00805 rectangle(disp2, Point2f(roi[1].x, roi[1].y), Point2f(roi[1].x + roi[1].width , roi[1].y + roi[1].height), CV_RGB(127, 255, 0));
00806
00807 std::copy(&(disp2.at<Vec3b>(0,0)[0]), &(disp2.at<Vec3b>(0,0)[0])+(disp2.cols*disp2.rows*disp2.channels()), msg_debug[1].data.begin());
00808 debug_pub[1].publish(msg_debug[1], debug_camera_info[1]);
00809
00810 rectificationCount++;
00811 }
00812
00813 void calibratorNode::performExtrinsicCalibration() {
00814
00815
00816 ROS_INFO("Initial intrinsics: ");
00817
00818 cout << "K1 = " << configData.K[0] << endl;
00819 cout << "K2 = " << configData.K[1] << endl;
00820
00821 cout << "imSize[0] = (" << imSize[0].height << ", " << imSize[0].width << ")" << endl;
00822 cout << "imSize[1] = (" << imSize[1].height << ", " << imSize[1].width << ")" << endl;
00823
00824 cv::vector<cv::vector<Point2f> > emptyPointSetVector;
00825 vector<int> emptyIntVector;
00826
00827 vector<vector<int> > extrinsicTagNames, extrinsicSelectedTags;
00828
00829
00830 cv::vector<Mat> extrinsicsDistributionMap;
00831 extrinsicsDistributionMap.resize(2);
00832
00833 for (unsigned int nnn = 0; nnn < configData.numCams; nnn++) {
00834
00835
00836 extrinsicTagNames.push_back(emptyIntVector);
00837
00838 extrinsicsDistributionMap.at(nnn) = Mat(imSize[nnn], CV_8UC1);
00839 }
00840
00841
00842 for (unsigned int iii = 0; iii < extrinsicsPointSets[0].size(); iii++) {
00843
00844 for (unsigned int nnn = 0; nnn < 2; nnn++) {
00845
00846
00847 extrinsicTagNames.at(nnn).push_back(iii);
00848 }
00849 }
00850
00851 vector<Size> extrinsicsSizes;
00852 extrinsicsSizes.push_back(imSize[0]);
00853 extrinsicsSizes.push_back(imSize[1]);
00854
00855
00856
00857
00858 ROS_INFO("Optimizing calibration sets, (%d) candidates and (%d) testing patterns", extrinsicCandidateSets[0].size(), extrinsicTestingSets[0].size());
00859
00860 int extrinsicsFlags = 0;
00861
00862 if (!configData.noConstraints) {
00863 extrinsicsFlags += CV_CALIB_ZERO_TANGENT_DIST;
00864 }
00865
00866 if (configData.fixPrincipalPoint) {
00867 extrinsicsFlags += CV_CALIB_FIX_PRINCIPAL_POINT;
00868 }
00869
00870 if (configData.fixIntrinsics) {
00871 ROS_WARN("Fixing intrinsic parameters for extrinsic calibration..");
00872 extrinsicsFlags += CV_CALIB_FIX_INTRINSIC + CV_CALIB_FIX_FOCAL_LENGTH + CV_CALIB_FIX_K6 + CV_CALIB_FIX_K5 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K3 + CV_CALIB_FIX_K2 + CV_CALIB_FIX_K1;
00873 } else {
00874 extrinsicsFlags += CV_CALIB_USE_INTRINSIC_GUESS;
00875 }
00876
00877 if (configData.useRationalModel) {
00878 extrinsicsFlags += CV_CALIB_RATIONAL_MODEL;
00879 }
00880
00881 cout << "Super-Original camera matrices: " << endl;
00882 cout << "cameraMatrices[0] = " << cameraMatrices[0] << endl;
00883 cout << "cameraMatrices[1] = " << cameraMatrices[1] << endl;
00884 cout << "distCoeffVecs[0] = " << distCoeffVecs[0] << endl;
00885 cout << "distCoeffVecs[1] = " << distCoeffVecs[1] << endl;
00886
00887 ROS_WARN("Optimizing with (%d) candidates and (%d) test patterns...", extrinsicCandidateSets[0].size(), extrinsicTestingSets[0].size());
00888
00889 optimizeCalibrationSets(extrinsicsSizes, 2, cameraMatrices, distCoeffVecs, extrinsicsDistributionMap, extrinsicCandidateSets, extrinsicTestingSets, row, configData.optimizationMethod, configData.setSize, extrinsicTagNames, extrinsicSelectedTags, extrinsicsFlags);
00890
00891
00892
00893
00894
00895
00896 TermCriteria term_crit;
00897 term_crit = TermCriteria(TermCriteria::COUNT+ TermCriteria::EPS, 30, 1e-6);
00898
00899
00900
00901 R[0] = Mat::eye(3, 3, CV_64FC1);
00902 T[0] = Mat::zeros(3, 1, CV_64FC1);
00903
00904 R[1] = Mat::eye(3, 3, CV_64FC1);
00905 T[1] = Mat::zeros(3, 1, CV_64FC1);
00906
00907 cv::vector< cv::vector<Point3f> > objectPoints;
00908
00909
00910
00911
00912
00913
00914
00915
00916 objectPoints.resize(extrinsicCandidateSets[0].size());
00917 for (unsigned int iii = 0; iii < extrinsicCandidateSets[0].size(); iii++)
00918 {
00919 for (int j = 0; j < configData.yCount; j++ ){
00920 for (int i = 0; i < configData.xCount; i++ ){
00921 objectPoints[iii].push_back(Point3f(float(j)*configData.gridSize/1000.0, float(i)*configData.gridSize/1000.0,0));
00922 }
00923 }
00924 }
00925
00926
00927 cout << "Original camera matrices: " << endl;
00928 cout << "cameraMatrices[0] = " << cameraMatrices[0] << endl;
00929 cout << "cameraMatrices[1] = " << cameraMatrices[1] << endl;
00930 cout << "distCoeffVecs[0] = " << distCoeffVecs[0] << endl;
00931 cout << "distCoeffVecs[1] = " << distCoeffVecs[1] << endl;
00932
00933 stereoError = stereoCalibrate(objectPoints,
00934 extrinsicCandidateSets[0], extrinsicCandidateSets[1],
00935 cameraMatrices[0], distCoeffVecs[0],
00936 cameraMatrices[1], distCoeffVecs[1],
00937 extrinsicsSizes[0],
00938 R[1], T[1],
00939 E[1], F[1],
00940 term_crit,
00941 extrinsicsFlags);
00942
00943
00944 cout << "Refined camera matrices: " << endl;
00945 cout << "cameraMatrices[0] = " << cameraMatrices[0] << endl;
00946 cout << "cameraMatrices[1] = " << cameraMatrices[1] << endl;
00947 cout << "distCoeffVecs[0] = " << distCoeffVecs[0] << endl;
00948 cout << "distCoeffVecs[1] = " << distCoeffVecs[1] << endl;
00949 cout << "R[1] = " << R[1] << endl;
00950 cout << "T[1] = " << T[1] << endl;
00951 printf("extrinsicsSizes[0] = (%d, %d)\n", extrinsicsSizes[0].width, extrinsicsSizes[0].height);
00952
00953
00954
00955
00956 if (configData.autoAlpha) {
00957 configData.alpha = findBestAlpha(cameraMatrices[0], distCoeffVecs[0], imSize[0]);
00958 ROS_INFO("Optimal alpha = (%f)", configData.alpha);
00959 }
00960
00961 cout << "alpha = " << configData.alpha << endl;
00962
00963
00964 stereoRectify(cameraMatrices[0], distCoeffVecs[0], cameraMatrices[1], distCoeffVecs[1],
00965 imSize[0],
00966 R[1], T[1],
00967 R_[0], R_[1], P_[0], P_[1],
00968 Q[0],
00969 CALIB_ZERO_DISPARITY,
00970 configData.alpha, imSize[0], &roi[0], &roi[1]);
00971
00972
00973
00974
00975
00976
00977
00978
00979
00980
00981
00982
00983
00984
00985
00986
00987
00988
00989
00990
00991 printf("imsize[0] = (%d, %d)\n", imSize[0].width, imSize[0].height);
00992 printf("imsize[1] = (%d, %d)\n", imSize[1].width, imSize[1].height);
00993
00994
00995 cout << "R_[0] = " << R_[0] << endl;
00996 cout << "R_[1] = " << R_[1] << endl;
00997 cout << "P_[0] = " << P_[0] << endl;
00998 cout << "P_[1] = " << P_[1] << endl;
00999 cout << "Q[0] = " << Q[0] << endl;
01000 cout << "Q[1] = " << Q[1] << endl;
01001
01002 printf("roi[0] = (%d, %d, %d, %d)\n", roi[0].x, roi[0].y, roi[0].width, roi[0].height);
01003 printf("roi[1] = (%d, %d, %d, %d)\n", roi[1].x, roi[1].y, roi[1].width, roi[1].height);
01004
01005 extendedExtrinsicReprojectionError = calculateExtrinsicERE(2, objectPoints.at(0), extrinsicsPointSets, cameraMatrices, distCoeffVecs, R, T);
01006
01007 cout << "R = " << R[1] << endl;
01008 cout << "T = " << T[1] << endl;
01009
01010 return;
01011
01012 }
01013
01014 void calibratorNode::performIntrinsicCalibration() {
01015
01016
01017
01018
01019
01020 int intrinsicsFlags = 0;
01021
01022 if (!configData.noConstraints) {
01023 intrinsicsFlags += CV_CALIB_FIX_ASPECT_RATIO;
01024 }
01025
01026 if (configData.useRationalModel) {
01027 intrinsicsFlags += CV_CALIB_RATIONAL_MODEL;
01028 }
01029
01030 if (configData.ignoreDistortion) {
01031 intrinsicsFlags += CV_CALIB_FIX_K6 + CV_CALIB_FIX_K5 + CV_CALIB_FIX_K4 + CV_CALIB_FIX_K3 + CV_CALIB_FIX_K2 + CV_CALIB_FIX_K1 + CV_CALIB_ZERO_TANGENT_DIST;
01032 }
01033
01034 if (configData.fixPrincipalPoint) {
01035 intrinsicsFlags += CV_CALIB_FIX_PRINCIPAL_POINT;
01036 }
01037
01038
01039 for (unsigned int xxx = 0; xxx < configData.numCams; xxx++) {
01040
01041 ROS_INFO("Intrinsically calibrating camera (%d) with (%d / %d / %d) patterns...", xxx, candidateSets[xxx].size(), testingSets[xxx].size(), pointSets[xxx].size());
01042
01043 bool debugOptimizationFunction = false;
01044 optimizeCalibrationSet(imSize[xxx], candidateSets[xxx], testingSets[xxx], row, subselectedTags_intrinsics[xxx], configData.optimizationMethod, configData.setSize, debugOptimizationFunction, configData.removeSpatialBias, configData.generateFigures, configData.useUndistortedLocations, intrinsicsFlags);
01045
01046 ROS_INFO("Set optimized (%d)", subselectedTags_intrinsics[xxx].size());
01047
01048 vector< vector<Point3f> > objectPoints;
01049 vector< vector<Point2f> > subselectedPatterns;
01050 vector<Mat> rvecs, tvecs;
01051
01052 for (unsigned int iii = 0; iii < subselectedTags_intrinsics[xxx].size(); iii++) {
01053 objectPoints.push_back(row);
01054 subselectedPatterns.push_back(candidateSets[xxx].at(iii));
01055 }
01056
01057 rvecs.resize(subselectedTags_intrinsics[xxx].size());
01058 tvecs.resize(subselectedTags_intrinsics[xxx].size());
01059
01060 Mat camMat, distCoeffs;
01061
01062
01063
01064 reprojectionError_intrinsics[xxx] = calibrateCamera(objectPoints, subselectedPatterns, imSize[xxx], cameraMatrices[xxx], distCoeffVecs[xxx], rvecs, tvecs, intrinsicsFlags);
01065
01066 ROS_ERROR("Reprojection error = %f, distCoeffVecs[%d].size() = (%d)", reprojectionError_intrinsics[xxx], xxx, distCoeffVecs[xxx].cols);
01067
01068
01069 extendedReprojectionError_intrinsics[xxx] = calculateERE(imSize[xxx], objectPoints.at(0), testingSets[xxx], cameraMatrices[xxx], distCoeffVecs[xxx], configData.removeSpatialBias, configData.generateFigures, configData.useUndistortedLocations);
01070
01071 ROS_INFO("Extended Reprojection error = %f (%d patterns)", extendedReprojectionError_intrinsics[xxx], testingSets[xxx].size());
01072 }
01073
01074
01075
01076 }
01077
01078 bool calibratorData::obtainStartingData(ros::NodeHandle& nh) {
01079
01080 nh.param<bool>("verboseMode", verboseMode, false);
01081
01082 nh.param<bool>("fixIntrinsics", fixIntrinsics, true);
01083
01084 nh.param<bool>("fixPrincipalPoint", fixPrincipalPoint, false);
01085
01086 nh.param<bool>("noConstraints", noConstraints, false);
01087
01088
01089 nh.param<std::string>("video_stream", video_stream, "video_stream");
01090
01091 if (video_stream != "video_stream") {
01092 ROS_INFO("<video_stream> (%s) selected.", video_stream.c_str());
01093 } else {
01094 ROS_ERROR("<video_stream> not specified.");
01095 return false;
01096 }
01097
01098 nh.param<bool>("ignoreDistortion", ignoreDistortion, false);
01099
01100
01101 nh.param<bool>("useRationalModel", useRationalModel, false);
01102 nh.param<std::string>("calibType", calibType, "calibType");
01103
01104 if (calibType == "single") {
01105 ROS_INFO("Single-camera calibration selected.");
01106 numCams = 1;
01107 } else if (calibType == "stereo") {
01108 ROS_INFO("Stereo calibration selected.");
01109 numCams = 2;
01110 } else if (calibType == "calibType") {
01111 ROS_WARN("No calibration type specified. Single-camera assumed.");
01112 calibType = "single";
01113 } else {
01114 ROS_ERROR("Invalid calibration type specified. Please choose either 'single' or 'stereo'.");
01115 return false;
01116 }
01117
01118 nh.param<bool>("useUndistortedLocations", useUndistortedLocations, false);
01119 nh.param<bool>("removeSpatialBias", removeSpatialBias, true);
01120
01121
01122 nh.param<std::string>("video_stream_secondary", video_stream_secondary, "video_stream_secondary");
01123
01124 if (numCams == 2) {
01125
01126 if (video_stream_secondary != "video_stream_secondary") {
01127 ROS_INFO("<video_stream_secondary> = (%s).", video_stream_secondary.c_str());
01128 } else {
01129 ROS_ERROR("<video_stream_secondary> not specified!");
01130 return false;
01131 }
01132
01133
01134
01135 }
01136
01137 nh.param<double>("flowThreshold", flowThreshold, 1e-4);
01138 nh.param<double>("maxFrac", maxFrac, 0.05);
01139 nh.param<double>("errorThreshold", errorThreshold, 0.0);
01140
01141 nh.param<std::string>("outputFolder", outputFolder, "outputFolder");
01142
01143 if (outputFolder != "outputFolder") {
01144 if (verboseMode) { ROS_INFO("Output folder (%s) selected.", outputFolder.c_str()); }
01145 } else {
01146 outputFolder = read_addr + "/../nodes/calibrator/data";
01147 if (verboseMode) { ROS_INFO("No output folder supplied. Defaulting to (%s)", outputFolder.c_str()); }
01148 }
01149
01150
01151
01152 nh.param<bool>("debugMode", debugMode, false);
01153
01154 if (debugMode) {
01155 if (verboseMode) { ROS_INFO("Running in DEBUG mode."); }
01156 } else {
01157 if (verboseMode) { ROS_INFO("Running in OPTIMIZED mode."); }
01158 }
01159
01160 nh.param<std::string>("patternType", patternType, "mask");
01161
01162 if (patternType == "mask") {
01163 pattType = MASK_FINDER_CODE;
01164 } else if (patternType == "chessboard") {
01165 pattType = CHESSBOARD_FINDER_CODE;
01166 } else if (patternType == "heated_chessboard") {
01167 pattType = HEATED_CHESSBOARD_FINDER_CODE;
01168 } else {
01169 ROS_ERROR("<patternType> incorrectly specified (mask/chessboard/heated-chessboard)");
01170 return false;
01171 }
01172
01173 if (verboseMode) { ROS_INFO("<patternType> = (%s) [%d]", patternType.c_str(), pattType); }
01174
01175 nh.param<int>("setSize", setSize, DEFAULT_SET_SIZE);
01176
01177 if (verboseMode) { ROS_INFO("maxFrames = %d; maxPatterns = %d; setSize = (%d)", maxFrames, maxPatterns, setSize); }
01178
01179 nh.param<double>("gridSize", gridSize, DEFAULT_GRID_SIZE);
01180
01181 nh.param<double>("maxTimeDiff", maxTimeDiff, DEFAULT_MAX_TIME_DIFF);
01182
01183 nh.param<int>("xCount", xCount, DEFAULT_X_COUNT);
01184 nh.param<int>("yCount", yCount, DEFAULT_Y_COUNT);
01185
01186 nh.param<std::string>("optMethod", optMethod, "enhancedMCM");
01187
01188 if (optMethod == "enhancedMCM") {
01189 optimizationMethod = ENHANCED_MCM_OPTIMIZATION_CODE;
01190 } else if (optMethod == "allPatterns") {
01191 optimizationMethod = ALL_PATTERNS_OPTIMIZATION_CODE;
01192 } else if (optMethod == "randomSet") {
01193 optimizationMethod = RANDOM_SET_OPTIMIZATION_CODE;
01194 } else if (optMethod == "firstN") {
01195 optimizationMethod = FIRST_N_PATTERNS_OPTIMIZATION_CODE;
01196 } else if (optMethod == "randomBest") {
01197 optimizationMethod = BEST_OF_RANDOM_PATTERNS_OPTIMIZATION_CODE;
01198 } else if (optMethod == "exhaustiveSearch") {
01199 optimizationMethod = EXHAUSTIVE_SEARCH_OPTIMIZATION_CODE;
01200 } else if (optMethod == "randomSeed") {
01201 optimizationMethod = RANDOM_SEED_OPTIMIZATION_CODE;
01202 } else if (optMethod == "scoreBased") {
01203 optimizationMethod = SCORE_BASED_OPTIMIZATION_CODE;
01204 } else {
01205 ROS_ERROR("<optMethod> incorrectly specified...");
01206 return false;
01207 }
01208
01209
01210 nh.param<bool>("undistortImages", undistortImages, false);
01211
01212 nh.param<bool>("rectifyImages", rectifyImages, false);
01213
01214 nh.param<bool>("wantsIntrinsics", wantsIntrinsics, false);
01215
01216 nh.param<std::string>("intrinsicsFile_primary", intrinsicsFiles[0], "intrinsicsFile");
01217 nh.param<std::string>("intrinsicsFile_secondary", intrinsicsFiles[1], "intrinsicsFile");
01218
01219
01220 if (calibType == "single") {
01221
01222 if (wantsIntrinsics == false) {
01223 ROS_WARN("<single> camera mode has been selected, but <wantsIntrinsics> has not. Intrinsics WILL be calculated.");
01224 wantsIntrinsics = true;
01225 }
01226
01227 } else if (calibType == "stereo") {
01228
01229 if (wantsIntrinsics) {
01230 ROS_INFO("<stereo> mode: option to also calculate intrinsics has been selected.");
01231 } else {
01232
01233 if (intrinsicsFiles[0] == "intrinsicsFile") {
01234 ROS_WARN("No intrinsics specified for primary camera, so will attempt to use those encoded in topic header.");
01235 }
01236
01237 if (intrinsicsFiles[1] == "intrinsicsFile") {
01238 ROS_WARN("No intrinsics specified for secondary camera, so will attempt to use those encoded in topic header.");
01239 }
01240
01241 }
01242
01243 }
01244
01245 nh.param<double>("alpha", alpha, 0.00);
01246
01247 nh.param<bool>("autoAlpha", autoAlpha, true);
01248
01249 nh.param<bool>("stopCapturing", stopCapturing, false);
01250 nh.param<std::string>("patternDetectionMode", patternDetectionMode, "find");
01251
01252 nh.param<bool>("useMutex", useMutex, false);
01253
01254
01255
01256 return true;
01257 }
01258
01259 void calibratorNode::updateIntrinsicMap(unsigned int idx) {
01260
01261 if (configData.verboseMode) { ROS_INFO("Updating map..."); }
01262
01263 if (configData.autoAlpha) {
01264 configData.alpha = findBestAlpha(cameraMatrices[idx], distCoeffVecs[idx], imSize[idx]);
01265 if (configData.verboseMode) { ROS_INFO("Optimal alpha = (%f)", configData.alpha); }
01266 }
01267
01268 Rect* validPixROI = 0;
01269 bool centerPrincipalPoint = true;
01270
01271 newCamMat[idx] = getOptimalNewCameraMatrix(cameraMatrices[idx], distCoeffVecs[idx], imSize[idx], configData.alpha, imSize[idx], validPixROI, centerPrincipalPoint);
01272
01273 if (configData.verboseMode) { ROS_WARN("Adjusting undistortion mapping..."); }
01274 initUndistortRectifyMap(cameraMatrices[idx], distCoeffVecs[idx], default_R, newCamMat[idx], imSize[idx], CV_32FC1, map1[idx], map2[idx]);
01275
01276 if (configData.verboseMode) { cout << "newCamMat: " << newCamMat[idx] << endl; }
01277
01278 alphaChanged = false;
01279
01280 }
01281
01282 void calibratorNode::assignDebugCameraInfo() {
01283
01284
01285
01286
01287 }
01288
01289 bool calibratorNode::isStillCollecting() {
01290 return stillCollecting;
01291 }
01292
01294
01295 bool calibratorNode::isVerifying() {
01296 return doVerify;
01297 }
01298
01299
01300 bool calibratorNode::findPattern(const Mat& im, vector<Point2f>& dst, Mat& prev, const int cameraNumber = -1) {
01301
01302 timeElapsedMS(tracking_timer);
01303
01304
01305 bool retVal = false;
01306 Mat tmpMat;
01307
01308 if ((configData.trackingMode) && (dst.size() > 0) && (prev.rows > 0) && (prev.cols > 0)) {
01309
01310 double distanceConstraint = (min(im.cols, im.rows) * configData.maxFrac) + ((((int) (max(im.cols, im.rows) * configData.maxFrac)) + 1) % 2);
01311
01312 vector<Point2f> src;
01313 vector<unsigned int> lost;
01314 lost.clear();
01315 src.insert(src.end(), dst.begin(), dst.end());
01316 dst.clear();
01317
01318
01319
01320
01321
01322
01323
01324 trackPoints2(prev, im, src, dst, distanceConstraint, configData.flowThreshold, lost, cvSize(configData.xCount, configData.yCount), configData.errorThreshold);
01325
01326 int windowSize = distanceConstraint;
01327
01328 if ((windowSize % 2) == 0) {
01329 windowSize++;
01330 }
01331
01332
01333 cornerSubPix(im, dst, Size(windowSize, windowSize), Size(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1));
01334
01335 double maxMotion = 0.0;
01336
01337 for (unsigned int iii = 0; iii < dst.size(); iii++) {
01338
01339 double dist = max((dst.at(iii).x - src.at(iii).x), (dst.at(iii).y - src.at(iii).y));
01340
01341 if (dist > maxMotion) {
01342 maxMotion = dist;
01343 }
01344
01345 }
01346
01347 if ((maxMotion > distanceConstraint) || (lost.size() > 0)) {
01348 if (configData.verboseMode) { ROS_WARN("Tracking failed."); }
01349 dst.clear();
01350 retVal = false;
01351 } else if (!verifyCorners(im.size(), cvSize(configData.xCount, configData.yCount), dst)) {
01352 if (configData.verboseMode) { ROS_WARN("Tracking verification failed."); }
01353 dst.clear();
01354 retVal = false;
01355 } else {
01356 if (configData.verboseMode) { ROS_INFO("Tracking successful."); }
01357
01358 retVal = true;
01359 }
01360
01361 }
01362
01363 if (!retVal) {
01364
01365 switch (configData.pattType) {
01366
01367 case CHESSBOARD_FINDER_CODE:
01368 retVal = findChessboardCorners(im, cvSize(configData.xCount, configData.yCount), dst);
01369 break;
01370 case MASK_FINDER_CODE:
01371 if (configData.adjustMSER[0] && cameraNumber == 0){
01372 retVal = findMaskCorners(im, cvSize(configData.xCount, configData.yCount), dst, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG, configData.delta[0], configData.maxVar[0], configData.minDiv[0]);
01373 } else if (configData.adjustMSER[1] && cameraNumber == 1){
01374 retVal = findMaskCorners(im, cvSize(configData.xCount, configData.yCount), dst, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG, configData.delta[1], configData.maxVar[1], configData.minDiv[1]);
01375 } else {
01376 retVal = findMaskCorners(im, cvSize(configData.xCount, configData.yCount), dst, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG);
01377 }
01378 break;
01379 case HEATED_CHESSBOARD_FINDER_CODE:
01380 invertMatIntensities(im, tmpMat);
01381 retVal = findChessboardCorners(tmpMat, cvSize(configData.xCount, configData.yCount), dst);
01382 break;
01383 default:
01384 retVal = findChessboardCorners(im, cvSize(configData.xCount, configData.yCount), dst);
01385 break;
01386
01387 }
01388 }
01389
01390 elapsedTrackingTime = timeElapsedMS(tracking_timer);
01391
01392
01393
01394
01395 frameTrackingTime += elapsedTrackingTime;
01396
01397 ROS_INFO("dst.size() = (%d); status = (%d)", dst.size(), retVal);
01398
01399 return retVal;
01400
01401 }
01402
01403 void calibratorNode::determineValidPairs() {
01404
01405 ROS_INFO("Determine valid frame pairs...");
01406
01407 unsigned int maxCam1 = std::min((unsigned int)(times[0].size()), frameCount[0]);
01408 unsigned int maxCam2 = std::min((unsigned int)(times[1].size()), frameCount[1]);
01409
01410
01411
01412
01413 while (checkIndex[0] < maxCam1) {
01414
01415 if (checkIndex[0] >= duplicateFlags[0].size()) {
01416 break;
01417 }
01418
01419
01420
01421 if (duplicateFlags[0].at(checkIndex[0]) == 1) {
01422 checkIndex[0]++;
01423 continue;
01424 }
01425
01426
01427
01428
01429
01430
01431 double leastDiff = 9e99;
01432 int bestMatch = -1;
01433
01434 bool candidatesExhausted = false;
01435
01436
01437
01438
01439
01440
01441
01442
01443 while ((timeDiff(times[1].at(checkIndex[1]), times[0].at(checkIndex[0])) < configData.maxTimeDiff)) {
01444
01445
01446
01447 if (checkIndex[1] >= (maxCam2-1)) {
01448 ROS_WARN("...");
01449 break;
01450
01451 }
01452
01453 if (checkIndex[1] >= duplicateFlags[1].size()) {
01454 break;
01455 }
01456
01457
01458 if (duplicateFlags[1].at(checkIndex[1]) == 1) {
01459 checkIndex[1]++;
01460 continue;
01461 }
01462
01463 double diff = abs(timeDiff(times[1].at(checkIndex[1]), times[0].at(checkIndex[0])));
01464
01465
01466
01467 if (diff < leastDiff) {
01468 leastDiff = diff;
01469 bestMatch = checkIndex[1];
01470 }
01471
01472 checkIndex[1]++;
01473 }
01474
01475
01476
01477 if ((timeDiff(times[1].at(checkIndex[1]), times[0].at(checkIndex[0])) >= configData.maxTimeDiff)) {
01478 candidatesExhausted = true;
01479 }
01480
01481
01482
01483 if ((leastDiff < configData.maxTimeDiff) && (bestMatch >= 0)) {
01484
01485
01486
01487 validPairs[0].push_back(checkIndex[0]);
01488 checkIndex[0]++;
01489 validPairs[1].push_back(bestMatch);
01490
01491
01492 } else if (candidatesExhausted) {
01493 checkIndex[0]++;
01494 } else {
01495 break;
01496 }
01497
01498 }
01499
01500 }
01501
01502
01503 void calibratorNode::evaluateFrames() {
01504
01505 ROS_WARN("evaluateFrames...");
01506
01507 vector<validPattern> validPatternData[2];
01508
01509
01510
01511
01512 for (unsigned int camNumber = 0; camNumber <= 2; camNumber++ ){
01513
01514 for (unsigned int i = 0; i < validPairs[camNumber].size(); i++){
01515
01516 for (unsigned int j = 0; j < frameCounts[camNumber].size(); j++){
01517
01518 validPattern tmpData;
01519 tmpData.validFrameID = validPairs[camNumber].at(i);
01520 tmpData.patternID = j;
01521
01522
01523 if (validPairs[camNumber].at(i)==frameCounts[camNumber].at(j)){
01524
01525 tmpData.patternFound = true;
01526 tmpData.isValidFrame = true;
01527
01528 validPatternData[camNumber].push_back(tmpData);
01529 break;
01530
01531
01532 ROS_INFO("Valid pattern found for cam (%d) in frame %d", camNumber, frameCounts[camNumber].at(j));
01533
01534 } else if(j==frameCounts[camNumber].size()-1){
01535
01536
01537
01538 tmpData.patternFound = false;
01539 tmpData.isValidFrame = true;
01540
01541 validPatternData[camNumber].push_back(tmpData);
01542
01543 }
01544 }
01545 }
01546 }
01547
01548
01549 Mat dispMat1, dispMat2;
01550
01551
01552
01553 int totalValidPatterns = 0;
01554
01555 for(unsigned int i=0; i < validPatternData[0].size(); i++){
01556
01557 if ( validPatternData[0].at(i).patternFound && validPatternData[1].at(i).patternFound){
01558
01559 if ( !configData.debugMode) {
01560 extrinsicsPointSets[0].push_back(pointSets[0].at(validPatternData[0].at(i).patternID));
01561 extrinsicsPointSets[1].push_back(pointSets[1].at(validPatternData[1].at(i).patternID));
01562 }
01563 totalValidPatterns++;
01564
01565 }
01566
01567 }
01568
01569 cout << "synced frames: " << validPairs[0].size() << endl;
01570 cout << "cam1 patterns found: " << frameCounts[0].size() << endl;
01571 cout << "cam2 patterns found: " << frameCounts[1].size() << endl;
01572
01573 cout << "valid pairs: " << validPatternData[0].size() << endl;
01574 cout << "total valid patterns: " << totalValidPatterns << endl;
01575
01576
01577
01578 if ( configData.debugMode) {
01579
01580 doVerify = true;
01581
01582 int currentValidPattern = 0;
01583
01584 for(unsigned int i=0; i < validPatternData[0].size(); i++){
01585
01586 if ( validPatternData[0].at(i).patternFound && validPatternData[1].at(i).patternFound){
01587
01588 ROS_INFO("\nValid pattern pair %d out of %d : frame cam1: %d, frame cam2 %d ", currentValidPattern + 1, totalValidPatterns, validPatternData[0].at(i).patternID , validPatternData[1].at(i).patternID );
01589
01590
01591
01592 int dispFrame1 = validPatternData[0].at(i).validFrameID;
01593 int dispFrame2 = validPatternData[1].at(i).validFrameID;
01594
01595 int dispPattern1 = validPatternData[0].at(i).patternID;
01596 int dispPattern2 = validPatternData[1].at(i).patternID;
01597
01598 if (displayImages[0].at(dispFrame1).type() == CV_8UC3) {
01599 displayImages[0].at(dispFrame1).copyTo(dispMat1);
01600 } else {
01601 cvtColor(displayImages[0].at(dispFrame1), dispMat1, CV_GRAY2RGB);
01602 }
01603
01604
01605 drawChessboardCorners(dispMat1, cvSize(configData.xCount, configData.yCount), Mat(pointSets[0].at(dispPattern1)), true);
01606
01607
01608 if (displayImages[1].at(dispFrame2).type() == CV_8UC3) {
01609 displayImages[1].at(dispFrame2).copyTo(dispMat2);
01610 } else {
01611 cvtColor(displayImages[1].at(dispFrame2), dispMat2, CV_GRAY2RGB);
01612 }
01613
01614 drawChessboardCorners(dispMat2, cvSize(configData.xCount, configData.yCount), Mat(pointSets[1].at(dispPattern2)), true);
01615
01616
01617 if(currentValidPattern < totalValidPatterns - 1){
01618
01619 imshow("pattern1", dispMat1);
01620 imshow("pattern2", dispMat2);
01621
01622 ROS_INFO("%d << Press 'c' to use this keyframes or any other key to discard...", i);
01623
01624
01625 char key = waitKey();
01626 if (key == 'c'){
01627 extrinsicsPointSets[0].push_back(pointSets[0].at(validPatternData[0].at(i).patternID));
01628 extrinsicsPointSets[1].push_back(pointSets[1].at(validPatternData[1].at(i).patternID));
01629 ROS_INFO("Added patterns for valid pair %d...", currentValidPattern);
01630 }
01631
01632 currentValidPattern++;
01633
01634 } else {
01635 destroyWindow("pattern1");
01636 destroyWindow("pattern2");
01637 doVerify = false;
01638 }
01639
01640 }
01641
01642 }
01643
01644 }
01645
01646 }
01647
01648
01649 void calibratorNode::preprocessImage(Mat src, Mat &dst, double a = 1.0, double b = 0.0, bool normalize = false, bool negative = false){
01650 \
01651
01652 Mat newImage = Mat::zeros(src.size(), src.type());
01653
01654 if (src.type() == CV_8UC1){
01655
01656 for (int j = 0; j < src.rows; j++){
01657 for (int i = 0; i < src.cols; i++){
01658 newImage.at<uchar>(j,i) = saturate_cast<uchar> (a * (src.at<uchar>(j,i))+ b);
01659 }
01660 }
01661
01662 }else{
01663
01664 for (int j = 0; j < src.rows; j++){
01665 for (int i = 0; i < src.cols; i++){
01666 for (int c = 0; c< 3; c++){
01667 newImage.at<Vec3b>(j,i)[c] = saturate_cast<uchar> (a * (src.at<Vec3b>(j,i)[c])+ b);
01668 }
01669 }
01670 }
01671
01672 }
01673
01674
01675
01676 if (normalize){
01677 cvtColor(newImage, src, CV_RGB2GRAY);
01678 equalizeHist(src,newImage);
01679 }
01680
01681
01682
01683
01684 if (negative){
01685
01686 if (src.type() == CV_8UC1){
01687
01688 for (int j = 0; j < newImage.rows; j++){
01689 for (int i = 0; i < newImage.cols; i++){
01690 newImage.at<uchar>(j,i) = 255 - saturate_cast<uchar> (newImage.at<uchar>(j,i));
01691 }
01692 }
01693
01694 }else{
01695
01696 for (int j = 0; j < newImage.rows; j++){
01697 for (int i = 0; i < newImage.cols; i++){
01698 for (int c = 0; c< 3; c++){
01699 newImage.at<Vec3b>(j,i)[c] = 255 - saturate_cast<uchar> ( newImage.at<Vec3b>(j,i)[c]);
01700 }
01701 }
01702 }
01703
01704 }
01705
01706 }
01707
01708
01709
01710
01711 dst = Mat(newImage);
01712
01713 }
01714
01715
01716 void calibratorNode::updatePairs() {
01717
01718 if (configData.numCams == 1) {
01719 validSets = pointSets[0].size();
01720 }
01721
01722
01723
01724 determineValidPairs();
01725 Mat dispMat1, dispMat2;
01726
01727
01728
01729
01730
01731
01732
01733
01734
01735
01736
01737
01738
01739
01740
01741
01742
01743
01744
01745
01746
01747
01748
01749
01750
01751
01752
01753
01754
01755
01756
01757
01758
01759
01760
01761
01762
01763
01764
01765
01766
01767
01768
01769
01770
01771
01772
01773
01774
01775
01776
01777
01778
01779
01780
01781
01782
01783
01784
01785
01786
01787
01788
01789
01790
01791
01792
01793
01794
01795
01796
01797
01798
01799
01800
01801 if (validPairs[0].size() > publishCount) {
01802
01803 int currentPair = validPairs[0].size()-1;
01804
01805 if (currentPair == -1) {
01806 return;
01807 }
01808
01809
01810
01811
01812
01813 bool patternFound_1 = false, patternFound_2 = false;
01814
01815 Mat tmpMat1, tmpMat2;
01816
01817 vector<Point2f> cornerSet_1, cornerSet_2;
01818
01819
01820 if ((validPairs[0].at(currentPair) >= displayImages[0].size()) || (validPairs[1].at(currentPair) >= displayImages[1].size())) {
01821 ROS_WARN("Skipping detection because there appear to be insufficient buffered frames [(%d)(%d) : (%d)(%d)", validPairs[0].at(currentPair), displayImages[0].size(), validPairs[1].at(currentPair), displayImages[1].size());
01822 return;
01823 }
01824
01825
01826
01827 Mat grayMat1, grayMat2;
01828
01829 if (displayImages[0].at(validPairs[0].at(currentPair)).type() == CV_8UC3) {
01830 cvtColor(displayImages[0].at(validPairs[0].at(currentPair)), grayMat1, CV_RGB2GRAY);
01831 } else {
01832 grayMat1 = Mat(displayImages[0].at(validPairs[0].at(currentPair)));
01833 }
01834
01835
01836
01837 if (displayImages[1].at(validPairs[1].at(currentPair)).type() == CV_8UC3) {
01838 cvtColor(displayImages[1].at(validPairs[1].at(currentPair)), grayMat2, CV_RGB2GRAY);
01839 } else {
01840 grayMat2 = Mat(displayImages[1].at(validPairs[1].at(currentPair)));
01841 }
01842
01843
01844 switch (configData.pattType) {
01845 case CHESSBOARD_FINDER_CODE:
01846 patternFound_1 = findChessboardCorners(grayMat1, cvSize(configData.xCount, configData.yCount), cornerSet_1);
01847 patternFound_2 = findChessboardCorners(grayMat2, cvSize(configData.xCount, configData.yCount), cornerSet_2);
01848 break;
01849 case MASK_FINDER_CODE:
01850 patternFound_1 = findMaskCorners(grayMat1, cvSize(configData.xCount, configData.yCount), cornerSet_1, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG);
01851 patternFound_2 = findMaskCorners(grayMat2, cvSize(configData.xCount, configData.yCount), cornerSet_2, PATTERN_FINDER_CV_CORNER_SUBPIX_FLAG);
01852 break;
01853 case HEATED_CHESSBOARD_FINDER_CODE:
01854 invertMatIntensities(grayMat1, tmpMat1);
01855 patternFound_1 = findChessboardCorners(tmpMat1, cvSize(configData.xCount, configData.yCount), cornerSet_1);
01856
01857 invertMatIntensities(grayMat2, tmpMat2);
01858 patternFound_2 = findChessboardCorners(tmpMat2, cvSize(configData.xCount, configData.yCount), cornerSet_2);
01859
01860 break;
01861 default:
01862 patternFound_1 = findChessboardCorners(grayMat1, cvSize(configData.xCount, configData.yCount), cornerSet_1);
01863 patternFound_2 = findChessboardCorners(grayMat2, cvSize(configData.xCount, configData.yCount), cornerSet_2);
01864 break;
01865 }
01866
01867 if (patternFound_1 && patternFound_2) {
01868
01869 extrinsicsPointSets[0].push_back(cornerSet_1);
01870 extrinsicsPointSets[1].push_back(cornerSet_2);
01871 ROS_INFO("(%d) patterns found from (%d) valid frame pairs...", extrinsicsPointSets[0].size(), validPairs[0].size());
01872 pointSets[0].push_back(cornerSet_1);
01873 pointSets[1].push_back(cornerSet_2);
01874
01875
01876 } else if (patternFound_1) {
01877 pointSets[0].push_back(cornerSet_1);
01878
01879 } else if (patternFound_2) {
01880 pointSets[1].push_back(cornerSet_2);
01881
01882 }
01883
01884 if (configData.debugMode) {
01885
01886 if (displayImages[0].at(validPairs[0].at(currentPair)).type() == CV_8UC3) {
01887 displayImages[0].at(validPairs[0].at(currentPair)).copyTo(dispMat1);
01888 } else {
01889 cvtColor(displayImages[0].at(validPairs[0].at(currentPair)), dispMat1, CV_GRAY2RGB);
01890 }
01891
01892
01893 drawChessboardCorners(dispMat1, cvSize(configData.xCount, configData.yCount), Mat(cornerSet_1), patternFound_1);
01894
01895 if (msg_debug[0].width > 0) {
01896 std::copy(&(dispMat1.at<Vec3b>(0,0)[0]), &(dispMat1.at<Vec3b>(0,0)[0])+(dispMat1.cols*dispMat1.rows*dispMat1.channels()), msg_debug[0].data.begin());
01897 }
01898
01899 if (displayImages[1].at(validPairs[1].at(currentPair)).type() == CV_8UC3) {
01900 displayImages[1].at(validPairs[1].at(currentPair)).copyTo(dispMat2);
01901 } else {
01902 cvtColor(displayImages[1].at(validPairs[1].at(currentPair)), dispMat2, CV_GRAY2RGB);
01903 }
01904
01905 drawChessboardCorners(dispMat2, cvSize(configData.xCount, configData.yCount), Mat(cornerSet_2), patternFound_2);
01906
01907 if (msg_debug[1].width > 0) {
01908 std::copy(&(dispMat2.at<Vec3b>(0,0)[0]), &(dispMat2.at<Vec3b>(0,0)[0])+(dispMat2.cols*dispMat2.rows*dispMat2.channels()), msg_debug[1].data.begin());
01909 }
01910
01911
01912 debug_pub[0].publish(msg_debug[0], debug_camera_info[0]);
01913 debug_pub[1].publish(msg_debug[1], debug_camera_info[1]);
01914
01915 }
01916
01917 publishCount = validPairs[0].size();
01918
01919 }
01920
01921
01922 }
01923
01924 void calibratorNode::handle_image(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg, const unsigned int camera_index) {
01925
01926 vacantInputTime = timeElapsedMS(vacant_timer);
01927
01928 if (frameCount[camera_index] == 0) {
01929 elapsedInputTime = timeElapsedMS(elapsed_timer);
01930 }
01931
01932
01933 if (configData.verboseMode) { ROS_INFO("Handling image (camera: %d)", camera_index); }
01934
01935 unsigned int totalMinPatterns = patternIndices[0].size();
01936
01937 for (unsigned int iii = 1; iii < configData.numCams; iii++) {
01938 totalMinPatterns = min(totalMinPatterns, ((unsigned int)(patternIndices[iii].size())));
01939 }
01940
01941 if ((configData.stopCapturing) || ((((int)totalMinPatterns) > configData.maxPatterns) && (configData.maxPatterns > 0)) || ((((int)frameCount[camera_index]) > configData.maxFrames) && (configData.maxFrames > 0)) ) { stillCollecting = false; }
01942
01943 if (!stillCollecting) { return; }
01944
01945 while (!infoProcessed[camera_index]) {
01946
01947 if (configData.wantsIntrinsics) {
01948 infoProcessed[camera_index] = true;
01949 } else {
01950
01951 if (configData.intrinsicsFiles[camera_index] != "intrinsicsFile") {
01952
01953 ROS_WARN("For camera (%d), using intrinsics file (%s)", camera_index, configData.intrinsicsFiles[camera_index].c_str());
01954
01955 if (configData.intrinsicsFiles[camera_index].at(0) != '/') {
01956 configData.intrinsicsFiles[camera_index] = configData.read_addr + configData.intrinsicsFiles[camera_index];
01957 }
01958
01959
01960 try {
01961
01962 FileStorage fs(configData.intrinsicsFiles[camera_index], FileStorage::READ);
01963 fs["cameraMatrix"] >> configData.K[camera_index];
01964 fs["distCoeffs"] >> configData.distCoeffs[camera_index];
01965 fs.release();
01966
01967 ROS_INFO("Calibration data read.");
01968
01969 if (configData.K[camera_index].empty()){
01970 ROS_ERROR("Intrinsics file (%s) invalid! Please check path and filecontent...\n", configData.intrinsicsFiles[camera_index].c_str());
01971 }
01972
01973 infoProcessed[camera_index] = true;
01974
01975 } catch (int e) {
01976 ROS_ERROR("Some failure in reading in the camera parameters for camera (%d) from a file.", camera_index);
01977 }
01978
01979 } else {
01980
01981 ROS_WARN("For camera (%d), trying to extract intrinsics from msg header", camera_index);
01982
01983
01984 try {
01985
01986 configData.K[camera_index] = Mat::eye(3, 3, CV_64FC1);
01987
01988 for (unsigned int mmm = 0; mmm < 3; mmm++) {
01989 for (unsigned int nnn = 0; nnn < 3; nnn++) {
01990 configData.K[camera_index].at<double>(mmm, nnn) = info_msg->K[3*mmm + nnn];
01991 }
01992 }
01993
01994 unsigned int maxDistortionIndex;
01995 if (info_msg->distortion_model == "plumb_bob") {
01996 maxDistortionIndex = 5;
01997 } else if (info_msg->distortion_model == "rational_polynomial") {
01998 maxDistortionIndex = 8;
01999 }
02000
02001 configData.distCoeffs[camera_index] = Mat::zeros(1, maxDistortionIndex, CV_64FC1);
02002
02003 for (unsigned int iii = 0; iii < maxDistortionIndex; iii++) {
02004 configData.distCoeffs[camera_index].at<double>(0, iii) = info_msg->D[iii];
02005 }
02006
02007 ROS_INFO("Camera (%d) intrinsics:", camera_index);
02008 cout << configData.K[camera_index] << endl;
02009 cout << configData.distCoeffs[camera_index] << endl;
02010 infoProcessed[camera_index] = true;
02011 ROS_INFO("Debug marker (%d) [%d]:", camera_index, 0);
02012 } catch (...) {
02013 ROS_ERROR("Some failure in reading in the camera parameters for camera (%d) from the topic header.", camera_index);
02014 }
02015
02016 }
02017 }
02018
02019 if (configData.verboseMode) { ROS_INFO("Camera info for cam (%d) now processed.", camera_index); };
02020
02021 }
02022
02023 if (infoProcessed[camera_index]) {
02024
02025 elapsedTime = timeElapsedMS(cycle_timer);
02026
02027 if (frameCount[camera_index] > 0) {
02028 avgTime += elapsedTime;
02029 }
02030
02031 times[camera_index].push_back(info_msg->header.stamp);
02032
02033 if (configData.verboseMode) { ROS_INFO("Bridging (%d)...", camera_index); };
02034
02035 cv_ptr[camera_index] = cv_bridge::toCvCopy(msg_ptr, enc::BGR8);
02036
02037 if (configData.verboseMode) { ROS_INFO("Bridged (%d).", camera_index); };
02038
02039 Mat newImage(cv_ptr[camera_index]->image);
02040
02041
02042 Mat preDisplay, grayMat;
02043
02044 if (newImage.type() == CV_8UC3) {
02045
02046 cvtColor(newImage, grayMat, CV_RGB2GRAY);
02047
02048 if (configData.invert[camera_index]) {
02049 invertMatIntensities(grayMat, grayMat);
02050 }
02051
02052 newImage.copyTo(preDisplay);
02053
02054 } else {
02055
02056 cvtColor(newImage, preDisplay, CV_GRAY2RGB);
02057
02058 newImage.copyTo(grayMat);
02059
02060 if (configData.invert[camera_index]) {
02061 invertMatIntensities(grayMat, grayMat);
02062 }
02063
02064
02065 }
02066
02067
02068 if (configData.verboseMode) { ROS_INFO("Color and inversion processing complete (%d).", camera_index); };
02069
02070
02071 if (displayImages[camera_index].size() > 0) {
02072 if (matricesAreEqual(displayImages[camera_index].at(displayImages[camera_index].size()-1), grayMat)) {
02073 duplicateFlags[camera_index].push_back(1);
02074 ROS_WARN("Received duplicate frame at cam 1 (%d)", frameCount[0]);
02075 } else {
02076 duplicateFlags[camera_index].push_back(0);
02077 }
02078 } else {
02079 duplicateFlags[camera_index].push_back(0);
02080 imSize[camera_index] = newImage.size();
02081 }
02082
02083
02084
02085
02086
02087
02088
02089 if (duplicateFlags[camera_index].at(duplicateFlags[camera_index].size()-1) == 0) {
02090
02091 displayImages[camera_index].push_back(grayMat);
02092
02093 if (configData.debugMode) {
02094
02095 if (configData.verboseMode) { ROS_INFO("Configuring debug image (%d).", camera_index); };
02096
02097 if (msg_debug[camera_index].width == 0) {
02098 msg_debug[camera_index].width = preDisplay.cols;
02099 msg_debug[camera_index].height = preDisplay.rows;
02100 msg_debug[camera_index].encoding = "bgr8";
02101 msg_debug[camera_index].is_bigendian = false;
02102 msg_debug[camera_index].step = preDisplay.cols*3;
02103 msg_debug[camera_index].data.resize(preDisplay.cols*preDisplay.rows*preDisplay.channels());
02104 }
02105
02106 if (configData.verboseMode) { ROS_INFO("Debug image configured (%d).", camera_index); };
02107
02108 }
02109
02110 if (configData.verboseMode) { ROS_INFO("Searching for pattern (%d).", camera_index); };
02111 bool patternFound = findPattern(grayMat, cornerSet[camera_index], prevMat[camera_index]);
02112 if (configData.verboseMode) { ROS_INFO("Pattern found? (%d) [%d].", camera_index, patternFound); };
02113
02114 if (patternFound) {
02115
02116 if (cornerSet[camera_index].size() == 0) {
02117 ROS_ERROR("Says pattern found, but no points!");
02118 imshow("tmp", prevMat[camera_index]);
02119 waitKey();
02120 }
02121
02122 patternIndices[camera_index].push_back(frameCount[camera_index]);
02123 grayMat.copyTo(prevMat[camera_index]);
02124 pointSets[camera_index].push_back(cornerSet[camera_index]);
02125
02126 patternTimestamps[camera_index].push_back(info_msg->header.stamp);
02127
02128 }
02129
02130
02131
02132 if (configData.debugMode) {
02133
02134 Mat dispMat;
02135
02136 preDisplay.copyTo(dispMat);
02137
02138 if (configData.verboseMode) { ROS_INFO("Drawing pattern (%d).", camera_index); };
02139 drawChessboardCorners(dispMat, cvSize(configData.xCount, configData.yCount), Mat(cornerSet[camera_index]), patternFound);
02140 if (configData.verboseMode) { ROS_INFO("Pattern drawn (%d).", camera_index); };
02141
02142
02143
02144
02145 if (configData.verboseMode) { ROS_INFO("About to copy (%d).", camera_index); };
02146
02147 std::copy(&(dispMat.at<Vec3b>(0,0)[0]), &(dispMat.at<Vec3b>(0,0)[0])+(dispMat.cols*dispMat.rows*dispMat.channels()), msg_debug[camera_index].data.begin());
02148
02149 if (configData.verboseMode) { ROS_INFO("Preparing to publish (%d).", camera_index); };
02150 debug_pub[camera_index].publish(msg_debug[camera_index], debug_camera_info[camera_index]);
02151 if (configData.verboseMode) { ROS_INFO("Published (%d).", camera_index); };
02152 }
02153
02154 frameCount[camera_index]++;
02155
02156 } else {
02157 ROS_ERROR("Skipping frame because it is a duplicate...");
02158 }
02159
02160
02161 }
02162
02163
02164
02165 }
02166
02167 calibratorNode::calibratorNode(ros::NodeHandle& nh, calibratorData startupData) {
02168
02169 alpha = 0.00;
02170
02171 vacantInputTime = timeElapsedMS(vacant_timer);
02172 vacantInputTime = 0.0;
02173 elapsedInputTime = timeElapsedMS(elapsed_timer);
02174 elapsedInputTime = 0.0;
02175 timer = nh.createTimer(ros::Duration(DEFAULT_TIMER_PERIOD), &calibratorNode::timerCallback, this);
02176
02177 alphaChanged = true;
02178
02179 publishCount = 0;
02180
02181 readyForOutput = false;
02182
02183 topValidHeight = 0;
02184 botValidHeight = 65535;
02185
02186
02187
02188 ref = &nh;
02189
02190 stillCollecting = true;
02191 doVerify = false;
02192
02193 default_R = cv::Mat::eye(3,3,CV_64FC1);
02194
02195 avgTime = 0.0;
02196 frameTrackingTime = 0.0;
02197
02198
02199
02200 for (unsigned int iii = 0; iii < MAX_ALLOWABLE_CAMS; iii++) {
02201 frameCount[iii] = 0;
02202 totalFrameCount[iii] = 0;
02203 infoProcessed[iii] = false;
02204 checkIndex[iii] = 0;
02205 }
02206
02207 undistortionCount = 0;
02208 rectificationCount = 0;
02209
02210 sprintf(nodeName, "%s", ros::this_node::getName().c_str());
02211
02212 configData = startupData;
02213
02214 if (configData.calibType == "single") {
02215
02216 sprintf(debug_pub_name[0], "/thermalvis%s/image_col", nodeName);
02217
02218 topic[0] = nh.resolveName(configData.video_stream);
02219
02220 ROS_INFO("Subscribing to topic (%s)", topic[0].c_str());
02221
02222 image_transport::ImageTransport it(nh);
02223
02224 camera_sub[0] = it.subscribeCamera(topic[0], 1, boost::bind(&calibratorNode::handle_image, this, _1, _2, 0));
02225
02226 if (configData.debugMode) {
02227 debug_pub[0] = it.advertiseCamera(debug_pub_name[0], 1);
02228 ROS_INFO("Advertising tracker debugging video (%s)", debug_pub_name[0]);
02229 }
02230
02231 } else if (configData.calibType == "stereo") {
02232
02233 sprintf(debug_pub_name[0], "thermalvis/calib_left/image_col");
02234 sprintf(debug_pub_name[1], "thermalvis/calib_right/image_col");
02235
02236 topic[0] = nh.resolveName(configData.video_stream);
02237 topic[1] = nh.resolveName(configData.video_stream_secondary);
02238
02239 if (configData.verboseMode) { ROS_INFO("(cam 1): Subscribing to topic (%s)...", topic[0].c_str()); }
02240 if (configData.verboseMode) { ROS_INFO("(cam 2): Subscribing to topic (%s)...", topic[1].c_str()); }
02241
02242 image_transport::ImageTransport it(nh);
02243
02244 camera_sub[0] = it.subscribeCamera(topic[0], 1, boost::bind(&calibratorNode::handle_image, this, _1, _2, 0));
02245 camera_sub[1] = it.subscribeCamera(topic[1], 1, boost::bind(&calibratorNode::handle_image, this, _1, _2, 1));
02246
02247 if (configData.verboseMode) { ROS_INFO("Subscribed to topics..."); }
02248
02249 if (configData.debugMode) {
02250 debug_pub[0] = it.advertiseCamera(debug_pub_name[0], 1);
02251 debug_pub[1] = it.advertiseCamera(debug_pub_name[1], 1);
02252 ROS_INFO("Publishing debug topics at (%s) & (%s)", debug_pub_name[0], debug_pub_name[1]);
02253 }
02254
02255 }
02256
02257
02258
02259
02260 for (unsigned int iii = 0; iii < configData.yCount; iii++) {
02261 for (unsigned int jjj = 0; jjj < configData.xCount; jjj++) {
02262 row.push_back(Point3f(((float)iii)*configData.gridSize/1000.0, ((float)jjj)*configData.gridSize/1000.0, 0.0));
02263 }
02264 }
02265
02266 ROS_INFO("Establishing server callback...");
02267 f = boost::bind (&calibratorNode::serverCallback, this, _1, _2);
02268 server.setCallback (f);
02269
02270 }
02271
02272 void calibratorNode::timerCallback(const ros::TimerEvent&) {
02273
02274 int totalFrameCount = 0;
02275 for (unsigned int iii = 0; iii < configData.numCams; iii++) {
02276 totalFrameCount += frameCount[iii];
02277 }
02278 if (totalFrameCount == 0) {
02279 vacantInputTime = timeElapsedMS(vacant_timer);
02280 elapsedInputTime = timeElapsedMS(elapsed_timer);
02281 return;
02282 }
02283
02284 vacantInputTime = timeElapsedMS(vacant_timer, 0);
02285 elapsedInputTime = timeElapsedMS(elapsed_timer, 0);
02286
02287 if ((configData.autoTimeout != 0.0) && (vacantInputTime > 1000.0*configData.autoTimeout)) {
02288 ROS_WARN("No new frames in (%f) seconds, terminating collection procedure..", vacantInputTime/1000.0);
02289 stillCollecting = false;
02290 }
02291
02292 if ((elapsedInputTime/1000.0 > configData.maxTime) && (configData.maxTime > 0) ) {
02293 ROS_WARN("Maximum data collection time of (%f) seconds reached, terminating collection procedure..", configData.maxTime);
02294 stillCollecting = false;
02295 }
02296
02297 }
02298
02299 void calibratorNode::serverCallback(thermalvis::calibratorConfig &config, uint32_t level) {
02300
02301 configData.verboseMode = config.verboseMode;
02302
02303 configData.drawGrids = config.drawGrids;
02304
02305 configData.maxFrames = config.maxFrames;
02306 configData.maxPatterns = config.maxPatterns;
02307 configData.maxTime = config.maxTime;
02308 configData.maxCandidates = config.maxCandidates;
02309 configData.maxTests = config.maxTests;
02310
02311 configData.maxInterpolationTimegap = config.maxInterpolationTimegap;
02312 configData.maxInterpolationMotion = config.maxInterpolationMotion;
02313
02314 configData.generateFigures = config.generateFigures;
02315
02316 configData.autoTimeout = config.autoTimeout;
02317
02318 if ((config.autoAlpha != configData.autoAlpha) || ((!config.autoAlpha) && (config.alpha != configData.alpha))) {
02319
02320 configData.autoAlpha = config.autoAlpha;
02321 configData.alpha = config.alpha;
02322
02323 alphaChanged = true;
02324
02325 if (configData.verboseMode) { ROS_WARN("About to try and update maps..."); }
02326 if (readyForOutput) {
02327 if (configData.verboseMode) { ROS_WARN("Updating maps..."); }
02328
02329 for (unsigned int iii = 0; iii < configData.numCams; iii++) {
02330 updateIntrinsicMap(iii);
02331 }
02332
02333 }
02334
02335 } else {
02336 configData.alpha = config.alpha;
02337 }
02338
02339 configData.trackingMode = config.trackingMode;
02340
02341
02342
02343 configData.flowThreshold = config.flowThreshold;
02344
02345 configData.maxFrac = config.maxFrac;
02346
02347
02348 configData.errorThreshold = config.errorThreshold;
02349
02350 configData.stopCapturing = config.stopCapturing;
02351
02352 configData.invert[0] = config.invertPrimary;
02353 configData.adjustMSER[0] = config.adjustMSER_primary;
02354 configData.delta[0] = config.delta_primary;
02355 configData.maxVar[0] = config.maxVar_primary;
02356 configData.minDiv[0] = config.minDiv_primary;
02357 configData.areaThreshold[0] = config.areaThreshold_primary;
02358
02359 configData.invert[1] = config.invertSecondary;
02360 configData.adjustMSER[1] = config.adjustMSER_secondary;
02361 configData.maxVar[1] = config.maxVar_secondary;
02362 configData.delta[1] = config.delta_secondary;
02363 configData.areaThreshold[1] = config.areaThreshold_secondary;
02364 configData.minDiv[1] = config.minDiv_secondary;
02365
02366 }
02367
02368 void calibratorNode::set_ready_for_output() {
02369 readyForOutput = true;
02370 }