00001
00006 #ifndef _THERMALVIS_FLOW_H_
00007 #define _THERMALVIS_FLOW_H_
00008
00009 #include "general_resources.hpp"
00010 #include "ros_resources.hpp"
00011 #include "opencv_resources.hpp"
00012
00013 #include "improc.hpp"
00014 #include "video.hpp"
00015 #include "features.hpp"
00016 #include "tracks.hpp"
00017 #include "initialization.hpp"
00018
00019 #include "feature_tracks.h"
00020
00021 #include "flowConfig.h"
00022
00023 #include "boost/filesystem.hpp"
00024
00025 const char __PROGRAM__[] = "THERMALVIS_FLOW";
00026
00027 typedef dynamic_reconfigure::Server < thermalvis::flowConfig > Server;
00028
00029 #define DEFAULT_ALPHA 0.00
00030 #define MS_PER_SEC 1000.0
00031
00032 #define MINIMUM_GFTT_SENSITIVITY 0.001
00033 #define MINIMUM_HARRIS_SENSITIVITY 0.0001
00034 #define FAST_DETECTOR_SENSITIVITY_SCALING 200.0
00035
00036 #define TIGHT_DISTANCE_CONSTRAINT 3
00037 #define FEATURE_DROP_TRIGGER 0.40
00038
00039 #define MINIMUM_PROP_OF_PREVIOUS_FEATURES 0.8
00040
00041 #define SKIP_WARNING_TIME_PERIOD 10.0
00042
00043
00044
00045
00046
00047
00048
00049
00050 #define COLOR_TRACKED_POINTS CV_RGB(255, 0, 0) // Red
00051 #define COLOR_TRACKED_PATH CV_RGB(255, 128, 127) // Light Red
00052 #define COLOR_RECOVERED_POINTS CV_RGB(0, 255, 0) // Green
00053 #define COLOR_UNMATCHED_POINTS CV_RGB(255, 0, 255) // Purple
00054 #define COLOR_MATCHED_POINTS CV_RGB(255, 255, 0) // Yellow
00055
00056 #define ADAPTIVE_WINDOW_CONTIGENCY_FACTOR 5.0
00057
00058 #define MIN_FEATURES_FOR_FEATURE_MOTION 10
00059 #define MAX_HISTORY_FRAMES 16
00060
00061 bool wantsToShutdown = false;
00062 void mySigintHandler(int sig);
00063
00064 void imageCallback(const sensor_msgs::ImageConstPtr& msg);
00065 void parameterCallback(const sensor_msgs::CameraInfo& msg);
00066
00068 struct trackerData {
00069
00070 string topic, topicParent;
00071 string read_addr;
00072
00073 string tracksOutputTopic;
00074
00075 bool detectEveryFrame;
00076
00077 bool verboseMode;
00078 bool adaptiveWindow;
00079
00080 double maxVelocity;
00081 bool attemptMatching;
00082
00083 bool attemptHistoricalRecovery;
00084
00085
00086 double newFeaturesPeriod;
00087 int multiplier[MAX_HISTORY_FRAMES];
00088
00089 bool autoTrackManagement;
00090
00091 string outputFolder;
00092
00093 bool velocityPrediction;
00094
00095 cameraParameters cameraData;
00096
00097
00098
00099 double delayTimeout;
00100 double flowThreshold;
00101 double maxFrac;
00102
00103 int matchingMode;
00104
00105 int maxFeatures, minFeatures;
00106
00107
00108 bool debugMode, showTrackHistory;
00109 bool outputTrackCount, outputFeatureMotion;
00110 bool normalizeFeatureVelocities;
00111
00112 string detector[MAX_DETECTORS], descriptor[MAX_DETECTORS];
00113 double sensitivity[MAX_DETECTORS];
00114 string method[MAX_DETECTORS];
00115 bool method_match[MAX_DETECTORS];
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127 int drawingHistory;
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137 double minSeparation;
00138
00139 unsigned int numDetectors;
00140
00141 bool obtainStartingData(ros::NodeHandle& nh);
00142 void initializeDetectors(cv::Ptr<cv::FeatureDetector> *det, cv::Ptr<cv::FeatureDetector> *hom);
00143 void initializeDescriptors(cv::Ptr<cv::DescriptorExtractor> *desc, cv::Ptr<cv::DescriptorExtractor> *hom);
00144
00145 };
00146
00147
00149 class featureTrackerNode {
00150 private:
00151
00152 image_transport::ImageTransport *it;
00153
00154 unsigned int cycleCount;
00155 bool cycleFlag;
00156 ros::Time lastCycleFrameTime;
00157
00158 unsigned int skippedFrameCount, capturedFrameCount;
00159
00160 long int lastAllocatedTrackIndex;
00161
00162
00163 ofstream trackCountStream, featureMotionStream;
00164
00165 bool debugInitialized;
00166
00167 int newlyDetectedFeatures;
00168
00169 string optical_frame;
00170
00171 bool lowPointsWarning;
00172
00173 cv::Size ksize;
00174 double blurSigma;
00175
00176 unsigned int historicalIndices[];
00177
00178 vector<unsigned int> activeTrackIndices;
00179
00180 int successfullyTrackedFeatures;
00181 int discardedNewFeatures;
00182
00183 vector<unsigned int> lostTrackIndices;
00184 int referenceFrame;
00185
00186 bool undergoingDelay;
00187 bool handleDelays;
00188
00189 cv::Mat dispMat;
00190
00191 bool infoProcessed, infoSent;
00192
00193 cv::Mat H12;
00194
00195 double factor, minResponse;
00196
00197 ros::Timer timer, features_timer;
00198
00199 trackerData configData;
00200
00201 sensor_msgs::CameraInfo debug_camera_info;
00202
00203 ros::Publisher tracks_pub;
00204 image_transport::CameraPublisher debug_pub;
00205
00206 image_transport::CameraSubscriber camera_sub;
00207 ros::Subscriber info_sub;
00208 image_transport::Subscriber image_sub;
00209
00210 unsigned int previousTrackedPointsPeak;
00211
00212 cv_bridge::CvImagePtr cv_ptr;
00213
00214 cv::Mat normalizedMat, blownImage, displayImage2, subscribedImage, drawImage2;
00215 cv::Mat newImage, grayImage, realImage, mappedImage, lastImage, olderImage;
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227 struct timeval cycle_timer, test_timer, skip_timer;
00228 double elapsedTime, testTime, skipTime;
00229
00230 unsigned int frameCount;
00231 unsigned int readyFrame;
00232
00233
00234 image_transport::CameraPublisher pub_debug;
00235 sensor_msgs::Image msg_debug;
00236
00237 char debug_pub_name[256];
00238 char tracks_pub_name[256];
00239 ros::NodeHandle private_node_handle;
00240
00241 char nodeName[256];
00242
00243 cv::Mat displayImage, drawImage, drawImage_resized;
00244
00245 unsigned int bufferIndices[2];
00246 cv::Mat grayImageBuffer[2];
00247
00248 unsigned int numHistoryFrames;
00249 unsigned int olderIndices[MAX_HISTORY_FRAMES];
00250 ros::Time olderTimes[MAX_HISTORY_FRAMES];
00251 cv::Mat olderImages[MAX_HISTORY_FRAMES];
00252 double featuresVelocity;
00253
00254
00255
00256 cv::Mat blurredImage;
00257
00258
00259 vector<featureTrack> displayTracks;
00260
00261 cv::Ptr<cv::FeatureDetector> keypointDetector[MAX_DETECTORS];
00262 cv::Ptr<cv::FeatureDetector> homographyDetector;
00263
00264 vector<cv::KeyPoint> homogPoints[2];
00265 cv::Ptr<cv::DescriptorExtractor> descriptorExtractor;
00266 cv::Ptr<cv::DescriptorExtractor> homographyExtractor;
00267
00268 cv::Mat homogDescriptors[2];
00269
00270
00271
00272 int newlyRecoveredFeatures;
00273 int lostTrackCount;
00274
00275
00276 vector<cv::Point2f> globalStartingPoints, globalFinishingPoints;
00277
00278 vector<cv::Point2f> allRecoveredPoints, preservedRecoveredPoints, unmatchedPoints, matchedPoints;
00279
00280
00281 vector<cv::Point2f> newlySensedFeatures;
00282 vector<cv::Point2f> matchedFeatures;
00283
00284 ros::Time info_time, image_time, previous_time, original_time, dodgeTime;
00285 int previousIndex, currentIndex;
00286
00287 int distanceConstraint;
00288
00289 int peakTracks;
00290
00291 vector<featureTrack> featureTrackVector;
00292
00293 bool freezeNextOutput;
00294
00295
00296
00297 dynamic_reconfigure::Server<thermalvis::flowConfig> server;
00298 dynamic_reconfigure::Server<thermalvis::flowConfig>::CallbackType f;
00299
00300 double averageTrackLength;
00301
00302 public:
00303 featureTrackerNode(ros::NodeHandle& nh, trackerData startupData);
00304
00305 void timed_loop(const ros::TimerEvent& event);
00306
00308 int calculateFeatureMotion(unsigned int idx, double& mx, double &my);
00309
00310 void features_loop(const ros::TimerEvent& event);
00311
00312 void handle_delay();
00313
00314 void trimFeatureTrackVector();
00315 void trimDisplayTrackVectors();
00316
00317 void handle_very_new();
00318
00319
00320 void updateDistanceConstraint();
00321 void attemptTracking();
00322 void detectNewFeatures();
00323 void publishRoutine();
00324 void matchWithExistingTracks();
00325
00326 void act_on_image();
00327 void process_info(const sensor_msgs::CameraInfoConstPtr& info_msg);
00328
00329 void handle_camera(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg);
00330
00331 void serverCallback(thermalvis::flowConfig &config, uint32_t level);
00332
00333 void prepareForTermination();
00334
00335 int publish_tracks(ros::Publisher *pub_message, unsigned int latestIndex);
00336 };
00337
00338
00339 void connected(const ros::SingleSubscriberPublisher&) {}
00340 void disconnected(const ros::SingleSubscriberPublisher&) {}
00341
00342 boost::shared_ptr < featureTrackerNode > *globalNodePtr;
00343
00344 #endif