Example #1
0
def get_test_data():
    cfg = configuration.configuration("../data/ROS_test_client/", "dummyScanner")
    pc = processor.processor(cfg)
    pc.scan_dataset = scan_dataset()
    pc.scan_dataset.image_artag_filename = ""
    pc.scan_dataset.table_plane_translation = np.matrix([0, 0, 0]).T
    # approximate height of hokuyo above the ground, assume it to be fixed for now.
    pc.scan_dataset.ground_plane_translation = np.matrix([0, 0, 1.32]).T
    pc.scan_dataset.ground_plane_rotation = ""
    # pc.scan_dataset.is_test_set = True
    pc.scan_dataset.is_labeled = True
    pc.scan_dataset.id = "ROStest"
    pc.load_raw_data(pc.scan_dataset.id)
    image_size = cv.cvGetSize(pc.img)

    # don't do any mapping to not change the pts3d!
    # create point cloud from raw range points
    (pc.pts3d, pc.scan_indices, pc.intensities) = pc.create_pointcloud(pc.laserscans, True, True)

    # region of interest in image pixels
    polygon = label_object()
    polygon.set_label("surface")
    width_min = image_size.width * 0.4
    width_max = image_size.width * 0.95
    height_min = image_size.height * 0.3
    height_max = image_size.height * 0.95
    polygon.add_point((width_min, height_min))
    polygon.add_point((width_min, height_max))
    polygon.add_point((width_max, height_max))
    polygon.add_point((width_max, height_min))

    return pc.pts3d, pc.intensities, None, pc.img, pc.image_angle, polygon
Example #2
0
def testPlanePointcloud():
    import roslib; roslib.load_manifest('laser_camera_segmentation')
    import laser_camera_segmentation.processor as processor
    import laser_camera_segmentation.configuration as configuration    

    cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling')
    #sc = scanner.scanner(cfg)
    pc = processor.processor(cfg)
    #pc.load_data('2009Oct30_162400')
    pc.load_data('2009Nov04_141226')
    pc.process_raw_data()
    
    
    debug = False
    model = PlaneLeastSquaresModel(debug)
    data = numpy.asarray(pc.pts3d_bound).T
    # run RANSAC algorithm
    ransac_fit, ransac_data = ransac(data,model,
                                     3, 1000, 0.02, 300, # misc. parameters
                                     debug=debug,return_all=True)
    print ransac_fit
    print ransac_data    
    print 'len inlier',len(ransac_data['inliers']),'shape pts',np.shape(pc.pts3d_bound)
    pc.pts3d_bound = pc.pts3d_bound[:,ransac_data['inliers']]
    pc.display_3d('height')
Example #3
0
 def slot_test_Classifiers_on_testset(self):
     if False == self.processor:       
         self.processor = processor.processor(self.config)
     self.slot_save()    #save data first so the processor can load it:
     self.processor.load_data(self.current_dataset.id)
     self.processor.train_and_save_Classifiers()
     self.processor.test_classifiers_on_testset()          
Example #4
0
    def slot_generate_save_features(self):

        if False == self.processor:       
            self.processor = processor.processor(self.config)
            
        #save data first so the processor can load it:
        self.slot_save()    
            
        self.processor.load_data(self.current_dataset.id)
        self.processor.generate_save_features()   
Example #5
0
    def slot_display_3d(self, spheres = False):

        if False == self.processor:       
            self.processor = processor.processor(self.config)
            
        #save data first so the processor can load it:
        self.slot_save()    
            
        self.processor.load_data(self.current_dataset.id)
        #self.processor.create_polygon_images()
        self.processor.process_raw_data()
        #pc.save_mapped_image(name)
        self.processor.display_3d(self.display_3d_type, spheres)
Example #6
0
    def slot_display_3d(self, spheres=False):

        if False == self.processor:
            self.processor = processor.processor(self.config)

        #save data first so the processor can load it:
        self.slot_save()

        self.processor.load_data(self.current_dataset.id)
        #self.processor.create_polygon_images()
        self.processor.process_raw_data()
        #pc.save_mapped_image(name)
        self.processor.display_3d(self.display_3d_type, spheres)
