thermalvis: thermalvis: flow.cpp Source File

00001 
00005 #include "flow.hpp"
00006 
00007 int main(int argc, char** argv) {
00008         
00009         ROS_INFO("Node launched.");
00010                 
00011         ros::init(argc, argv, "flow");
00012         
00013         ros::NodeHandle private_node_handle("~");
00014         
00015         trackerData startupData;
00016                 
00017         bool inputIsValid = startupData.obtainStartingData(private_node_handle);
00018         
00019         startupData.read_addr = argv[0];
00020         startupData.read_addr = startupData.read_addr.substr(0, startupData.read_addr.size()-8);
00021         
00022         if (!inputIsValid) {
00023                 ROS_ERROR("Configuration invalid.");
00024                 return -1;
00025         }
00026                 
00027         ROS_INFO("Startup data processed.");
00028         
00029         ros::NodeHandle nh;
00030         
00031         boost::shared_ptr < featureTrackerNode > tracker_node (new featureTrackerNode (nh, startupData));
00032         
00033         globalNodePtr = &tracker_node;
00034         
00035         signal(SIGINT, mySigintHandler);
00036         
00037         ROS_INFO("Node configured.");
00038         
00039         while (!::wantsToShutdown) {
00040                 ros::spinOnce();
00041         }
00042         
00043         
00044         return 0;
00045         
00046 }
00047 
00048 void mySigintHandler(int sig) {
00049         wantsToShutdown = true;
00050         ROS_WARN("Requested shutdown... terminating feeds...");
00051         
00052         (*globalNodePtr)->prepareForTermination();
00053 }
00054 
00055 bool trackerData::obtainStartingData(ros::NodeHandle& nh) {
00056         
00057         numDetectors = 0;
00058         
00059         softSync = false;
00060         
00061         soft_diff_limit = 5000000;
00062         
00063         nh.param<double>("syncDiff", syncDiff, 0.005);
00064         
00065         nh.param<std::string>("video_stream", video_stream, "video_stream");
00066         
00067         if (video_stream != "video_stream") {
00068                 ROS_INFO("Video stream (%s) selected.", video_stream.c_str());
00069         } else {
00070                 video_stream = "thermalvis/streamer/";
00071                 ROS_WARN("No video stream supplied; assuming (%s)", video_stream.c_str());
00072                 //return false;
00073         }
00074         
00075         //nh.param<int>("maxFeatures", maxFeatures, 100);
00076         
00077         nh.param<std::string>("video_sequence", video_sequence, "video_sequence");
00078         
00079         if (video_sequence != "video_sequence") {
00080                 ROS_INFO("Video sequence (%s) selected.", video_sequence.c_str());
00081         } else {
00082                 video_sequence = "image_mono";
00083                 ROS_WARN("No video topic supplied; assuming (%s)", video_sequence.c_str());
00084                 //return false;
00085         }
00086 
00087         nh.param<bool>("softSync", softSync, false);
00088 
00089         camera_parameters = video_stream + "/" + "camera_info";
00090         
00091         //nh.param<bool>("debug_mode", debugMode, false);
00092         
00093         nh.param<bool>("outputTrackCount", outputTrackCount, false);
00094         
00095         //HGH/*
00096         
00097         for (int iii = 0; iii < MAX_DETECTORS; iii++) {
00098                 
00099                 char detector_tag[256], sensitivity_tag[256], method_tag[256], descriptor_tag[256];
00100                 
00101                 sprintf(detector_tag, "detector_%d", iii + 1);
00102                 sprintf(sensitivity_tag, "sensitivity_%d", iii + 1);
00103                 sprintf(method_tag, "method_%d", iii + 1);
00104                 sprintf(descriptor_tag, "descriptor_%d", iii + 1);
00105                 
00106                 //printf("%s << detector_tag = %s\n", __FUNCTION__, detector_tag);
00107                 
00108                 
00109                 if (iii == 0) {
00110                         nh.param<std::string>(detector_tag, detector[iii], "GFTT");
00111                 } else {
00112                         nh.param<std::string>(detector_tag, detector[iii], "NONE");
00113                 }
00114                 
00115                 //printf("%s << detector[%d] = %s\n", __FUNCTION__, iii, detector[iii].c_str());
00116                 
00117                 if ((detector[iii] == "SURF") || (detector[iii] == "FAST") || (detector[iii] == "GFTT") || (detector[iii] == "HARRIS") || (detector[iii] == "ORB") || (detector[iii] == "STAR")) {
00118                         ROS_INFO("detector [%d]: %s", iii, detector[iii].c_str());
00119                 } else if (detector[iii] == "NONE"){
00120                         // printf("%s << No [%d] detector requested.\n", __FUNCTION__, iii);
00121                         break;
00122                 } else {
00123                         ROS_INFO("ERROR! Detector (%s) not recognized.", detector[iii].c_str());
00124                         return false;
00125                 }
00126                 
00127                 nh.param<double>(sensitivity_tag, sensitivity[iii], 0.2);
00128                 
00129                 ROS_INFO("detector [%d] sensitivity: %f", iii, sensitivity[iii]);
00130                 
00131                 nh.param<std::string>(method_tag, method[iii], "match");
00132                 
00133                 if ((method[iii] == "match") || (method[iii] == "track")) {
00134                         ROS_INFO("detector [%d] matching method: %s", iii, method[iii].c_str());
00135                         if (method[iii] == "match") {
00136                                 method_match[iii] = true;
00137                         } else {
00138                                 method_match[iii] = false;
00139                         }
00140                 } else {
00141                         ROS_INFO("ERROR! detector [%d] matching method (%s) not recognized.", iii, method[iii].c_str());
00142                         return false;
00143                 }
00144                 
00145                 nh.param<std::string>(descriptor_tag, descriptor[iii], "BRIEF");
00146                 
00147                 if (method_match[iii]) {
00148                         if ((descriptor[iii] == "SURF") || (descriptor[iii] == "ORB") || (descriptor[iii] == "BRIEF") || (descriptor[iii] == "SIFT")) {
00149                                 ROS_INFO("descriptor [%d]: %s", iii, descriptor[iii].c_str());
00150                         } else {
00151                                 ROS_INFO("ERROR! descriptor [%d] (%s) not recognized.", iii, descriptor[iii].c_str());
00152                                 return false;
00153                         }
00154                 }
00155                 
00156                 numDetectors++;
00157         }
00158         
00159         
00160         
00161         ROS_INFO("[%d] detectors to be implemented.", numDetectors);
00162         
00163         //*/
00164         
00165         return true;
00166 }
00167 
00168 void trackerData::initializeDescriptors(Ptr<DescriptorExtractor> *desc, Ptr<DescriptorExtractor> *hom) {
00169         
00170         for (unsigned int iii = 0; iii < numDetectors; iii++) {
00171                 if (descriptor[iii] == "SURF") {
00172                         ROS_ERROR("SURF has been deactivated due to copyright protection!");
00173                         //desc[iii] = new SurfDescriptorExtractor( );
00174                 } else if (descriptor[iii] == "SIFT") {
00175                         ROS_ERROR("SIFT has been deactivated due to copyright protection!");
00176                         //desc[iii] = new SiftDescriptorExtractor( );
00177                 } else if (descriptor[iii] == "BRIEF") {
00178                         desc[iii] = new BriefDescriptorExtractor( );
00179                 } else if (descriptor[iii] == "ORB") {
00180                         desc[iii] = new OrbDescriptorExtractor( );
00181                 }
00182         }
00183         
00184         hom[0] = new BriefDescriptorExtractor( );
00185         
00186 }
00187 
00188 int featureTrackerNode::publish_tracks(ros::Publisher *pub_message) {
00189         
00190         averageTrackLength = 0.0;
00191         
00192         int publishedTrackCount = 0;
00193         
00194         thermalvis::feature_tracks msg;
00195 
00196         msg.source = configData.video_stream;
00197         
00198         vector<unsigned int> tracksToInclude;
00199         for (unsigned int iii = 0; iii < featureTrackVector.size(); iii++) {
00200                 
00201                 if (featureTrackVector.at(iii).locations.at(featureTrackVector.at(iii).locations.size()-1).imageIndex == (frameCount-1)) {
00202                         tracksToInclude.push_back(iii);
00203                         publishedTrackCount++;
00204                         averageTrackLength += featureTrackVector.at(iii).locations.size();
00205                 }
00206                 
00207         }
00208         
00209         averageTrackLength /= ((double) publishedTrackCount);
00210         
00211         unsigned int projCount = 0;
00212         for (unsigned int iii = 0; iii < tracksToInclude.size(); iii++) {
00213                 projCount += std::min(configData.maxProjectionsPerFeature, ((int)featureTrackVector.at(tracksToInclude.at(iii)).locations.size()));
00214         }
00215         
00216         //ROS_WARN("Publishing (%d) projections from (%d) tracks", projCount, tracksToInclude.size());
00217         
00218         msg.projection_count = projCount;
00219 
00220         Mat trackMatrix;
00221         
00222         if (configData.debugMode) {
00223                 if (createTrackMatrix(featureTrackVector, trackMatrix)) {
00224                     //HGH
00225                     //imshow("trackMatrix", trackMatrix);
00226                     imshow("trackMatrix " + configData.video_stream, trackMatrix);
00227                         waitKey(1);
00228                 }               
00229         }
00230 
00231         
00232         vector<unsigned int> currentIndices;
00233         vector<Point2f> currentProjections;
00234         
00235         msg.cameras.clear();
00236         msg.indices.clear();
00237         msg.projections_x.clear();
00238         msg.projections_y.clear();
00239         
00240         for (unsigned int iii = 0; iii < tracksToInclude.size(); iii++) {
00241                 
00242                 unsigned int startIndex = featureTrackVector.at(tracksToInclude.at(iii)).locations.size() - std::min(configData.maxProjectionsPerFeature, ((int)featureTrackVector.at(tracksToInclude.at(iii)).locations.size()));
00243                 
00244                 for (unsigned int jjj = startIndex; jjj < featureTrackVector.at(tracksToInclude.at(iii)).locations.size(); jjj++) {
00245                 
00246                         msg.indices.push_back(tracksToInclude.at(iii));
00247                         msg.cameras.push_back(featureTrackVector.at(tracksToInclude.at(iii)).locations.at(jjj).imageIndex);
00248                         msg.projections_x.push_back(((double) featureTrackVector.at(tracksToInclude.at(iii)).locations.at(jjj).featureCoord.x));
00249                         msg.projections_y.push_back(((double) featureTrackVector.at(tracksToInclude.at(iii)).locations.at(jjj).featureCoord.y));
00250                 }
00251                 
00252         }
00253         
00254         pub_message->publish(msg);
00255         
00256         return publishedTrackCount;
00257 }
00258 
00259 void featureTrackerNode::handle_camera(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg) {
00260         if (configData.softSync) {
00261                 return;
00262         }
00263         
00264         while (!infoProcessed) { 
00265                 process_info(info_msg);
00266         }
00267         
00268         
00269         debug_camera_info = (*info_msg);
00270         cv_ptr = cv_bridge::toCvCopy(msg_ptr, enc::BGR8);                                       // For some reason it reads as BGR, not gray
00271         msg_debug.header.stamp = info_msg->header.stamp;
00272         act_on_image();
00273         
00274 }
00275 
00276 void featureTrackerNode::process_info(const sensor_msgs::CameraInfoConstPtr& info_msg) {
00277         
00278         try     {
00279 
00280                 // Read in camera matrix
00281                 configData.cameraData.K = Mat::eye(3, 3, CV_64FC1);
00282                 
00283                 for (unsigned int mmm = 0; mmm < 3; mmm++) {
00284                                 for (unsigned int nnn = 0; nnn < 3; nnn++) {
00285                                                 configData.cameraData.K.at<double>(mmm, nnn) = info_msg->K[3*mmm + nnn];
00286                                 }
00287                 }
00288                 
00289                 configData.cameraData.cameraSize.width = info_msg->width;
00290                 configData.cameraData.cameraSize.height = info_msg->height;
00291                 
00292                 distanceConstraint = (min(configData.cameraData.cameraSize.width, configData.cameraData.cameraSize.height) * configData.maxFrac) + ((((int) (max(configData.cameraData.cameraSize.width, configData.cameraData.cameraSize.height) * configData.maxFrac)) + 1) % 2);
00293                 
00294                 //ROS_INFO("Distance constraint calculated as (%d)", distanceConstraint);
00295                 
00296                 //ROS_INFO("(%d, %d)\n", configData.cameraData.cameraSize.width, configData.cameraData.cameraSize.height);
00297                 
00298                 unsigned int maxDistortionIndex;
00299                 if (info_msg->distortion_model == "plumb_bob") {
00300                                 maxDistortionIndex = 5;
00301                 } else if (info_msg->distortion_model == "rational_polynomial") {
00302                                 maxDistortionIndex = 8;
00303                 }
00304                 
00305                 configData.cameraData.distCoeffs = Mat::zeros(1, maxDistortionIndex, CV_64FC1);
00306                 
00307                 for (unsigned int iii = 0; iii < maxDistortionIndex; iii++) {
00308                                 configData.cameraData.distCoeffs.at<double>(0, iii) = info_msg->D[iii];
00309                 }
00310                 
00311                 cout << configData.cameraData.distCoeffs << endl;
00312                 
00313                 configData.cameraData.newCamMat = Mat::zeros(3, 3, CV_64FC1);
00314                 
00315                 Rect validPixROI;;
00316                 
00317                 bool centerPrincipalPoint = true;
00318                 
00319                 configData.cameraData.newCamMat = getOptimalNewCameraMatrix(configData.cameraData.K, configData.cameraData.distCoeffs, configData.cameraData.cameraSize, DEFAULT_ALPHA, configData.cameraData.cameraSize, &validPixROI, centerPrincipalPoint);
00320                 
00321                 //double optimalRatio = validPixROI.width / validPixROI.height;
00322                 //double desiredRatio = configData.cameraData.cameraSize.width / configData.cameraData.cameraSize.height;
00323                 
00324                 double expansionFactor = std::min(((double) validPixROI.width) / ((double) configData.cameraData.cameraSize.width), ((double) validPixROI.height) / ((double) configData.cameraData.cameraSize.height));
00325                 
00326                 configData.cameraData.expandedSize = Size(((double) configData.cameraData.cameraSize.width) / expansionFactor, ((double) configData.cameraData.cameraSize.height) / expansionFactor);
00327                 
00328                 //ROS_INFO("originalSize = (%d, %d)\n", configData.cameraData.cameraSize.width, configData.cameraData.cameraSize.height);
00329                 //ROS_INFO("correctedSize = (%d, %d)\n", configData.cameraData.expandedSize.width, configData.cameraData.expandedSize.height);
00330                 
00331                 configData.cameraData.K.copyTo(configData.cameraData.Kx);
00332                 
00333                 configData.cameraData.Kx.at<double>(0,0) /= expansionFactor;
00334                 configData.cameraData.Kx.at<double>(0,2) /= expansionFactor;
00335                 configData.cameraData.Kx.at<double>(1,1) /= expansionFactor;
00336                 configData.cameraData.Kx.at<double>(1,2) /= expansionFactor;
00337                 
00338                 configData.cameraData.expandedCamMat = getOptimalNewCameraMatrix(configData.cameraData.Kx, configData.cameraData.distCoeffs, configData.cameraData.expandedSize, DEFAULT_ALPHA, configData.cameraData.expandedSize, &validPixROI, centerPrincipalPoint);
00339                 
00340                 msg_debug.width = configData.cameraData.cameraSize.width; 
00341                 msg_debug.height = configData.cameraData.cameraSize.height;
00342                 msg_debug.encoding = "bgr8";
00343                 msg_debug.is_bigendian = false;
00344                 msg_debug.step = configData.cameraData.cameraSize.width*3;
00345                 msg_debug.data.resize(configData.cameraData.cameraSize.width*configData.cameraData.cameraSize.height*3);
00346                 
00347                 infoProcessed = true;
00348                 
00349                 ROS_INFO("Image info processed.");
00350                 
00351         } catch (...) {
00352                 ROS_ERROR("Some failure in reading in the camera parameters...");
00353         }
00354                 
00355 }
00356 
00357 
00358 void featureTrackerNode::handle_info(const sensor_msgs::CameraInfoConstPtr& info_msg) {
00359         
00360         if (!configData.softSync) {
00361                 return;
00362         }
00363         
00364         info_time = ros::Time::now();
00365         original_time = info_msg->header.stamp;
00366         
00367         while (!infoProcessed) { 
00368                 process_info(info_msg);
00369         }
00370         
00371         
00372         debug_camera_info = (*info_msg);
00373         
00374         if (abs(image_time.toNSec() - info_time.toNSec()) < configData.soft_diff_limit) {
00375                 //ROS_ERROR("handle_info : image_time == info_time");
00376                 image_time = dodgeTime;
00377                 msg_debug.header.stamp = info_msg->header.stamp;
00378                 act_on_image();
00379         }
00380         
00381 }
00382 
00383 void featureTrackerNode::handle_image(const sensor_msgs::ImageConstPtr& msg_ptr) {
00384         
00385         if (!configData.softSync) {
00386                 return;
00387         }
00388         
00389         testTime = timeElapsedMS(test_timer, true);
00390                         
00391         while (!infoProcessed) { }
00392 
00393             //HGH
00394             ros::spinOnce();
00395         if (abs(image_time.toNSec() - info_time.toNSec()) < configData.soft_diff_limit) {
00396                 // ROS_ERROR("handle_image : image_time == info_time");
00397                 
00398                 image_time = dodgeTime;
00399                 act_on_image();
00400         }
00401         
00402 }
00403 
00404 
00405 void featureTrackerNode::act_on_image() {
00406         Mat newImage(cv_ptr->image);
00407         
00408         if ((newImage.type() == CV_16UC3) || (newImage.type() == CV_16UC1)) {
00409                 ROS_ERROR("This node should only receive 8-bit images..");
00410                 return;
00411         } else if (newImage.type() == CV_8UC3) {
00412                 cvtColor(newImage, grayImage, CV_RGB2GRAY);
00413         } else if (newImage.type() == CV_8UC1) {
00414                 grayImage = newImage;
00415         }
00416         
00417         testTime = timeElapsedMS(test_timer, true);
00418         //ROS_WARN("Handle time: (%f)", testTime);
00419         
00420         if (frameCount > 0) {
00421                 if (matricesAreEqual(grayImage, grayImageBuffer[(frameCount-1) % MAXIMUM_FRAMES_TO_STORE])) {
00422                         
00423                         //ROS_WARN("Matrices are equal! (%d)", frameCount);
00424                         
00425                         elapsedTime = timeElapsedMS(cycle_timer, false);
00426                         
00427                         if ((elapsedTime > configData.delayTimeout*MS_PER_SEC) && !undergoingDelay) {
00428                                 // Call the NUC process
00429                                 handle_delay();
00430                         }
00431                         
00432                         return;
00433                 }
00434         }
00435         
00436         testTime = timeElapsedMS(test_timer);
00437         //ROS_WARN("After equality test: (%f)", testTime);
00438         
00439         elapsedTime = timeElapsedMS(cycle_timer);
00440         
00441         
00442         grayImage.copyTo(grayImageBuffer[(frameCount) % MAXIMUM_FRAMES_TO_STORE]);
00443         
00444         //mappedImage = grayImage;
00445         //mappedImage.copyTo(mappedImageBuffer[(frameCount) % MAXIMUM_FRAMES_TO_STORE]);
00446         
00447         testTime = timeElapsedMS(test_timer);
00448         //ROS_WARN("After remapping: (%f)", testTime);
00449         
00450         // Now this is where you should prepare the image pair for tracking...
00451         
00452         
00453         
00454         Mat tmpX, tmpY;
00455         
00456         #pragma omp parallel for
00457         for (unsigned int ppp = 0; ppp < 2; ppp++) {
00458                                         
00459                 if (frameCount < (configData.lossGaps[ppp]) || ((frameCount % configData.lossGaps[ppp]) != 0)) {
00460                         continue;
00461                 }
00462                 
00463                 grayImageBuffer[((frameCount)-configData.lossGaps[ppp]) % MAXIMUM_FRAMES_TO_STORE].copyTo(olderImages[(frameCount) % MAXIMUM_FRAMES_TO_STORE][ppp]);
00464                 
00465                 //normalize_16(tmpX, mappedImageBuffer[((frameCount)-lossGaps[ppp]) % MAXIMUM_FRAMES_TO_STORE], minVal, maxVal);
00466                 //GaussianBlur(tmpX, tmpY, ksize, blurSigma);
00467                 //tmpY.convertTo(olderImages[(frameCount) % MAXIMUM_FRAMES_TO_STORE][ppp], CV_8UC1, 1.0 / 257.0);
00468                 
00469         }
00470         
00471         testTime = timeElapsedMS(test_timer, true);
00472         //ROS_WARN("Historical prep time: (%f)", testTime);
00473                 
00474         if (configData.debugMode) {
00475                 // downsampleToColor(grayImageBuffer[(frameCount-1) % MAXIMUM_FRAMES_TO_STORE], drawImage);
00476                 //adaptiveDownsample(grayImageBuffer[(frameCount-1) % MAXIMUM_FRAMES_TO_STORE], dispMat, STANDARD_NORM_MODE);
00477                 
00478                 
00479                 //double vals[2], percentiles[2], thresh = 0.005;
00480                 //percentiles[0] = thresh;
00481                 //percentiles[1] = 1 - thresh;
00482                 
00483                 //findPercentiles(grayImageBuffer[(frameCount) % MAXIMUM_FRAMES_TO_STORE], vals, percentiles, 2);
00484                 
00485                 //displayImage = Mat(grayImageBuffer[(frameCount) % MAXIMUM_FRAMES_TO_STORE].size(), CV_8UC1);
00486                 
00487                 //double v;
00488                 
00489                 //for (int iii = 0; iii < displayImage.rows; iii++) {
00490                         //for (int jjj = 0; jjj < displayImage.cols; jjj++) {
00491                                 
00492                                 //v = (double) grayImageBuffer[(frameCount) % MAXIMUM_FRAMES_TO_STORE].at<unsigned char>(iii,jjj);
00493                                 
00494                                 //v -= vals[0];
00495                                 //v *= (255.0/(vals[1]-vals[0]));
00496                                 //v = max(0.0, v);
00497                                 //v = min(255.0, v);
00498                                 
00499                                 //displayImage.at<unsigned char>(iii,jjj) = (unsigned char) v;
00500                                 
00501                         //}
00502                 //}
00503                 
00504                 
00505                 displayImage = grayImageBuffer[(frameCount) % MAXIMUM_FRAMES_TO_STORE];
00506                 
00507                 //normalize(grayImageBuffer[(frameCount) % MAXIMUM_FRAMES_TO_STORE], displayImage, -255*vals[0], 255 + 255*(vals[1]-255), NORM_MINMAX);
00508                 //normalize(grayImageBuffer[(frameCount) % MAXIMUM_FRAMES_TO_STORE], displayImage, 0, 255, NORM_MINMAX);
00509                 //straightCLAHE(grayImageBuffer[(frameCount) % MAXIMUM_FRAMES_TO_STORE], displayImage, 0.2);
00510                 
00511                 cvtColor(displayImage, drawImage, CV_GRAY2RGB);
00512                 
00513                 
00514                 //displayImage.copyTo(drawImage);
00515         }
00516         
00517         frameCount++;
00518 
00519         if (undergoingDelay) {
00520                 handle_very_new();              // Use this to get homography between images to guide continued tracking
00521         }
00522         
00523         testTime = timeElapsedMS(test_timer, true);
00524         //ROS_WARN("Detection time: (%f)", testTime);
00525 }
00526 
00527 void featureTrackerNode::features_loop(const ros::TimerEvent& event) {
00528         
00529         int successfullyTrackedFeatures = 0;
00530         int newlyDetectedFeatures = 0;
00531         int newlyRecoveredFeatures = 0;
00532         int discardedNewFeatures = 0;
00533         int lostTrackCount = 0;
00534         
00535         if (readyFrame >= frameCount) {
00536                 return;
00537         }
00538         
00539         //ROS_INFO("Entered features loop for frame (%d)", frameCount);
00540         
00541         vector<Point2f> finalPoints1, finalPoints2;
00542         vector<Point2f> recPoints1, recPoints2;
00543         
00544         testTime = timeElapsedMS(test_timer, true);
00545         //ROS_WARN("?NUC time: (%f)", testTime);
00546         
00547         if (readyFrame > 0) {
00548                 
00549                 //ROS_INFO("Debug (%d)", 0);
00550                 
00551                 for (unsigned int kkk = 0; kkk < configData.numDetectors; kkk++) {
00552                         
00553                         //ROS_INFO("Debug (%d)", 1);
00554 
00555                         startingPoints[kkk].clear();
00556                         startingPoints[kkk].insert(startingPoints[kkk].end(), finishingPoints[kkk].begin(), finishingPoints[kkk].end());
00557                         
00558                         //ROS_INFO("Points carried over from previous run: (%d)", startingPoints[kkk].size()); 
00559                         
00560                         if (H12.rows != 0) {
00561                                 
00562                                 ROS_INFO("Shifting points with homography... (%d):(%d)", kkk, finishingPoints[kkk].size());
00563                                 
00564                                 // Need to shift finishing points now using homography so that they can form estimates                  
00565                                 transformPoints(finishingPoints[kkk], H12);
00566                                 
00567                                 // Then debug publish warped image with these new finishing points
00568                                 
00569                                 /*
00570                                 Mat im1;
00571                                 //warpPerspective(grayImageBuffer[(frameCount-2) % MAXIMUM_FRAMES_TO_STORE], im1, H12, grayImageBuffer[(frameCount-1) % MAXIMUM_FRAMES_TO_STORE].size());
00572                                 grayImageBuffer[(frameCount-1) % MAXIMUM_FRAMES_TO_STORE].copyTo(im1);
00573                                 Mat im1_col;
00574                                 cvtColor(im1, im1_col, CV_GRAY2RGB);
00575                                 displayKeypoints(im1_col, finishingPoints[kkk], im1_col, keyDrawingColors[kkk], 0);
00576                                 
00577                                 while (1) {
00578                                         std::copy(&(im1_col.at<unsigned char>(0,0)), &(im1_col.at<unsigned char>(0,0))+(drawImage.cols*drawImage.rows*drawImage.channels()), msg_debug.data.begin());                           
00579                                         debug_pub.publish(msg_debug, debug_camera_info);
00580                                         waitKey(500);
00581                                 }
00582                                 */
00583                                 
00584                         } else {
00585                                 finishingPoints[kkk].clear();
00586                         }
00587                         
00588                         //ROS_INFO("Debug (%d)", 2);
00589 
00590                         // =========================================================
00591                         // TRACK RECOVERY
00592                         // =========================================================
00593                         bool performedRecovery = false;
00594                         for (unsigned int ppp = 0; ppp < 2; ppp++) {
00595  
00596                                 if ((((int) startingPoints[kkk].size()) >= ((int) configData.maxFeatures)) || (((int) readyFrame) < ((int) (configData.lossGaps[ppp]))) || ((((int) readyFrame) % ((int) configData.lossGaps[ppp])) != 0)) {
00597                                         continue;
00598                                 }
00599                                 
00600                                 performedRecovery = true;
00601                                 
00602                                 //ROS_INFO("Entered track recovery block: (readyFrame = %d)", readyFrame);
00603                                 
00604                                 historicalPoints[kkk].clear();
00605                                 recoveredPoints[kkk].clear();
00606                                 assignHistoricalPoints(featureTrackVector, (readyFrame)-configData.lossGaps[ppp]-1, (readyFrame-1), historicalPoints[kkk]);
00607                                 
00608                                 if (historicalPoints[kkk].size() > 0) {
00609                                         
00610                                         //ROS_INFO("historicalPoints[%d].size() = (%d); from frame (%d) to (%d)", kkk, historicalPoints[kkk].size(), (readyFrame)-lossGaps[ppp]-1, (readyFrame-1));
00611                                         
00612                                         vector<unsigned int> dummyIndices;
00613                                         
00614                                                                                 
00615                                         //ROS_ERROR("olderImages[%d][%d].rows = (%d)", readyFrame % MAXIMUM_FRAMES_TO_STORE, ppp, olderImages[readyFrame % MAXIMUM_FRAMES_TO_STORE][ppp].rows);
00616                                         //ROS_ERROR("workingImOld[%d].rows = (%d)", readyFrame % MAXIMUM_FRAMES_TO_STORE, workingImOld[readyFrame % MAXIMUM_FRAMES_TO_STORE].rows);
00617                                 
00618                                         if (H12.rows != 0) {
00619                                                 trackPoints(olderImages[readyFrame % MAXIMUM_FRAMES_TO_STORE][ppp], grayImageBuffer[(readyFrame-1) % MAXIMUM_FRAMES_TO_STORE], historicalPoints[kkk], recoveredPoints[kkk], 8, configData.flowThreshold, dummyIndices, H12, configData.cameraData);
00620                                         } else {
00621                                                 Mat H_;
00622                                                 trackPoints(olderImages[readyFrame % MAXIMUM_FRAMES_TO_STORE][ppp], grayImageBuffer[(readyFrame-1) % MAXIMUM_FRAMES_TO_STORE], historicalPoints[kkk], recoveredPoints[kkk], distanceConstraint, configData.flowThreshold, dummyIndices, H_, configData.cameraData);
00623                                         }
00624                                         
00625 
00626                                         //ROS_INFO("recoveredPoints[%d].size() = %d", kkk, recoveredPoints[kkk].size());
00627                                         
00628                                         // figure out how to add recovered points to featureTrackVector (and re-activate tracks)
00629                                         
00630                                         //ROS_INFO("Adding additional recovered pts (%d) to start with...", recoveredPoints[kkk].size());
00631                                         
00632                                         // only want to recover up to certain number of tracks
00633                                         
00634                                         newlyRecoveredFeatures += recoveredPoints[kkk].size();
00635                                         
00636                                         unsigned int mmm = 0;
00637                                         while ((((int) startingPoints[kkk].size()) < ((int) configData.maxFeatures)) && (((int) mmm) < ((int) recoveredPoints[kkk].size()))) {
00638                                                 addMatchToVector(featureTrackVector, (readyFrame)-configData.lossGaps[ppp]-1, historicalPoints[kkk][mmm], (readyFrame-1), recoveredPoints[kkk][mmm]);
00639                                                 
00640                                                 
00641                                                 // Also need to add to starting points:
00642                                                 startingPoints[kkk].push_back(recoveredPoints[kkk][mmm]);
00643                                                 mmm++;
00644                                         }
00645                                         
00646                                         newlyRecoveredFeatures += mmm;
00647                                         
00648                                         //for (unsigned int mmm = 0; mmm < recoveredPoints[kkk].size(); mmm++) {
00649                                                 
00650                                         //}
00651                                         
00652                                         
00653                                         
00654                                 }
00655                                 
00656                         }
00657                         
00658                         //ROS_INFO("Debug (%d)", 3);
00659                         
00660                         if (performedRecovery) {
00661                                 lostTrackIndices.clear();
00662                         
00663                                 testTime = timeElapsedMS(test_timer, false);
00664                                 //ROS_WARN("Intermediate time: (%f)", testTime);
00665                         }
00666                         
00667                         
00668                         
00669                         vector<Point2f> copyOfBeforePoints;
00670                         
00671                         copyOfBeforePoints.insert(copyOfBeforePoints.end(), startingPoints[kkk].begin(), startingPoints[kkk].end());
00672                         
00673                         unsigned int ptsBefore = startingPoints[kkk].size();
00674                         
00675                         testTime = timeElapsedMS(test_timer, false);
00676                         //ROS_WARN("1: (%f)", testTime);
00677                         
00678                         //ROS_INFO("Debug (%d)", 4);
00679                         
00680                         // =========================================================
00681                         // ACTUAL TRACKING
00682                         // =========================================================
00683                         if (H12.rows != 0) {
00684                                 elapsedTime = timeElapsedMS(cycle_timer, false);
00685                                 trackPoints(grayImageBuffer[(readyFrame-1) % MAXIMUM_FRAMES_TO_STORE], grayImageBuffer[readyFrame % MAXIMUM_FRAMES_TO_STORE], startingPoints[kkk], finishingPoints[kkk], 8, configData.flowThreshold, lostTrackIndices, H12, configData.cameraData);
00686                                 elapsedTime = timeElapsedMS(cycle_timer, false);
00687                                 
00688                         } else {
00689                                 Mat H_;
00690                                 trackPoints(grayImageBuffer[(readyFrame-1) % MAXIMUM_FRAMES_TO_STORE], grayImageBuffer[readyFrame % MAXIMUM_FRAMES_TO_STORE], startingPoints[kkk], finishingPoints[kkk], distanceConstraint, configData.flowThreshold, lostTrackIndices, H_, configData.cameraData);
00691                         }
00692                         
00693                         //ROS_INFO("Debug (%d)", 5);
00694                         
00695                         testTime = timeElapsedMS(test_timer, false);
00696                         //ROS_WARN("2: (%f)", testTime);
00697                         
00698                         //ROS_INFO("(%d / %d) points successfully tracked. last time: (%d) tracks.", finishingPoints[kkk].size(), ptsBefore, previouslyTrackedPoints);
00699                         
00700                         successfullyTrackedFeatures += finishingPoints[kkk].size();
00701                         
00702                         lostTrackCount += lostTrackIndices.size();
00703                         
00704                         if ((readyFrame > 0) && (finishingPoints[kkk].size() < configData.minPointsForWarning)) {
00705                                 ROS_WARN("Very few (%d) points tracked...", finishingPoints[kkk].size());
00706                                 
00707                                 while (0) {
00708                                         
00709                                         imshow("temp_debug", grayImageBuffer[(readyFrame-1) % MAXIMUM_FRAMES_TO_STORE]);
00710                                         waitKey(500);
00711                                         imshow("temp_debug", grayImageBuffer[readyFrame % MAXIMUM_FRAMES_TO_STORE]);
00712                                         waitKey(500);
00713                                         
00714                                 }
00715                                 
00716                                 
00717                         }
00718                         
00719                         // Filter points that may be in high-noise region...
00720                         //double distProp = 0.75;
00721                         //filterTrackingsByRadialProportion(startingPoints[kkk], finishingPoints[kkk], configData.cameraData.Kx, configData.cameraData.expandedCamMat, configData.cameraData.distCoeffs, configData.cameraData.cameraSize, distProp);
00722                         
00723                         //printf("%s::%s << Points (%d:%d) after tracking: %d\n", __PROGRAM__, __FUNCTION__, current_idx, kkk, finishingPoints[kkk].size());
00724                         
00725                         testTime = timeElapsedMS(test_timer, false);
00726                         //ROS_WARN("3: (%f)", testTime);
00727                         
00728                         //ROS_INFO("Debug (%d)", 6);
00729                                                         
00730                         if (finishingPoints[kkk].size() < (peakTracks/2)) {
00731                                 ROS_WARN("Substantial dropping of tracks from (%d) to (%d)...", ptsBefore, finishingPoints[kkk].size());
00732                                 peakTracks = finishingPoints[kkk].size();
00733                                 //printf("%s::%s << WARNING! Substantial droppage of tracks...\n", __PROGRAM__, __FUNCTION__);
00734                                 //cin.get();
00735                         
00736                                 //Mat im_t_1, im_t_2;
00737                                 
00738                                 //double minVal = 8120.0;
00739                                 //double maxVal = 8184.0;
00740                                 
00741                                 //if (matricesAreEqual(mappedImage, olderImage)) {
00742                                 //      printf("%s::%s << This test says realImage/olderImage are equal...?\n", __PROGRAM__, __FUNCTION__);
00743                                 //}
00744                                 
00745                                 //normalize_16(im_t_1, mappedImage, minVal, maxVal);
00746                                 //normalize_16(im_t_2, olderImage, minVal, maxVal);
00747                                 
00748                                 //if (matricesAreEqual(im_t_1, im_t_2)) {
00749                                         //printf("%s::%s << This test says im_t_1/im_t_2 are equal...?\n", __PROGRAM__, __FUNCTION__);
00750                                 //}
00751                                         
00752                                 
00753                                 
00754                                 
00755                         }
00756                         
00757                         //ROS_INFO("Debug (%d)", 7);
00758                         
00759                         finalPoints1.insert(finalPoints1.end(), startingPoints[kkk].begin(), startingPoints[kkk].end());
00760                         finalPoints2.insert(finalPoints2.end(), finishingPoints[kkk].begin(), finishingPoints[kkk].end());
00761                         
00762                         
00763                         
00764                         //printf("%s << tracked points; %d\n", __FUNCTION__, finishingPoints[kkk].size());
00765                         
00766                         for (unsigned int mmm = 0; mmm < finalPoints1.size(); mmm++) {
00767                                 addMatchToVector(featureTrackVector, (readyFrame-1), finalPoints1.at(mmm), (readyFrame), finalPoints2.at(mmm));
00768                         }
00769                         
00770                         testTime = timeElapsedMS(test_timer, false);
00771                         //ROS_WARN("4: (%f)", testTime);
00772                         
00773                         // Finish tracks that are too old
00774                         //finishTracks(featureTrackVector, finalPoints1, 0.80, 25);
00775                         
00776                         //ROS_INFO("Debug (%d)", 8);
00777                         
00778                         if (configData.debugMode) {
00779                                 for (unsigned int mmm = 0; mmm < startingPoints[kkk].size(); mmm++) {
00780                                         addMatchToVector(displayTracks[kkk], (readyFrame-1), startingPoints[kkk].at(mmm), (readyFrame), finishingPoints[kkk].at(mmm));
00781                                 }
00782                                 
00783                                 
00784                                 
00785                                 //drawFeatureTracks(drawImage2, drawImage2, displayTracks[kkk], drawingColors[kkk], current_idx, history);
00786                                 
00787                                 //vector<featureTrack> redistortedTracks;
00788                                 //redistortTracks(displayTracks[kkk], redistortedTracks, configData.cameraData.Kx, configData.cameraData.distCoeffs, (readyFrame), configData.cameraData.expandedCamMat, history);
00789                                 drawFeatureTracks(drawImage, drawImage, displayTracks[kkk], trackDrawingColors[kkk], keyDrawingColors[kkk], (readyFrame), configData.drawingHistory);
00790                         }
00791                         
00792                         //ROS_INFO("Debug (%d)", 9);
00793                         
00794                         testTime = timeElapsedMS(test_timer, false);
00795                         //ROS_WARN("Intermediate time: (%f)", testTime);
00796                         
00797                 }
00798                 
00799                 previouslyTrackedPoints = finalPoints1.size();
00800 
00801                 
00802                 for (unsigned int kkk = 0; kkk < finalPoints1.size(); kkk++) {
00803                         addMatchToVector(featureTrackVector, (readyFrame-1), finalPoints1.at(kkk), (readyFrame), finalPoints2.at(kkk));
00804                 }
00805         }
00806         
00807         //ROS_INFO("Debug (%d)", 10);
00808         
00809         testTime = timeElapsedMS(test_timer, true);
00810         //ROS_WARN("Tracking time: (%f)", testTime);
00811         
00812         // NEW DETECTION?
00813         
00814         vector<Point2f> allPoints;
00815 
00816         for (unsigned int jjj = 0; jjj < configData.numDetectors; jjj++) {
00817                 allPoints.insert(allPoints.end(), finishingPoints[jjj].begin(), finishingPoints[jjj].end());
00818                 
00819         }
00820         
00821         //ROS_INFO("Debug (%d)", 11);
00822         
00823         for (unsigned int jjj = 0; jjj < configData.numDetectors; jjj++) {
00824                 
00825                 //ROS_INFO("Debug (%d)", 12);
00826                 
00827                 testTime = timeElapsedMS(test_timer, false);
00828                 //ROS_WARN("1: (%f)", testTime);
00829 
00830                 if ((readyFrame) == 0) {
00831                         lastPeak[jjj] = configData.maxTrackedFeaturesPerDetector;
00832                 }
00833                 
00834                 int ptsToTriggerNewDetection = (int) (0.8 * ((double) lastPeak[jjj]));
00835                 
00836                 bool wantNewDetection = false;
00837                 
00838                 if ((((int) finishingPoints[jjj].size()) < ((int) configData.maxFeatures)) && (((int) finishingPoints[jjj].size()) < ((int) std::max(configData.minTrackedFeaturesPerDetector, ptsToTriggerNewDetection)))) {
00839                         wantNewDetection = true;
00840                 } else if ((((int) finishingPoints[jjj].size()) < ((int) configData.maxFeatures)) && (((readyFrame) % configData.newDetectionFramecountTrigger) == 0)) {
00841                         wantNewDetection = true;
00842                 }
00843                 
00844                 testTime = timeElapsedMS(test_timer, false);
00845                 //ROS_WARN("2: (%f)", testTime);
00846 
00847                 
00848                 //ROS_INFO("Debug (%d)", 13);
00849                 
00850                 if (wantNewDetection) {
00851                         
00852                         //ROS_INFO("Debug (%d)", 14);
00853                         
00854                         currPoints[jjj].clear();
00855                         keypointDetector[jjj] -> detect(grayImageBuffer[readyFrame % MAXIMUM_FRAMES_TO_STORE], currPoints[jjj]);
00856                         
00857                         //ROS_INFO("Debug (%d)", 15);
00858                         
00859                         //ROS_INFO("(%d) new points detected...", currPoints[jjj].size());
00860                         
00861                         discardedNewFeatures += currPoints[jjj].size();
00862 
00863                         testTime = timeElapsedMS(test_timer, false);
00864                         //ROS_WARN("3: (%f)", testTime);
00865                         
00866                         
00867                         if (currPoints[jjj].size() == 0) {
00868                                 lastPeak[jjj] = 0;
00869                                 
00870                         } else {
00871                                 
00872                                 //ROS_INFO("Debug (%d)", 16);
00873                         
00874                                 sortKeypoints(currPoints[jjj]);
00875                                 
00876                                 //ROS_INFO("Debug (%d) : (%d)", 161, currPoints[jjj].size());
00877                                 
00878                                 testTime = timeElapsedMS(test_timer, false);
00879                                 //ROS_WARN("4: (%f)", testTime);
00880                                                                         
00881                                 vector<Point2f> candidates;
00882                                 
00883                                 reduceEdgyFeatures(currPoints[jjj], configData.cameraData);
00884                                 
00885                                 //ROS_INFO("Debug (%d)", 162);
00886                                 
00887                                 testTime = timeElapsedMS(test_timer, false);
00888                                 //ROS_WARN("5: (%f)", testTime);
00889                                 
00890                                 //ROS_INFO("(%d) points after edge-reduction...", currPoints[jjj].size());
00891 
00892                                 //reduceWeakFeatures(workingImNew, currPoints[jjj], minResponse);
00893                                 
00894                                 testTime = timeElapsedMS(test_timer, false);
00895                                 //ROS_WARN("6: (%f)", testTime);
00896                                 
00897                                 //ROS_INFO("(%d) points after weakness-reduction...", currPoints[jjj].size());
00898                                 
00899                                 // Want to cull new points to fit limit
00900                                 
00901                                 //ROS_INFO("Debug (%d)", 17);
00902                                 
00903                                 for (unsigned int zzz = 0; zzz < currPoints[jjj].size(); zzz++) {
00904                                         // if distance between [ currPoints[jjj].at(zzz) ] and all points in finishingPoints[jjj]
00905                                         // is greater than [ configData.minDistance ] , delete and decrement index
00906                                         
00907                                         bool violatesProximity = false;
00908                                         
00909                                         for (unsigned int yyy = 0; yyy < finishingPoints[jjj].size(); yyy++) {
00910                                                 
00911                                                 if (distBetweenPts2f(currPoints[jjj].at(zzz).pt, finishingPoints[jjj].at(yyy)) < configData.minDistance) {
00912                                                         violatesProximity = true;
00913                                                         break;
00914                                                 }
00915                                                 
00916                                         }
00917                                         
00918                                         if (violatesProximity) {
00919                                                 
00920                                                 // delete
00921                                                 currPoints[jjj].erase(currPoints[jjj].begin() + zzz);
00922                                                 zzz--;
00923                                         }
00924                                 }
00925                                 
00926                                 //ROS_INFO("Debug (%d)", 18);
00927                                 
00928                                 if ((currPoints[jjj].size() + finishingPoints[jjj].size()) > configData.maxFeatures) {
00929                                         currPoints[jjj].erase(currPoints[jjj].begin() + (configData.maxFeatures - finishingPoints[jjj].size()), currPoints[jjj].end());
00930                                         
00931                                         //ROS_ERROR("Reduced to (%d) points because of maxFeatures...", currPoints[jjj].size());
00932                                         
00933                                 }
00934                                 
00935                                 newlyDetectedFeatures += currPoints[jjj].size();
00936                                 discardedNewFeatures -= currPoints[jjj].size();
00937                                 
00938                                 KeyPoint::convert(currPoints[jjj], candidates);
00939                                 
00940                                 //ROS_INFO("Debug (%d)", 19);
00941                                 
00942                                 if (candidates.size() > 0) {
00943                                         
00944                                         //ROS_INFO("Debug (%d)", 20);
00945                                         
00946                                         //cornerSubPix(workingImNew, candidates, Size(1,1), Size(-1,-1), cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1)); // cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 15, 0.1)
00947                                 
00948                                         testTime = timeElapsedMS(test_timer, false);
00949                                         //ROS_WARN("7: (%f)", testTime);
00950                                 
00951                                         // reduce candidates based on features existing in close proximity
00952                                         //reduceProximalCandidates(allPoints, candidates);
00953                                         //reduceEdgyCandidates(candidates, configData.cameraData);
00954                                         
00955                                         testTime = timeElapsedMS(test_timer, false);
00956                                         //ROS_WARN("8: (%f)", testTime);
00957                                         
00958                                         //ROS_INFO("(%d) points after proximity-reduction...", candidates.size());
00959                                         
00960                                         //printf("%s << candidates after reducing ones close to edge: %d\n", __FUNCTION__, candidates.size());
00961                                         //reduceUnrefinedCandidates(candidates);
00962                                         //printf("%s << candidates after reducing ones with subpixel refinement: %d\n", __FUNCTION__, candidates.size());
00963                                         allPoints.insert(allPoints.end(), candidates.begin(), candidates.end());
00964                                         
00965                                         concatenateWithExistingPoints(finishingPoints[jjj], candidates, configData.maxTrackedFeaturesPerDetector, configData.minDistance);
00966                                         
00967                                         //printf("%s << left after concatenation: %d\n", __FUNCTION__, finishingPoints[jjj].size());
00968                                         
00969                                         lastPeak[jjj] =  finishingPoints[jjj].size();
00970                                         
00971                                         testTime = timeElapsedMS(test_timer, false);
00972                                         //ROS_WARN("9: (%f)", testTime);
00973                                         
00974                                         //ROS_INFO("Debug (%d)", 21);
00975                                         
00976                                 }
00977                                 
00978                                 //ROS_INFO("Debug (%d)", 22);
00979                                 
00980                                 if (configData.debugMode) {
00981                                         //vector<Point2f> redistortedPoints;
00982                                         //redistortPoints(finishingPoints[jjj], redistortedPoints, configData.cameraData.Kx, configData.cameraData.distCoeffs, configData.cameraData.expandedCamMat);
00983                                         displayKeypoints(drawImage, finishingPoints[jjj], drawImage, keyDrawingColors[jjj], 0);
00984                                 }
00985                                 
00986                                 //ROS_INFO("Debug (%d)", 23);
00987                                 
00988                         }
00989                         
00990                         //ROS_INFO("Debug (%d)", 24);
00991                         
00992                 }
00993                 
00994         }
00995         
00996         //ROS_INFO("Debug (%d)", 25);
00997         
00998         if (H12.rows != 0) {
00999                 H12.release();
01000         }
01001         
01002         // =================================
01003         // PUBLICATION
01004         // =================================
01005         if (configData.debugMode) {
01006                 
01007                 //ROS_INFO("Debug (%d)", 26);
01008                 
01009                 if (drawImage.size() != configData.cameraData.cameraSize) {
01010                         resize(drawImage, drawImage_resized, configData.cameraData.cameraSize, 0, 0, INTER_CUBIC);
01011                 } else {
01012                         drawImage_resized = drawImage;
01013                 }
01014                 
01015                 //ROS_INFO("Debug (%d)", 27);
01016                                                 
01017                 std::copy(&(drawImage_resized.at<Vec3b>(0,0)[0]), &(drawImage_resized.at<Vec3b>(0,0)[0])+(drawImage_resized.cols*drawImage_resized.rows*drawImage_resized.channels()), msg_debug.data.begin());
01018                 
01019                 //ROS_INFO("Debug (%d)", 28);
01020                 
01021                 //ROS_WARN("Publishing to: (%s)", debug_pub_name);
01022                 debug_pub.publish(msg_debug, debug_camera_info);
01023                 
01024                 //ROS_INFO("Debug (%d)", 29);
01025                 
01026                 //imshow("test", drawImage);
01027                 //waitKey(1);
01028                 
01029         }
01030         
01031         //ROS_INFO("Debug (%d)", 30);
01032         
01033         int publishedTrackCount = publish_tracks(&tracks_pub);
01034         
01035         if (configData.outputTrackCount) {
01036                 // Successfully tracked
01037                 // Newly detected
01038                 // Ancient recovered
01039                 // Newly discarded
01040                 // Total published
01041                 
01042                 // finishingPoints[0].size()
01043                 trackCountStream << readyFrame << " ";
01044                 trackCountStream << successfullyTrackedFeatures << " ";
01045                 trackCountStream << publishedTrackCount << " ";
01046                 trackCountStream << newlyDetectedFeatures << " ";
01047                 trackCountStream << newlyRecoveredFeatures << " ";
01048                 //trackCountStream << discardedNewFeatures << " ";
01049                 trackCountStream << lostTrackCount << " ";
01050                 trackCountStream << averageTrackLength << endl;
01051         }
01052         
01053         //ROS_INFO("Debug (%d)", 31);
01054         
01055         readyFrame++;
01056         
01057         
01058 }
01059 
01060 void featureTrackerNode::prepareForTermination() {
01061         if (trackCountStream.is_open()) trackCountStream.close();       
01062 }
01063 
01064 void featureTrackerNode::handle_very_new() {
01065         
01066         unsigned int current_idx = frameCount-1;
01067         
01068         ROS_INFO("Handling the *very* new frame (%d) now... [%d]", current_idx, frameCount);
01069         
01070         
01071         
01072         // Feature detection
01073         
01074         homogPoints[1].clear();
01075         homographyDetector -> detect(grayImageBuffer[current_idx % MAXIMUM_FRAMES_TO_STORE], homogPoints[1]);
01076         
01077         // Feature description
01078         
01079         homographyExtractor -> compute(grayImageBuffer[current_idx % MAXIMUM_FRAMES_TO_STORE], homogPoints[1], homogDescriptors[1]);
01080         
01081         // Feature matching
01082         
01083         Mat matchingMatrix;
01084         
01085         createMatchingMatrix(matchingMatrix, homogDescriptors[0], homogDescriptors[1]);
01086                         
01087         //constrainMatchingMatrix(matchingMatrix, homogPoints[0], homogPoints[1], MATCHING_DIST_CONSTRAINT, MATCHING_SIZE_CONSTRAINT);
01088         
01089         vector<vector<DMatch> > matches;
01090         
01091         twoWayPriorityMatching(matchingMatrix, matches);
01092         
01093         sortMatches(matches);
01094         
01095         // 0.5 for ratio prioritization
01096         //filterMatches(matches, 0.5);
01097         
01098         if (configData.debugMode) {
01099                 /*
01100                 displayImage.copyTo(drawImage);
01101                 displayKeypoints(displayImage, homogPoints[1], drawImage, CV_RGB(255, 0, 0), 0);
01102                 
01103                 drawMatchPaths(drawImage, drawImage, homogPoints[0], homogPoints[1], matches);
01104                 
01105                 std::copy(&(drawImage.at<unsigned char>(0,0)), &(drawImage.at<unsigned char>(0,0))+(drawImage.cols*drawImage.rows*drawImage.channels()), msg_debug.data.begin());                               
01106                 debug_pub.publish(msg_debug, debug_camera_info);
01107                 */
01108         }
01109         
01110         // Reduce points to matched points
01111         
01112         vector<Point2f> points1, points2;
01113         
01114         vector<int> queryIdxs( matches.size() );
01115         vector<int> trainIdxs( matches.size() );
01116         
01117         for( size_t i = 0; i < matches.size(); i++ ) {
01118                 queryIdxs[i] = matches[i][0].queryIdx;
01119                 trainIdxs[i] = matches[i][0].trainIdx;
01120         }
01121         
01122         KeyPoint::convert(homogPoints[0], points1, queryIdxs);
01123         KeyPoint::convert(homogPoints[1], points2, trainIdxs);
01124 
01125         vector<uchar> validityMask;
01126         vector<char> validityMask2;  
01127         
01128         // Homography determination
01129         
01130         if ((points1.size() < 4) || (points2.size() < 4)) {
01131                 H12 = Mat::eye(3, 3, CV_64FC1);
01132                 undergoingDelay = false;
01133                 return;
01134         }
01135         
01136         H12 = findHomography( Mat(points1), Mat(points2), validityMask, CV_RANSAC, 5.0 );
01137         
01138         
01139         Mat im1, im2;
01140         warpPerspective(grayImageBuffer[(current_idx-1) % MAXIMUM_FRAMES_TO_STORE], im2, H12, grayImageBuffer[(current_idx-1) % MAXIMUM_FRAMES_TO_STORE].size());
01141         //grayImageBuffer[(current_idx-1) % MAXIMUM_FRAMES_TO_STORE].copyTo(im2);
01142         
01143         grayImageBuffer[current_idx % MAXIMUM_FRAMES_TO_STORE].copyTo(im1);
01144         
01145         
01146         while (0) {
01147                 
01148                 Mat im1_col, im2_col;
01149                 
01150                 cvtColor(im1, im1_col, CV_GRAY2RGB);
01151                 cvtColor(im2, im2_col, CV_GRAY2RGB);
01152                 
01153                 //imshow("temp_disp", im1);     // Current image
01154                 std::copy(&(im1_col.at<unsigned char>(0,0)), &(im1_col.at<unsigned char>(0,0))+(drawImage.cols*drawImage.rows*drawImage.channels()), msg_debug.data.begin());                           
01155                 debug_pub.publish(msg_debug, debug_camera_info);
01156                 waitKey(500);
01157                 //imshow("temp_disp", im2);     // Previous image under transform
01158                 std::copy(&(im2_col.at<unsigned char>(0,0)), &(im2_col.at<unsigned char>(0,0))+(drawImage.cols*drawImage.rows*drawImage.channels()), msg_debug.data.begin());                           
01159                 debug_pub.publish(msg_debug, debug_camera_info);
01160                 waitKey(500);
01161                 
01162                 
01163         }
01164         
01165         unsigned int inlierPoints = countNonZero(validityMask);
01166         
01167         ROS_INFO("Number of matches = %d -> %d/%d -> %d", homogPoints[0].size(), matches.size(), points1.size(), inlierPoints);
01168         
01169         cout << H12 << endl;
01170         
01171         // Then, do you want to move "finishingPoints" so that they're at the locations estimated by H12?
01172         
01173         
01174         
01175         undergoingDelay = false;
01176 }
01177 
01178 void featureTrackerNode::handle_delay() {
01179                 
01180         unsigned int current_idx = frameCount-1;
01181         
01182         ROS_INFO("Handling a delay now (%d) [%d]...", current_idx, frameCount);
01183         
01184         
01185         // Feature detection
01186         
01187         
01188         
01189         ROS_INFO("Current index: (%d)", current_idx);
01190         
01191         ROS_INFO("grayIm: (%d, %d, %d, %d))", grayImageBuffer[current_idx % MAXIMUM_FRAMES_TO_STORE].rows, grayImageBuffer[current_idx % MAXIMUM_FRAMES_TO_STORE].cols, grayImageBuffer[current_idx % MAXIMUM_FRAMES_TO_STORE].depth(), grayImageBuffer[current_idx % MAXIMUM_FRAMES_TO_STORE].channels());
01192         
01193         homogPoints[0].clear();
01194         homographyDetector -> detect(grayImageBuffer[current_idx % MAXIMUM_FRAMES_TO_STORE], homogPoints[0]);
01195         //keypointDetector[0] -> detect(grayImageBuffer[current_idx % MAXIMUM_FRAMES_TO_STORE], homogPoints[0]);
01196         
01197         ROS_INFO("Homography frame #1 (%d) pts detected", homogPoints[0].size());
01198         
01199         if (homogPoints[0].size() == 0) {
01200                 ROS_ERROR("Insufficient points detected (%d)", homogPoints[0].size());
01201                 
01202         } else {
01203 
01204                 sortKeypoints(homogPoints[0], 200);
01205                 
01206                 ROS_INFO("Sorting points...");
01207         
01208                 if (configData.debugMode) {
01209                         /*
01210                         displayImage.copyTo(drawImage);
01211                         displayKeypoints(displayImage, homogPoints[0], drawImage, CV_RGB(255, 0, 0), 0);
01212                         
01213                         std::copy(&(drawImage.at<unsigned char>(0,0)), &(drawImage.at<unsigned char>(0,0))+(drawImage.cols*drawImage.rows*drawImage.channels()), msg_debug.data.begin());                               
01214                         debug_pub.publish(msg_debug, debug_camera_info);
01215                         */
01216                 }
01217                 
01218                 homographyExtractor -> compute(grayImageBuffer[current_idx % MAXIMUM_FRAMES_TO_STORE], homogPoints[0], homogDescriptors[0]);
01219         }
01220         
01221         // Feature description
01222         undergoingDelay = true;
01223 }
01224 
01225 featureTrackerNode::featureTrackerNode(ros::NodeHandle& nh, trackerData startupData) {
01226         
01227         initializeDrawingColors(keyDrawingColors, trackDrawingColors, MAX_DETECTORS);
01228         
01229         peakTracks = 0;
01230         previouslyTrackedPoints = 0;
01231         
01232         undergoingDelay = false;
01233         
01234         referenceFrame = -1;
01235         
01236         frameCount = 0;
01237         readyFrame = 0;
01238         
01239         sprintf(nodeName, "%s", ros::this_node::getName().c_str());
01240 
01241         sprintf(debug_pub_name, "thermalvis%s/image_col", nodeName);
01242         
01243         infoProcessed = false;
01244         infoSent = false;
01245         
01246         configData = startupData;
01247         
01248         configData.initializeDetectors(keypointDetector, &homographyDetector);
01249         configData.initializeDescriptors(descriptorExtractor, &homographyExtractor);
01250         
01251         //printf("%s::%s << Startup data: (%s)\n", __PROGRAM__, __FUNCTION__, configData.video_stream.c_str());
01252         
01253         std::string topic = nh.resolveName(configData.video_stream + configData.video_sequence);
01254         //std::string topic = nh.resolveName("image");
01255         
01256         std::string topic_info = nh.resolveName(configData.video_stream + "camera_info");
01257         
01258         ROS_INFO("Connecting to camera_info. %s", topic_info.c_str());
01259         
01260         image_transport::ImageTransport it(nh);
01261         
01262         if (configData.debugMode) {
01263                 debug_pub = it.advertiseCamera(debug_pub_name, 1);
01264                 ROS_INFO("Advertising tracker debugging video (%s)", debug_pub_name);
01265         }
01266         
01267         ROS_INFO("Softsync mode: (%d)", configData.softSync?1:0);
01268         if (configData.softSync) {
01269                 info_sub = nh.subscribe<sensor_msgs::CameraInfo>(topic_info, 1, &featureTrackerNode::handle_info, this);
01270                 image_sub = it.subscribe(topic, 1, &featureTrackerNode::handle_image, this);
01271         } else {
01272                 camera_sub = it.subscribeCamera(topic, 1, &featureTrackerNode::handle_camera, this);
01273         }
01274                 
01275         sprintf(tracks_pub_name, "thermalvis%s/tracks", nodeName);
01276         
01277         tracks_pub = nh.advertise<thermalvis::feature_tracks>(tracks_pub_name, 1);
01278         
01279         timer = nh.createTimer(ros::Duration(0.05), &featureTrackerNode::timed_loop, this);
01280         
01281         features_timer = nh.createTimer(ros::Duration(0.01), &featureTrackerNode::features_loop, this);
01282         
01283         //printf("%s::%s << Image transport subscribed.\n", __PROGRAM__, __FUNCTION__);
01284         
01285         char timeString[256];
01286         
01287         sprintf(timeString, "%010d.%09d", ros::Time::now().sec, ros::Time::now().nsec);
01288         
01289         
01290         if (configData.outputTrackCount) {
01291                 string outputTrackCountFile = configData.read_addr + "nodes/flow/log/" + timeString + "-" + ros::this_node::getName().substr(1,ros::this_node::getName().size()-1) + ".txt";
01292         
01293                 ROS_INFO("outputTrackCountFile = (%s)", outputTrackCountFile.c_str());
01294                 
01295                 trackCountStream.open(outputTrackCountFile.c_str(), ios::out);
01296         }
01297         
01298         
01299         
01300         //cin.get();
01301         
01302         ROS_INFO("Establishing server callback...");
01303         f = boost::bind (&featureTrackerNode::serverCallback, this, _1, _2);
01304     server.setCallback (f);
01305     
01306 }
01307 
01308 
01309 
01310 void trackerData::initializeDetectors(Ptr<FeatureDetector> *det, Ptr<FeatureDetector> *hom) {
01311         
01312         for (unsigned int iii = 0; iii < numDetectors; iii++) {
01313                 if (detector[iii] == "SURF") {
01314                         ROS_ERROR("SURF has been deactivated due to copyright protection!");
01315                         //det[iii] = new SurfFeatureDetector( sensitivity[iii] );
01316                 } else if (detector[iii] == "FAST") {
01317                         det[iii] = new FastFeatureDetector( sensitivity[iii] * 100.0 );
01318                 } else if (detector[iii] == "GFTT") {
01319                         det[iii] = new GoodFeaturesToTrackDetector( maximumDetectionFeatures, sensitivity[iii], 1.0, 3, false );
01320                 } else if (detector[iii] == "STAR") {
01321                         det[iii] = new StarFeatureDetector( 16, sensitivity[iii] );
01322                 } else if (detector[iii] == "ORB") {
01323                         det[iii] = new OrbFeatureDetector( maximumDetectionFeatures );
01324                 } else if (detector[iii] == "HARRIS") {
01325                         det[iii] = new GoodFeaturesToTrackDetector( maximumDetectionFeatures, sensitivity[iii], 1.0, 3, true );
01326                 }
01327         }
01328         
01329         hom[0] = new OrbFeatureDetector( maximumDetectionFeatures );
01330         //hom[0] = new SurfFeatureDetector( 1.00 );
01331         
01332 }
01333 
01334 void featureTrackerNode::timed_loop(const ros::TimerEvent& event) {
01335         
01336         elapsedTime = timeElapsedMS(cycle_timer, false);
01337         
01338 }
01339 
01340 void featureTrackerNode::serverCallback(thermalvis::flowConfig &config, uint32_t level) {
01341       
01342     configData.minDistance = config.minDistance;
01343       
01344     configData.sensitivity[0] = config.sensitivity_1;
01345         configData.sensitivity[1] = config.sensitivity_2;
01346         configData.sensitivity[2] = config.sensitivity_3;
01347         
01348         configData.softSync = config.softSync;
01349         configData.syncDiff = config.syncDiff;
01350         
01351         configData.soft_diff_limit = (unsigned long) (configData.syncDiff * 1000000000.0);
01352         
01353         ROS_INFO("Sens = (%f, %f, %f)", configData.sensitivity[0], configData.sensitivity[1], configData.sensitivity[2]);
01354         
01355         configData.maxFeatures = config.maxFeatures;
01356         
01357         int det_array[3];
01358         det_array[0] = config.detector_1;
01359         det_array[1] = config.detector_2;
01360         det_array[2] = config.detector_3;
01361         
01362         for (unsigned int iii = 0; iii < 3; iii++) {
01363                 
01364                 if (det_array[iii] == 1) {
01365                         configData.detector[iii] = "GFTT";
01366                 } else if (det_array[iii] == 2) {
01367                         configData.detector[iii] = "FAST";
01368                 } else if (det_array[iii] == 3) {
01369                         configData.detector[iii] = "HARRIS";
01370                 }
01371                 
01372         }
01373         
01374         if (config.detector_1 > 0) {
01375                 
01376                 if (config.detector_2 > 0) {
01377                         
01378                         if (config.detector_3 > 0) {
01379                                 configData.numDetectors = 3;
01380                                 
01381                         } else {
01382                                 configData.numDetectors = 2;
01383                         }
01384                         
01385                 } else {
01386                         configData.numDetectors = 1;
01387                 }
01388                 
01389         } else {
01390                 configData.numDetectors = 0;
01391         }
01392     
01393     
01394     
01395     
01396         configData.debugMode = config.debugMode;
01397         
01398         configData.flowThreshold = config.flowThreshold;
01399         
01400         configData.maxFrac = config.maxFrac;
01401         
01402         configData.minTrackedFeaturesPerDetector = config.minTrackedFeaturesPerDetector;
01403         configData.maxTrackedFeaturesPerDetector = config.maxTrackedFeaturesPerDetector;
01404         configData.delayTimeout = config.delayTimeout;
01405         
01406         configData.maximumDetectionFeatures = config.maximumDetectionFeatures;
01407         
01408         configData.newDetectionFramecountTrigger = config.newDetectionFramecountTrigger;
01409         
01410         configData.maxProjectionsPerFeature = config.maxProjectionsPerFeature;
01411         
01412         configData.nearSearchHistory = config.nearSearchHistory;
01413         configData.farSearchHistory = config.farSearchHistory;
01414                 
01415         configData.lossGaps[0] = configData.nearSearchHistory;
01416         configData.lossGaps[1] = configData.farSearchHistory;
01417         
01418         configData.initializeDetectors(keypointDetector, &homographyDetector);
01419         
01420         configData.drawingHistory = config.drawingHistory;
01421         
01422         configData.minPointsForWarning = config.minPointsForWarning;
01423         
01424 }
01425