laser_pointer_detector_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # laser_interface
00003 #
00004 #  Copyright (c) 2008, Willow Garage, Inc.
00005 #  All rights reserved.
00006 #  
00007 #  Redistribution and use in source and binary forms, with or without
00008 #  modification, are permitted provided that the following conditions are met:
00009 #  
00010 #      * Redistributions of source code must retain the above copyright
00011 #        notice, this list of conditions and the following disclaimer.
00012 #      * Redistributions in binary form must reproduce the above copyright
00013 #        notice, this list of conditions and the following disclaimer in the
00014 #        documentation and/or other materials provided with the distribution.
00015 #      * Neither the name of the <ORGANIZATION> nor the names of its
00016 #        contributors may be used to endorse or promote products derived from
00017 #        this software without specific prior written permission.
00018 #  
00019 #  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020 #  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021 #  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022 #  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023 #  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024 #  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025 #  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026 #  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027 #  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028 #  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029 #  POSSIBILITY OF SUCH DAMAGE.
00030 #
00031 #
00032 ##@mainpage
00033 #
00034 # @b laser_interface implments a laser pointer finder taking in images from a
00035 # stereo pair and outputing a 3D point that can be used a mouse cursor in the
00036 # world.  This works by first filtering the stereo pair based on image
00037 # intensity and image motion.  Large connected components are then thrown out
00038 # leaving smaller components representative of laser points.  Each components
00039 # is then fed to a random forest classifier to find whether the detection is a
00040 # laser point or not.  To gather positive and negative training examples for
00041 # the classifier, use the interface launched by the @b user_interface_node.
00042 # By default the @b laser_pointer_detector_node gathers negative training example
00043 # so just point the camera to an area containing motion for it to grab negative samples.
00044 # To gather positive examples, point the camera at a static scene where the laser
00045 # point is the only moving object then switch to 'positive' mode with the GUI
00046 # spawned by @ user_interface_node. The modes offered by the interface are:
00047 # - 'positive': tells the detector to gather positive examples of laser points.
00048 # - 'rebuild' : rebuild classifier
00049 # - 'clear' : clear out training examples gathered so far
00050 # - 'debug' : prints timing information 
00051 # - 'display': show images being processed
00052 # - 'verbose': print statistics
00053 #
00054 # All parameters for the algorithm is stored in params.xml.  For triangulation
00055 # this code uses the camera calibrations provided by cameras.xml.
00056 #
00057 # @author Hai Nguyen/hai@gatech.edu
00058 #
00059 #<hr>
00060 #
00061 #@section usage Usage
00062 #@verbatim
00063 #$ roslaunch launch.xml
00064 #@endverbatim
00065 #
00066 #<hr>
00067 #
00068 #@section topic ROS topics
00069 #
00070 #Subscribes to (name/type):
00071 #- @b "mouse_click"/String : 'True' or 'False' indicating if a normal desktop mouse has been clicked.
00072 #- @b "laser_mode"/String : sets the mode to either 'debug' 'display' 'verbose' 'rebuild' 'positive' or 'clear'.
00073 #
00074 #Publishes to (name / type):
00075 #- @b "cursor3d"/PointStamped:
00076 #
00077 #<hr>
00078 #
00079 # @todo remove dependency from the opencv version modified for Videre stereo pairs.
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 # Build detectors for both camera in stereo, create debugging images, triangulates and return 3D points
00150 # Decides when to gather data
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 #clicked is here as data gathering logic is here
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         #import pdb
00169         #pdb.set_trace()
00170         frames = hardware_camera.next()
00171         self.left_detector = LaserPointerDetector(frames[0], self.dataset_file)#, exposure=exposure)
00172         self.right_detector = LaserPointerDetector(frames[1], self.dataset_file, #exposure=exposure, 
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             #Don't return anything if point is behind camera
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                 #store as positives
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                 #store as negatives 
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                 #don't store anything as this case is ambiguous
00260             else:
00261                 #store as negatives (false positives)
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         #import pdb
00274         #pdb.set_trace()
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 # Grab images, calls detectors, sends results out based on inputs from a user interface node
00290 class LaserPointerDetectorNode:
00291     def __init__(self, camera_root_topic, calibration_root_topic, 
00292             #exposure = LaserPointerDetector.SUN_EXPOSURE, 
00293             #video = None, 
00294             dataset_file,
00295             display=False):
00296 
00297         image_type = 'image_rect_color'
00298         #if video is None:
00299         #self.video    = cam.StereoFile('measuring_tape_red_left.avi','measuring_tape_red_right.avi')
00300         #else:
00301         #    self.video = video
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         #self.exposure = exposure
00311         self.display = display
00312         self.debug = False #Require display = True
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'): #Rebuild detector based on new training data
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'): #Will toggle gathering positive examples
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                 #rospy.logdebug('Running at ' + str(1./(t1 - t0)) + ' hz.')
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 #if __name__ == '__main__':
00422 #    from pkg import *
00423 #    rospy.init_node('laser_pointer_detector')
00424 #    print 'PARAM is', rospy.get_param('~shade_exposure')
00425 #    import pdb
00426 #    pdb.set_trace()
00427 #    print 'PARAM is', rospy.get_param('~shade_exposure')
00428 #    exit()
00429 
00430 if __name__ == '__main__':
00431     import optparse
00432     p = optparse.OptionParser()
00433     #move this to params too?
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     #move this to params too?
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     #p.add_option('-r', '--run',  action='store_true', 
00445     #            dest='mode_run', help='classify')
00446 
00447     #p.add_option('-o', '--office', action='store_true', dest='office', 
00448     #             help='true for operating robot in office environments')
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     #Move this to a param file
00456     #if opt.office == True:
00457     #    print 'opt.office == True, using SHADE exposure'
00458     #    exposure = LaserPointerDetector.SHADE_EXPOSURE
00459     #else:
00460     #    exposure = LaserPointerDetector.SUN_EXPOSURE
00461     #    print 'opt.office == False, using SUN exposure'
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     #print 'Exposure set to', exposure
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 


laser_interface
Author(s): Hai Nguyen and Travis Deyle. Advisor: Prof. Charlie Kemp, Lab: Healthcare Robotics Lab at Georgia Tech
autogenerated on Wed Nov 27 2013 11:45:51