Example #7
0
 def display3d(self):
     import laser_camera_segmentation.processor as processor
     import laser_camera_segmentation.configuration as configuration       
     print 'display in 3d...'
     cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling')
     cfg.cam_vec = np.array(self.vals)
     import scanr_transforms as trs
     cfg.camTlaser = trs.camTlaser(cfg.cam_vec)
     
     pc = processor.processor(cfg)
     pc.load_data(self.dataset_id)
     pc.process_raw_data()
     pc.display_3d('labels', False)        
Example #8
0
def segment_pointcloud(request):
    #convert data from ROS
    pts3d, intensities, labels = convert_ROS_pointcloud_to_pointcloud(
        request.pointcloud)
    cvimage = convert_ROS_image_to_cvimage(request.image, request.imageWidth,
                                           request.imageHeight)
    polygon = convert_ROS_polygon_to_polygon(request.regionOfInterest2D)
    polygon.set_label('surface')
    print polygon, polygon.get_points()

    #create processor and configuration
    #cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/ROS_server_test', 'codyRobot')
    cfg = configuration.configuration('../data/ROS_server', 'dummyScanner')
    pc = processor.processor(cfg)

    pc.scan_dataset = scan_dataset()
    pc.scan_dataset.image_artag_filename = ''
    pc.scan_dataset.table_plane_translation = np.matrix([0, 0, 0]).T

    pc.scan_dataset.ground_plane_translation = np.matrix(
        [0, 0, request.laserHeightAboveGround]).T
    pc.scan_dataset.ground_plane_rotation = ''
    #pc.scan_dataset.is_test_set = True
    pc.scan_dataset.is_labeled = True
    pc.scan_dataset.id = 'ROStest'

    pc.img = cvimage
    pc.image_angle = request.imageAngle
    pc.pts3d = pts3d
    pc.intensities = intensities
    pc.scan_indices = np.zeros(len(intensities))

    pc.scan_dataset.polygons.append(polygon)
    pc.do_all_point_cloud()
    pc.do_polygon_mapping()

    pc.scan_dataset.ground_plane_normal = np.matrix([0., 0., 1.]).T

    if request.numberOfPointsToClassify == -1:
        n = 999999999999
    else:
        n = request.numberOfPointsToClassify
    feature_data = pc.generate_features(n, False, True)
    labels, testresults = pc.load_classifier_and_test_on_dataset(
        'all_post', feature_data)

    response = SegmentationResponse()
    response.pointcloud = convert_pointcloud_to_ROS(pc.pts3d_bound,
                                                    pc.intensities_bound,
                                                    labels)
    return response
Example #9
0
def test_segmentation():
    rospy.wait_for_service("segment_pointcloud")
    try:
        pts3d, intensities, labels, image, image_angle, polygon = get_test_data()
        ROS_pointcloud = convert_pointcloud_to_ROS(pts3d, intensities, labels)
        ROS_image = convert_cvimage_to_ROS(image)
        imageSize = cv.cvGetSize(image)

        ROS_polygon = convert_polygon_to_ROS(polygon)

        segment_pointcloud = rospy.ServiceProxy("segment_pointcloud", Segmentation)
        request = SegmentationRequest()
        request.imageWidth = 41
        request.pointcloud = ROS_pointcloud
        request.image = ROS_image
        request.imageWidth = imageSize.width
        request.imageHeight = imageSize.height
        request.imageAngle = image_angle
        request.regionOfInterest2D = ROS_polygon
        request.laserHeightAboveGround = 1.32
        request.numberOfPointsToClassify = 1000  # -1 == all

        response = segment_pointcloud(request)

        pts3d_bound, intensities, labels = convert_ROS_pointcloud_to_pointcloud(response.pointcloud)
        cfg = configuration.configuration("../data/ROS_test_client/", "dummyScanner")
        pc = processor.processor(cfg)

        pc.scan_dataset = scan_dataset()
        pc.scan_dataset.image_artag_filename = ""
        pc.scan_dataset.table_plane_translation = np.matrix([0, 0, 0]).T

        pc.scan_dataset.ground_plane_translation = np.matrix([0, 0, request.laserHeightAboveGround]).T
        pc.scan_dataset.ground_plane_rotation = ""
        pc.scan_dataset.is_labeled = True
        pc.scan_dataset.id = "ROStest"
        pc.image_angle = ""
        pc.pts3d_bound = pts3d_bound
        pc.map_polys = labels
        pc.scan_dataset.ground_plane_normal = np.matrix([0.0, 0.0, 1.0]).T

        from enthought.mayavi import mlab

        pc.display_3d("labels")
        mlab.show()

        return response
    except rospy.ServiceException, e:
        print "Service call failed: %s" % e
