00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 from pkg import *
00030 import cv
00031 import numpy as np
00032
00033 import itertools as it
00034 import time
00035 import functools as ft
00036
00037 import util as ut
00038 import random_forest as rf
00039 import laser_interface.cv_actions as cvact
00040 import laser_interface.blob as blob
00041
00042 class LaserPointerDetector:
00043
00044 def __init__(self, sample_frame, dataset_file, classifier = None):
00045 self.EXPOSURE = rospy.get_param('~exposure')
00046
00047
00048
00049 self.MAX_BLOB_AREA = rospy.get_param('~max_blob_area')
00050 self.LASER_POINT_SIZE = rospy.get_param('~laser_point_size')
00051 self.MIN_AGE = rospy.get_param('~min_age')
00052 self.NUMBER_OF_LEARNERS = rospy.get_param('~number_of_learners')
00053 self.PCA_VARIANCE_RETAIN = rospy.get_param('~pca_variance_retain')
00054
00055 self.TRACKER_MAX_PIX_TRESHOLD = rospy.get_param('~tracker_max_pix_treshold')
00056 self.TRACKER_MAX_TIME_THRESHOLD = rospy.get_param('~tracker_max_time_threshold')
00057
00058 self.CLASSIFICATION_WINDOW_WIDTH = rospy.get_param('~classification_window_width')
00059
00060 self.COLOR_CHANNEL = rospy.get_param('~color_channel')
00061
00062
00063
00064
00065 if classifier is None:
00066 try:
00067
00068 loaded_dataset = ut.load_pickle(dataset_file)
00069 self.classifier = PatchClassifier(loaded_dataset,
00070 self.NUMBER_OF_LEARNERS,
00071 self.CLASSIFICATION_WINDOW_WIDTH)
00072 except IOError, e:
00073 rospy.logerr('LaserPointerDetector: no data file detected (not using classifier) at loc' + dataset_file)
00074 self.classifier = None
00075 else:
00076 self.classifier = classifier
00077
00078
00079 self.intensity_filter = cvact.BrightnessThreshold(sample_frame, self.MAX_BLOB_AREA)
00080 self.motion_filter = cvact.MotionSubtract(sample_frame, self.MAX_BLOB_AREA)
00081 self.combine = cvact.CombineMasks(sample_frame)
00082 self.tracker = Tracker(self.TRACKER_MAX_PIX_TRESHOLD, self.TRACKER_MAX_TIME_THRESHOLD)
00083 self.splitter = cvact.SplitColors(sample_frame)
00084
00085
00086 self.copy = cv.CreateImage(cv.GetSize(sample_frame), 8, 3)
00087 self.combined_grey_scale = cv.CreateImage(cv.GetSize(sample_frame), 8, 1)
00088 self.channel = self.COLOR_CHANNEL
00089
00090 def get_motion_intensity_images(self):
00091 return (self.motion_filter.get_thresholded_image(), self.intensity_filter.get_thresholded_image())
00092
00093 def detect(self, image):
00094 cv.Copy(image, self.copy)
00095 image = self.copy
00096
00097 if self.COLOR_CHANNEL == 'red':
00098 channel = 2
00099 elif self.COLOR_CHANNEL == 'green':
00100 channel = 1
00101 elif self.COLOR_CHANNEL == 'blue':
00102 channel = 0
00103 r, g, b = self.splitter.split(image)
00104 coi = [r,g,b][channel]
00105 thres_low = rospy.get_param('~intensity_threshold_low')
00106 thres_high = rospy.get_param('~intensity_threshold_high')
00107 intensity_filtered = self.intensity_filter.threshold(thres_low, thres_high, coi)
00108 motion_filtered = self.motion_filter.subtract(coi)
00109 combined = self.combine.combine([intensity_filtered, motion_filtered])
00110
00111
00112 cv.Threshold(combined, combined, 0, 1, cv.CV_THRESH_BINARY)
00113 cv.Mul(coi, combined, self.combined_grey_scale)
00114 intensity_motion_blob = blob.blob_statistics(self.combined_grey_scale)
00115
00116 if len(intensity_motion_blob) > 100:
00117 rospy.logwarn('Too many... ' + str(len(intensity_motion_blob)))
00118 return image, combined, None, intensity_motion_blob
00119
00120 components = intensity_motion_blob
00121 if self.classifier is not None:
00122 number_components_before = len(components)
00123 components = self.classifier.classify(image, components)
00124 if number_components_before != len(components):
00125 rospy.logdebug( 'LaserPointerDetector: PatchClassifier: %d -> %d' % (number_components_before, len(components)))
00126
00127 laser_blob = select_laser_blob(components, approx_laser_point_size=self.LASER_POINT_SIZE)
00128 if laser_blob != None:
00129 tracks = self.tracker.track(components_to_detections([laser_blob]))
00130 laser_track = select_laser_track(tracks, self.MIN_AGE)
00131 if laser_blob != None and laser_track == None:
00132 rospy.logdebug(' Tracker: image motion filter activated.')
00133 if laser_track is not None:
00134 laser_blob = laser_track.last_detection.component
00135 laser_blob['track'] = laser_track
00136 else:
00137 laser_blob = None
00138
00139 return laser_blob, intensity_motion_blob, image, combined
00140
00141 class PatchClassifier:
00142 def __init__(self, dataset, number_of_learners, classification_window_width):
00143 rospy.logdebug('PatchClassifier.__init__: dataset size ' + str(dataset.num_examples()))
00144 self.classification_window_width = classification_window_width
00145 rospy.loginfo('PatchClassifier: building classifier...')
00146
00147 dataset.reduce_input()
00148 self.dataset = dataset
00149 self.classifier = rf.RFBreiman(dataset, number_of_learners=number_of_learners)
00150 rospy.loginfo('PatchClassifier: done building.')
00151
00152 def classify(self, image, components):
00153 def predict_all(c):
00154 instance = blob.blob_to_input_instance(image, c,
00155 classification_window_width=self.classification_window_width)
00156 if instance == None:
00157 classification = np.matrix([False])
00158 return (classification, None, c)
00159 classification, votes = self.classifier.predict(self.dataset.reduce(instance))
00160 return (classification, votes, c)
00161
00162 def select_valid(c):
00163 classification, votes, component = c
00164 if classification[0,0] == 0:
00165 return False
00166 else:
00167 return True
00168
00169 def hide_votes_in_object(c):
00170 classification, votes, component = c
00171 component['votes'] = votes
00172 return component
00173
00174 return map(hide_votes_in_object, filter(select_valid, map(predict_all, components)))
00175
00176 class Track:
00177 def __init__(self, id, pos, cur_time):
00178 self.id = id
00179 self.pos = pos
00180 self.start_time = cur_time
00181 self.end_time = cur_time
00182 self.track = [pos]
00183 self.last_detection = None
00184 self.fresh = True
00185 self.dist_moved = 0.0
00186
00187 def update(self, detection, cur_time):
00188 self.dist_moved = np.linalg.norm(self.pos - detection.pos)
00189 self.pos = detection.pos
00190 self.end_time = cur_time
00191 self.track.append(detection.pos)
00192 self.last_detection = detection
00193 self.fresh = True
00194
00195 def age(self):
00196 return self.end_time - self.start_time
00197
00198 def last_detection_time(self, cur_time):
00199 return cur_time - self.end_time
00200
00201 class Detection:
00202 def __init__(self, pos):
00203 self.pos = pos
00204
00205 class Tracker:
00206 def __init__(self, max_pix_thres, max_time_thres):
00207 self.tracks = []
00208 self.ids = 0
00209 self.cur_time = 0
00210 self.MAX_PIX_TRESHOLD = max_pix_thres
00211 self.MAX_TIME_THRESHOLD = max_time_thres
00212
00213 def track(self, detections):
00214
00215 pairs = []
00216 pairs_dist = []
00217 for track in self.tracks:
00218 track.fresh = False
00219 for d in detections:
00220 pairs.append((track, d))
00221 pairs_dist.append(np.linalg.norm(track.pos - d.pos))
00222
00223 for d in detections:
00224 d.used = False
00225
00226
00227 sorted_indices = np.argsort(pairs_dist)
00228
00229
00230 pairs = np.array(pairs)
00231 pairs_dist = np.array(pairs_dist)
00232 selected_pairs = pairs[sorted_indices[0:len(detections)]]
00233 selected_dists = pairs_dist[sorted_indices[0:len(detections)]]
00234
00235
00236 for idx in xrange(len(selected_pairs)):
00237 if selected_dists[idx] < self.MAX_PIX_TRESHOLD:
00238 track, detection = selected_pairs[idx]
00239 track.update(detection, self.cur_time)
00240 detection.used = True
00241
00242
00243 for d in detections:
00244 if not d.used:
00245 self.tracks.append(Track(self.ids, d.pos, self.cur_time))
00246 self.ids = self.ids + 1
00247
00248
00249 new_tracks = []
00250 for t in self.tracks:
00251 if t.last_detection_time(self.cur_time) <= self.MAX_TIME_THRESHOLD:
00252 new_tracks.append(t)
00253 self.tracks = new_tracks
00254 self.cur_time = self.cur_time + 1
00255 return self.tracks
00256
00257 def components_to_detections(components):
00258 ds = []
00259 for c in components:
00260 d = Detection(np.matrix(c['centroid']).T)
00261 d.component = c
00262 ds.append(d)
00263 return ds
00264
00265
00266
00267
00268 def select_laser_blob(blobs, approx_laser_point_size=40, min_area = 1.0):
00269 size_filtered_blobs = []
00270
00271 for b in blobs:
00272
00273 if b['rect'].width <= approx_laser_point_size and b['rect'].height <= approx_laser_point_size and b['area'] >= min_area:
00274 size_filtered_blobs.append(b)
00275
00276
00277
00278
00279 if len(size_filtered_blobs) == 0:
00280 return None
00281
00282 largest_area = 0
00283 largest_blob = None
00284 for b in size_filtered_blobs:
00285 if b['area'] > largest_area:
00286 largest_area = b['area']
00287 largest_blob = b
00288
00289
00290 return largest_blob
00291
00292 def select_laser_track(tracks, min_age):
00293 oldest_age = -1
00294 oldest = None
00295 for t in tracks:
00296 if t.age() > oldest_age:
00297 oldest_age = t.age()
00298 oldest = t
00299 if oldest_age >= min_age:
00300 if oldest.fresh:
00301 return oldest
00302 else:
00303 return None
00304 else:
00305 return None
00306
00307 def draw_detection(frame, laser_blob):
00308 if frame.nChannels == 1:
00309 color = cv.Scalar(200)
00310 else:
00311 color = cv.Scalar(255, 255, 0)
00312
00313 if laser_blob != None:
00314 point = (int(round(laser_blob['centroid'][0])), int(round(laser_blob['centroid'][1])))
00315 cv.Circle(frame, point, 10, cv.Scalar(0,0,255), 1)
00316 cv.Circle(frame, point, 1, color, cv.CV_FILLED)