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
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')
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()
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()
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)
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)
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)
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
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
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'
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!"
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)
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)
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!"
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)
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
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
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
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)
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()
def slot_load_Classifiers(self): if False == self.processor: self.processor = processor.processor(self.config) self.processor.load_Classifiers()
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)