Example #10
0
 def slot_display_labels(self):
     if self.display_mode != 'labels':
         if False == self.processor:
             self.processor = processor.processor(self.config)
             
         self.processor.load_data(self.current_dataset.id)
         self.processor.process_labels(self.display_3d_type)
         filename = self.processor.save_labels_image(self.display_3d_type)
         
         self.draw_widget.set_image(filename)
         self.display_mode = 'labels'
     else:
         #display normal image
         self.draw_widget.set_image(self.scans_database.get_path() + '/' + self.current_dataset.image_filename)
         self.display_mode = 'image'               
Example #11
0
 def slot_take_artag_image(self):
     if False == self.scanner:
         self.scanner = scanner.scanner(self.config)
     if False == self.processor:
         self.processor = processor.processor(self.config)
     
     img = self.scanner.take_artag_image()
     self.current_dataset.image_artag_filename = self.scanner.save_artag_image(self.current_dataset.id)
     
     self.slot_save() #save for consistency with files
     
     if self.processor.read_artag(img).any():
         print "SUCCESS in reading ARTag"
     else:
         print "FAILURE in reading ARTag - try again!"
Example #12
0
    def display3d(self):
        import laser_camera_segmentation.processor as processor
        import laser_camera_segmentation.configuration as configuration
        print 'display in 3d...'
        cfg = configuration.configuration(
            '/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling'
        )
        cfg.cam_vec = np.array(self.vals)
        import scanr_transforms as trs
        cfg.camTlaser = trs.camTlaser(cfg.cam_vec)

        pc = processor.processor(cfg)
        pc.load_data(self.dataset_id)
        pc.process_raw_data()
        pc.display_3d('labels', False)
Example #13
0
 def slot_display_features(self):
     if self.display_mode != 'features':
         if False == self.processor:
             self.processor = processor.processor(self.config)
             
         self.processor.load_data(self.current_dataset.id)
         self.processor.process_intensities()
         filename = self.processor.save_intensity_image(self.current_dataset.id)
         self.display_mode = 'features'
         self.draw_widget.set_image(filename)
         
     else:
         #display normal image
         self.display_mode = 'image'
         self.draw_widget.set_image(self.scans_database.get_path() + '/' + self.current_dataset.image_filename)
Example #14
0
    def slot_display_labels(self):
        if self.display_mode != 'labels':
            if False == self.processor:
                self.processor = processor.processor(self.config)

            self.processor.load_data(self.current_dataset.id)
            self.processor.process_labels(self.display_3d_type)
            filename = self.processor.save_labels_image(self.display_3d_type)

            self.draw_widget.set_image(filename)
            self.display_mode = 'labels'
        else:
            #display normal image
            self.draw_widget.set_image(self.scans_database.get_path() + '/' +
                                       self.current_dataset.image_filename)
            self.display_mode = 'image'
Example #15
0
    def slot_take_artag_image(self):
        if False == self.scanner:
            self.scanner = scanner.scanner(self.config)
        if False == self.processor:
            self.processor = processor.processor(self.config)

        img = self.scanner.take_artag_image()
        self.current_dataset.image_artag_filename = self.scanner.save_artag_image(
            self.current_dataset.id)

        self.slot_save()  #save for consistency with files

        if self.processor.read_artag(img).any():
            print "SUCCESS in reading ARTag"
        else:
            print "FAILURE in reading ARTag - try again!"
Example #16
0
    def slot_display_features(self):
        if self.display_mode != 'features':
            if False == self.processor:
                self.processor = processor.processor(self.config)

            self.processor.load_data(self.current_dataset.id)
            self.processor.process_intensities()
            filename = self.processor.save_intensity_image(
                self.current_dataset.id)
            self.display_mode = 'features'
            self.draw_widget.set_image(filename)

        else:
            #display normal image
            self.display_mode = 'image'
            self.draw_widget.set_image(self.scans_database.get_path() + '/' +
                                       self.current_dataset.image_filename)
