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
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082 from pkg import *
00083 from std_msgs.msg import String
00084 from geometry_msgs.msg import PointStamped
00085 from geometry_msgs.msg import PoseStamped
00086 import sys, time
00087 import cv
00088
00089 import laser_interface.camera as cam
00090 import laser_interface.random_forest as rf
00091 import laser_interface.dimreduce as dr
00092 import laser_interface.util as ut
00093 from laser_interface.laser_detector import *
00094 import laser_interface.blob as blob
00095 import laser_interface.cv_actions as cva
00096 from threading import RLock
00097
00098 def show_processed(image, masks, detection, blobs, detector):
00099 masker = cva.Mask(image)
00100 splitter = cva.SplitColors(image)
00101 r, g, b = splitter.split(image)
00102 thresholded_image = masker.mask(masks[0], r, g, b)
00103 draw_detection(thresholded_image, detection)
00104 cv.ShowImage('thresholded', thresholded_image)
00105
00106 draw_detection(image, detection)
00107 blob.draw_blobs(image, blobs, classification_window_width=rospy.get_param('~classification_window_width'))
00108
00109 cva.make_visible_binary_image(masks[0])
00110 draw_detection(masks[0], detection)
00111 cva.make_visible_binary_image(masks[1])
00112 cva.make_visible_binary_image(masks[2])
00113
00114 cv.ShowImage("video", image)
00115 cv.ShowImage('motion', masks[1])
00116 cv.ShowImage('intensity', masks[2])
00117
00118 def matrix_to_dataset(examples, type=1):
00119 outputs_mat = np.matrix(np.zeros((1, examples.shape[1]), dtype='int'))
00120 outputs_mat[:,:] = type
00121 return rf.Dataset(examples, outputs_mat)
00122
00123 def confirmation_prompt(confirm_phrase):
00124 print confirm_phrase
00125 print 'y(es) / n(no)'
00126 k = cv.WaitKey()
00127 if k == 'y':
00128 return True
00129 else:
00130 return False
00131
00132 def append_examples_from_file(dataset, file):
00133 try:
00134 loaded_set = ut.load_pickle(file)
00135 dataset.append(loaded_set)
00136 except IOError:
00137 rospy.logerr('append_examples_from_file: training file \'' + str(file) + '\'not found!')
00138 return dataset.num_examples()
00139
00140 def print_friendly(votes):
00141 new_dict = {}
00142 total = 0
00143 for k in votes.keys():
00144 new_key = k[0,0]
00145 new_dict[new_key] = votes[k]
00146 return new_dict
00147
00148
00149
00150
00151 class EmbodiedLaserDetector:
00152
00153 def __init__(self, geometric_camera, hardware_camera, dataset_file):
00154 self.examples = []
00155 self.labels = []
00156 self.gather_positive_examples = False
00157 self.clicked = False
00158 self.stereo_cam = geometric_camera
00159 self.dataset_file = dataset_file
00160 self.build_detectors(hardware_camera)
00161
00162 def clear_examples(self):
00163 self.examples = []
00164 self.labels = []
00165
00166 def build_detectors(self, hardware_camera):
00167 self.write()
00168
00169
00170 frames = hardware_camera.next()
00171 self.left_detector = LaserPointerDetector(frames[0], self.dataset_file)
00172 self.right_detector = LaserPointerDetector(frames[1], self.dataset_file,
00173 classifier=self.left_detector.classifier)
00174 for i in xrange(10):
00175 frames = hardware_camera.next()
00176 self.left_detector.detect(frames[0])
00177 self.right_detector.detect(frames[1])
00178
00179 def run(self, images, display=True, debug=False):
00180 results = None
00181 left_detection, left_intensity_motion_activations, left_image, left_combined_masks = \
00182 self.left_detector.detect(images[0])
00183 self.record(left_detection, left_image, left_intensity_motion_activations)
00184
00185 right_detection, right_intensity_motion_activations, right_image, right_combined_masks = \
00186 self.right_detector.detect(images[1])
00187 self.record(right_detection, right_image, right_intensity_motion_activations)
00188
00189 if debug:
00190 motion, intensity = self.left_detector.get_motion_intensity_images()
00191 show_processed(left_image, [left_combined_masks, motion, intensity],
00192 left_detection, left_intensity_motion_activations, self.left_detector)
00193 elif display:
00194 draw_detection(left_image, left_detection)
00195 cv.ShowImage('video', left_image)
00196
00197 if left_detection != None and right_detection != None:
00198 results = self.triangulate(left_detection, right_detection)
00199
00200 if left_detection != None and left_detection.has_key('vote'):
00201 rospy.loginfo('EmbodiedLaserDetector.triangulate: votes ' + str(print_friendly(left_detection['votes'])))
00202
00203 if self.clicked:
00204 return results
00205 else:
00206 return None
00207
00208 def set_debug(self, v):
00209 self.debug = v
00210 self.left_detector.set_debug(v)
00211 self.right_detector.set_debug(v)
00212
00213 def triangulate(self, left_cam_detection, right_cam_detection):
00214 if right_cam_detection.has_key('votes'):
00215 rospy.loginfo('EmbodiedLaserDetector.triangulate: votes ' \
00216 + str(print_friendly(right_cam_detection['votes'])) \
00217 + ' ' + str(print_friendly(left_cam_detection['votes'])))
00218 x = np.matrix(left_cam_detection['centroid']).T
00219 xp = np.matrix(right_cam_detection['centroid']).T
00220 rospy.loginfo('triangulate: x' + str(x.T) + ' xp ' + str(xp.T))
00221 result = self.stereo_cam.triangulate_3d(x, xp)
00222 rospy.loginfo('3D point located at' + str(result['point'].T) + \
00223 ('distance %.2f error %.3f' % (np.linalg.norm(result['point']), result['error'])))
00224 if result['point'][2,0] < 0:
00225
00226 rospy.loginfo('EmbodiedLaserDetector.triangulate: point was behind camera, ignoring')
00227 return None
00228
00229 if result['point'][2,0] > 5:
00230 rospy.loginfo('EmbodiedLaserDetector.triangulate: was too far, ignoring')
00231 return None
00232
00233 return result
00234
00235 def record(self, picked_blob, image, other_candidates):
00236 def store(label):
00237 instance = blob.blob_to_input_instance(image, picked_blob,
00238 self.left_detector.CLASSIFICATION_WINDOW_WIDTH)
00239 if instance != None:
00240 self.examples.append(instance)
00241 self.labels.append(np.matrix([label]))
00242
00243 if self.gather_positive_examples:
00244 if self.clicked:
00245
00246 if picked_blob != None:
00247 store(1)
00248 rospy.loginfo('EmbodiedLaserDetector.record: expected 1 got 1, ' + \
00249 str(len(self.examples)))
00250 else:
00251
00252 if picked_blob != None:
00253 store(0)
00254 rospy.loginfo('EmbodiedLaserDetector.record: expected 0 (no laser detections) but got 1 (laser detection), ' + \
00255 str(len(self.examples)) + ' instances')
00256 else:
00257 if self.clicked:
00258 pass
00259
00260 else:
00261
00262 if picked_blob != None:
00263 store(0)
00264 rospy.loginfo('EmbodiedLaserDetector.record: expected 0 (no laser detections) got 1 (laser detection), ' + \
00265 str(len(self.examples)) + 'instances')
00266
00267 def write(self):
00268 if not (len(self.examples) > 0):
00269 rospy.loginfo('EmbodiedLaserDetector.write: no examples to record')
00270 return
00271 inputs = ut.list_mat_to_mat(self.examples, axis = 1)
00272 outputs = ut.list_mat_to_mat(self.labels, axis = 1)
00273
00274
00275 rospy.loginfo('EmbodiedLaserDetector.write: inputs.shape, outputs.shape ' + str(inputs.shape) + ' ' + str(outputs.shape))
00276 dim_reduce_set = rf.LinearDimReduceDataset(inputs, outputs)
00277 rospy.loginfo('EmbodiedLaserDetector.write: appending examples from disk to dataset')
00278 n = append_examples_from_file(dim_reduce_set, file=self.dataset_file)
00279 rospy.loginfo('EmbodiedLaserDetector.write: calculating pca projection vectors')
00280 dim_reduce_set.set_projection_vectors(dr.pca_vectors(dim_reduce_set.inputs, percent_variance=self.left_detector.PCA_VARIANCE_RETAIN))
00281 rospy.loginfo('EmbodiedLaserDetector.write: writing...')
00282 ut.dump_pickle(dim_reduce_set, self.dataset_file)
00283 rospy.loginfo('EmbodiedLaserDetector: recorded examples to disk. Total in dataset ' + str(n))
00284 self.examples = []
00285 self.labels = []
00286
00287
00288
00289
00290 class LaserPointerDetectorNode:
00291 def __init__(self, camera_root_topic, calibration_root_topic,
00292
00293
00294 dataset_file,
00295 display=False):
00296
00297 image_type = 'image_rect_color'
00298
00299
00300
00301
00302 self.video = cam.ROSStereoListener(['/' + camera_root_topic + '/left/' + image_type,
00303 '/' + camera_root_topic + '/right/' + image_type])
00304
00305 self.video_lock = RLock()
00306 self.camera_model = cam.ROSStereoCalibration('/' + calibration_root_topic + '/left/camera_info' ,
00307 '/' + calibration_root_topic + '/right/camera_info')
00308 self.detector = EmbodiedLaserDetector(self.camera_model, self.video, dataset_file)
00309
00310
00311 self.display = display
00312 self.debug = False
00313 self.windows_made = False
00314 if self.display:
00315 self._make_windows()
00316
00317 rospy.Subscriber(MOUSE_CLICK_TOPIC, String, self._click_handler)
00318 rospy.loginfo('Suscribed to ' + MOUSE_CLICK_TOPIC)
00319 rospy.Subscriber(LASER_MODE_TOPIC, String, self._mode_handler)
00320 rospy.loginfo('Subscribed to ' + LASER_MODE_TOPIC)
00321 self.topic = rospy.Publisher(CURSOR_TOPIC, PointStamped)
00322 rospy.loginfo('Publishing to ' + CURSOR_TOPIC)
00323 self.viz_topic = rospy.Publisher(VIZ_TOPIC, PoseStamped)
00324 rospy.loginfo('Publishing to ' + VIZ_TOPIC)
00325
00326 def _click_handler(self, evt):
00327 message = evt.data
00328 if self.detector is not None:
00329 if message == 'True':
00330 self.detector.clicked = True
00331 rospy.loginfo('LaserPointerDetector.click_handler: click!')
00332 elif message == 'False':
00333 self.detector.clicked = False
00334 rospy.loginfo('LaserPointerDetector.click_handler: released click')
00335 else:
00336 raise RuntimeError('unexpected click message from topic' + MOUSE_CLICK_TOPIC)
00337
00338 def _mode_handler(self, evt):
00339 message = evt.data
00340 if(message == 'debug'):
00341 self.debug = not self.debug
00342 rospy.loginfo('LaserPointerDetector.mode_handler: debug' + str(self.debug))
00343 self._make_windows()
00344
00345 elif (message == 'display'):
00346 self.display = not self.display
00347 rospy.loginfo('LaserPointerDetector.mode_handler: display ' + str(self.display))
00348 self._make_windows()
00349
00350 elif(message == 'rebuild'):
00351 self.video_lock.acquire()
00352 if self.detector is not None:
00353 self.detector.build_detectors(self.video)
00354 self.video_lock.release()
00355
00356 elif(message == 'positive'):
00357 if self.detector is not None:
00358 self.detector.gather_positive_examples = not self.detector.gather_positive_examples
00359 rospy.loginfo('LaserPointerDetector.mode_handler: gather_positive_examples' + str(self.detector.gather_positive_examples))
00360
00361 elif(message == 'clear'):
00362 self.detector.clear_examples()
00363 else:
00364 raise RuntimeError('unexpected mode message from topic' + LASER_MODE_TOPIC)
00365
00366 def _make_windows(self):
00367 if self.windows_made:
00368 return
00369 windows = ['video', 'right', 'thresholded', 'motion', 'intensity', 'patch', 'big_patch']
00370 for n in windows:
00371 cv.NamedWindow(n, 1)
00372 cv.MoveWindow("video", 0, 0)
00373 cv.MoveWindow("right", 800, 0)
00374 cv.MoveWindow("thresholded", 800, 0)
00375 cv.MoveWindow("intensity", 0, 600)
00376 cv.MoveWindow("motion", 800, 600)
00377 self.windows_made = True
00378
00379 def set_debug(self, v):
00380 self.detector.set_debug(v)
00381 self.debug = v
00382
00383 def run(self):
00384 i = 0
00385 try:
00386 while not rospy.is_shutdown():
00387 i = i + 1
00388 t0 = time.time()
00389 self.video_lock.acquire()
00390 frames = list(self.video.next())
00391 result = self.detector.run(frames, display=self.display, debug=self.debug)
00392 self.video_lock.release()
00393
00394 if result != None:
00395 p = result['point']
00396 ps = PointStamped()
00397 ps.header.stamp = rospy.get_rostime()
00398 ps.header.frame_id = rospy.get_param('laser_pointer_detector/detector_frame')
00399 ps.point.x = p[0,0]
00400 ps.point.y = p[1,0]
00401 ps.point.z = p[2,0]
00402 self.topic.publish(ps)
00403
00404 pose_stamped = PoseStamped()
00405 pose_stamped.header = ps.header
00406 pose_stamped.pose.position = ps.point
00407 self.viz_topic.publish(pose_stamped)
00408
00409 if self.display:
00410 k = cv.WaitKey(10)
00411 t1 = time.time()
00412
00413
00414 if i % 100 == 0:
00415 rospy.loginfo('Running at ' + str(1./(t1 - t0)) + ' hz.')
00416
00417 except StopIteration, e:
00418 if self.state_object.__class__ == GatherExamples:
00419 self.state_object.write()
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430 if __name__ == '__main__':
00431 import optparse
00432 p = optparse.OptionParser()
00433
00434 p.add_option('-c', '--camera', action='store',
00435 dest='camera', default='wide_stereo',
00436 help='stereo pair root topic (wide_stereo, narrow_stereo, etc)')
00437
00438 p.add_option('-k', '--calibration', action='store',
00439 dest='calibration', default=None,
00440 help='stereo pair calibration root topic (usually the same as given in -c)')
00441 p.add_option('-f', '--dataset', action='store',
00442 dest='dataset_file', default='PatchClassifier.dataset.pickle', help='dataset for classifier')
00443
00444
00445
00446
00447
00448
00449 p.add_option('-d', '--display', action='store_true',
00450 dest='display', default=False, help='show display')
00451 p.add_option('-t', '--time', action = 'store_true',
00452 dest='time', help='display timing information')
00453 opt, args = p.parse_args()
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463 if opt.calibration == None:
00464 opt.calibration = opt.camera
00465
00466 rospy.loginfo('===========================================================')
00467 rospy.loginfo('# Detections are red circles. =')
00468 rospy.loginfo('# Hypothesis blobs are blue squares. =')
00469 rospy.loginfo('===========================================================')
00470
00471 rospy.init_node('laser_pointer_detector')
00472 lpdn = LaserPointerDetectorNode(opt.camera, opt.calibration,
00473 opt.dataset_file, display=opt.display)
00474 if opt.time != None:
00475 lpdn.set_debug(True)
00476 lpdn.run()
00477