Example #17
0
def segment_pointcloud(request):
    #convert data from ROS
    pts3d, intensities, labels = convert_ROS_pointcloud_to_pointcloud(request.pointcloud)
    cvimage = convert_ROS_image_to_cvimage(request.image, request.imageWidth, request.imageHeight)
    polygon = convert_ROS_polygon_to_polygon(request.regionOfInterest2D)
    polygon.set_label('surface')
    print polygon, polygon.get_points()
    
    #create processor and configuration
    #cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/ROS_server_test', 'codyRobot')
    cfg = configuration.configuration('../data/ROS_server', 'dummyScanner')
    pc = processor.processor(cfg)
    
    pc.scan_dataset = scan_dataset()
    pc.scan_dataset.image_artag_filename = ''
    pc.scan_dataset.table_plane_translation = np.matrix([0,0,0]).T

    pc.scan_dataset.ground_plane_translation = np.matrix([0,0,request.laserHeightAboveGround]).T 
    pc.scan_dataset.ground_plane_rotation = ''
    #pc.scan_dataset.is_test_set = True
    pc.scan_dataset.is_labeled = True
    pc.scan_dataset.id = 'ROStest'
    
    pc.img = cvimage
    pc.image_angle = request.imageAngle
    pc.pts3d = pts3d
    pc.intensities = intensities
    pc.scan_indices = np.zeros(len(intensities))
    
    pc.scan_dataset.polygons.append(polygon)
    pc.do_all_point_cloud()
    pc.do_polygon_mapping()
    
    pc.scan_dataset.ground_plane_normal = np.matrix([0.,0.,1.]).T
    
    if request.numberOfPointsToClassify == -1:
        n = 999999999999
    else:
        n = request.numberOfPointsToClassify
    feature_data = pc.generate_features(n, False, True)
    labels, testresults = pc.load_classifier_and_test_on_dataset('all_post', feature_data)

    response = SegmentationResponse()
    response.pointcloud = convert_pointcloud_to_ROS(pc.pts3d_bound, pc.intensities_bound, labels)
    return response
Example #18
0
    def slot_take_scan(self):

        #save database, let scanner add dataset, reload it then
        self.slot_save()

        if False == self.scanner:
            self.scanner = scanner.scanner(self.config)
        if False == self.processor:
            self.processor = processor.processor(self.config)

        name = ut.formatted_time()
        self.scanner.capture_and_save(name)
        #self.processor.load_raw_data(name)
        #self.processor.load_metadata(name)
        #self.processor.process_raw_data()
        #self.processor.save_mapped_image(name)
        #self.processor.display_all_data()
        print 'scan ' + name + ' taken'

        self.scans_database.load(self.path, 'database.pkl')

        #proceed to new scan:
        while True == self.slot_next_dataset():
            pass
Example #19
0
 def slot_take_scan(self):
     
     #save database, let scanner add dataset, reload it then
     self.slot_save()
     
     if False == self.scanner:
         self.scanner = scanner.scanner(self.config)
     if False == self.processor:
         self.processor = processor.processor(self.config)
     
     name = ut.formatted_time()
     self.scanner.capture_and_save(name)
     #self.processor.load_raw_data(name)
     #self.processor.load_metadata(name)
     #self.processor.process_raw_data()
     #self.processor.save_mapped_image(name)
     #self.processor.display_all_data()
     print 'scan ' + name + ' taken'
     
     self.scans_database.load(self.path,'database.pkl')
     
     #proceed to new scan: 
     while True == self.slot_next_dataset():
         pass
Example #20
0
 def slot_display_intensity(self):
     if self.display_mode != 'intensities':
         if False == self.processor:
             self.processor = processor.processor(self.config)
             
         #reset ground plane:
         self.current_dataset.ground_plane_normal = ''
         self.current_dataset.ground_plane_three_points = ''
         self.slot_save()    
             
         
         self.processor.load_data(self.current_dataset.id)
         self.processor.process_intensities()
         filename = self.processor.save_intensity_image(self.current_dataset.id)
         
         #self.processor.display_intensities()
         
         self.display_mode = 'intensities'
         self.draw_widget.set_image(filename)
     else:
         #display normal image
         
         self.display_mode = 'image'
         self.draw_widget.set_image(self.scans_database.get_path() + '/' + self.current_dataset.image_filename)
Example #21
0
except ImportError:
    pass

import time 
def getTime():
    return '['+time.strftime("%H:%M:%S", time.localtime())+']'
        
import subprocess        
   
print getTime(), 'start'    

import laser_camera_segmentation.processor as processor
import laser_camera_segmentation.configuration as configuration       
        
cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling')
pc = processor.processor(cfg)

#pc.calculate_and_save_ground_and_table_transformations_for_all_scans(use_RANSAC_table_plane = False)

pc.generate_save_features(True, True)  


folds = 40       
for current_fold in range(folds): #test_crossvalidation_fold.py
    #output = subprocess.Popen(["python", 'test_crossvalidation_fold.py',str(current_fold)], stdout=subprocess.PIPE, env={"ROS_PACKAGE_PATH":"/home/martin/ros/pkgs:/home/martin/gt-ros-pkg/hrl:/home/martin/robot1/src:/home/martin/gt-ros-pkg/hrl:/home/martin/robot1/src:/home/martin/gt-ros-pkg/hrl:/home/martin/robot1/src", "ROS_ROOT":"/home/martin/ros/ros", "PYTHONPATH": "/home/martin/robot1/src/libraries:/home/martin/robot1/src/libraries/katana:/home/martin/robot1/src/libraries:/home/martin/robot1/src/libraries/katana:/home/martin/robot1/src/libraries:/home/martin/robot1/src/libraries/katana:/home/martin/ros/ros/core/roslib/src:/home/martin/robot1/src/libraries:/home/martin/robot1/src/libraries/katana:/usr/local/lib/python2.5/site-packages/m3rt/:/home/martin/gt-ros-pkg/hrl/segway_omni/src:/home/martin/gt-ros-pkg/hrl/hrl_opencv/src:/home/martin/gt-ros-pkg/hrl/force_torque/src:/home/martin/gt-ros-pkg/hrl/hrl_lib/src:/home/martin/gt-ros-pkg/hrl/zenither/src:/home/martin/gt-ros-pkg/hrl/robotis/src:/usr/local/lib/python2.5/site-packages/m3rt/:/home/martin/gt-ros-pkg/hrl/segway_omni/src:/home/martin/gt-ros-pkg/hrl/hrl_opencv/src:/home/martin/gt-ros-pkg/hrl/force_torque/src:/home/martin/gt-ros-pkg/hrl/hrl_lib/src:/home/martin/gt-ros-pkg/hrl/zenither/src:/home/martin/gt-ros-pkg/hrl/robotis/src:/usr/local/lib/python2.5/site-packages/m3rt/:/home/martin/gt-ros-pkg/hrl/segway_omni/src:/home/martin/gt-ros-pkg/hrl/hrl_opencv/src:/home/martin/gt-ros-pkg/hrl/force_torque/src:/home/martin/gt-ros-pkg/hrl/hrl_lib/src:/home/martin/gt-ros-pkg/hrl/zenither/src:/home/martin/gt-ros-pkg/hrl/robotis/src"}).communicate()[0]
    output = subprocess.Popen(["python", 'test_crossvalidation_fold.py',str(current_fold)]).communicate()[0]
    #print getTime(), output
    
    #todo: parallel: don't use communicate() but just create 3 processes with Popen and then wait for them with wait()

Example #22
0
 def slot_load_Classifiers(self):
     if False == self.processor:       
         self.processor = processor.processor(self.config)
     self.processor.load_Classifiers()   
Example #23
0
    def slot_display_global_stats(self):
        if False == self.processor:
            self.processor = processor.processor(self.config)

        self.processor.load_data(self.current_dataset.id)
        self.processor.display_stats(True)
Example #24
0
 def slot_load_Classifiers(self):
     if False == self.processor:
         self.processor = processor.processor(self.config)
     self.processor.load_Classifiers()
Example #25
0
 def slot_display_global_stats(self):
     if False == self.processor:
         self.processor = processor.processor(self.config)
         
     self.processor.load_data(self.current_dataset.id)
     self.processor.display_stats(True)