def create_capture_plasm(bag_name, angle_thresh, segmentation_cell, n_desired, orb_template='', res=SXGA_RES, fps=FPS_30, orb_matches=False, preview=False, use_turn_table=True): ''' Creates a plasm that will capture openni data into a bag, using a dot pattern to sparsify views. @param bag_name: A filename for the bag, will write to this file. @param angle_thresh: The angle threshhold in radians to sparsify the views with. ''' graph = [] # Create ros node, subscribing to openni topics argv = sys.argv[:] ecto_ros.strip_ros_args(argv) ecto_ros.init(argv, "object_capture_msd_pcl", False) cloud_sub = ecto_ros.ecto_sensor_msgs.Subscriber_PointCloud2("cloud_sub", topic_name='/camera/depth_registered/points') rgb_image_sub = ecto_ros.ecto_sensor_msgs.Subscriber_Image("rgb_image_sub", topic_name='/camera/rgb/image_raw') depth_image_sub = ecto_ros.ecto_sensor_msgs.Subscriber_Image("depth_image_sub", topic_name='/camera/depth_registered/image_raw') rgb_camera_info_sub = ecto_ros.ecto_sensor_msgs.Subscriber_CameraInfo("rgb_camera_info_sub", topic_name='/camera/rgb/camera_info') depth_camera_info_sub = ecto_ros.ecto_sensor_msgs.Subscriber_CameraInfo("depth_camera_info_sub", topic_name='/camera/depth_registered/camera_info') # Converte ros topics to cv types msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud", format=XYZRGB) image2cv = ecto_ros.Image2Mat() depth2cv = ecto_ros.Image2Mat() rgb_info2cv = ecto_ros.CameraInfo2Cv() depth_info2cv = ecto_ros.CameraInfo2Cv() graph += [ cloud_sub['output'] >> msg2cloud[:], rgb_image_sub[:] >> image2cv[:], depth_image_sub[:] >> depth2cv[:], rgb_camera_info_sub[:] >> rgb_info2cv[:], depth_camera_info_sub[:] >> depth_info2cv[:] ] #display the mask display = highgui.imshow(name='Poses') graph += [image2cv[:] >> display[:], ] # convert the image to grayscale # rgb2gray = imgproc.cvtColor('rgb -> gray', flag=imgproc.Conversion.RGB2GRAY) # graph += [image2cv[:] >> rgb2gray[:] ] # get a pose use the dot pattern: there might be a scale ambiguity as this is 3d only poser = OpposingDotPoseEstimator(rows=5, cols=3, pattern_type=calib.ASYMMETRIC_CIRCLES_GRID, square_size=0.04, debug=True) graph += [ image2cv[:] >> poser['color_image'], rgb_info2cv['K'] >> poser['K_image'], image2cv[:] >> poser['image'] ] plasm = ecto.Plasm() plasm.connect(graph) return (plasm, segmentation_cell) # return segmentation for tuning of parameters.
def read_arguments(parser=None, argv=sys.argv): """ Returns: params, pipeline_params, db_dict, db params: all the pipeline parameters specified in the config file or command line pipeline_params: an array of parameters for each pipeline db_dict: the dictionary of the db parameters db: a db object created from those parameters """ if parser is None: parser = ObjectRecognitionParser() parser.add_argument('-c', '--config_file', help='Config file') parser.add_argument('--object_ids', help='If set, it overrides the list of object_ids in the config file') parser.add_argument('--object_names', help='If set, it overrides the list of object names in the config file') parser.add_argument('--visualize', help='If set, it will display some windows with temporary results', default=False, action='store_true') dbtools.add_db_options(parser) try: import ecto_ros ecto_ros.strip_ros_args(argv) except: pass if '--help' in sys.argv or '-h' in sys.argv: args = parser.parse_args() else: args = parser.parse_args()#args=argv) # define the input if args.config_file is None or not os.path.exists(args.config_file): print >>sys.stderr, "The option file does not exist. --help for usage." sys.exit(-1) params = yaml.load(open(args.config_file)) # read some parameters db_params = args_to_db_params(args, params.get('db', {})) # initialize the DB db = dbtools.db_params_to_db(db_params) # read the object_ids object_ids = set() for obj in (args.__dict__, params): ids = obj.get('object_ids', None) names = obj.get('object_names', None) if 'all' in (ids, names): object_ids = set([ str(x.id) for x in models.Object.all(db) ]) #unicode without the str() break if 'missing' in (ids, names): tmp_object_ids = set([ str(x.id) for x in models.Object.all(db) ]) tmp_object_ids_from_names = set([ str(x.object_id) for x in models.Model.all(db) ]) object_ids.update(tmp_object_ids.difference(tmp_object_ids_from_names)) if ids and ids != 'missing': object_ids.update(ids[1:-1].split(',')) if names and names != 'missing': for object_name in names[1:-1].split(','): object_ids.update([str(x.id) for x in models.objects_by_name(db, object_name)]) # if we got some ids through the command line, just stop here if object_ids: break object_ids = list(object_ids) params['object_ids'] = object_ids pipeline_params = [] for key , value in params.iteritems(): if key.startswith('pipeline'): pipeline_params.append(value) args = vars(args) return params, args, pipeline_params, args['visualize'], db_params
'-a', '--angle_thresh', metavar='RADIANS', dest='angle_thresh', type=float, default=math.pi / 36, help='''The delta angular threshold in pose. Default is pi/36 radians. Frames will not be recorded unless they are not closer to any other pose by this amount. ''') from ecto.opts import scheduler_options #add ecto scheduler args. group = parser.add_argument_group('ecto scheduler options') scheduler_options(group, default_scheduler='Threadpool') args = parser.parse_args() if len(args.bag) < 1: print parser.print_help() sys.exit(1) return args if "__main__" == __name__: argv = sys.argv[:] ecto_ros.strip_ros_args(sys.argv) options = parse_args() ecto_ros.init(argv, "openni_capture", False) plasm = openni_capture.create_capture_plasm(options.bag, options.angle_thresh) from ecto.opts import run_plasm run_plasm(options, plasm)
def create_capture_plasm(bag_name, angle_thresh, segmentation_cell, n_desired, sensor, res=SXGA_RES, fps=FPS_30, orb_matches=False, preview=False, use_turn_table=True): ''' Creates a plasm that will capture openni data into a bag, using a dot pattern to sparsify views. @param bag_name: A filename for the bag, will write to this file. @param angle_thresh: The angle threshhold in radians to sparsify the views with. ''' graph = [] # Create ros node, subscribing to openni topics argv = sys.argv[:] ecto_ros.strip_ros_args(argv) ecto_ros.init(argv, "object_capture_msd_pcl", False) cloud_sub = ecto_ros.ecto_sensor_msgs.Subscriber_PointCloud2("cloud_sub", topic_name='/camera/depth_registered/points') # openni&freenect requires '/camera/rgb/image_rect_color', openni2 just need '/camera/rgb/image_raw' if sensor in ['kinect']: rgb_image_sub = ecto_ros.ecto_sensor_msgs.Subscriber_Image("rgb_image_sub", topic_name='/camera/rgb/image_color') elif sensor in ['xtion']: rgb_image_sub = ecto_ros.ecto_sensor_msgs.Subscriber_Image("rgb_image_sub", topic_name='/camera/rgb/image_raw') depth_image_sub = ecto_ros.ecto_sensor_msgs.Subscriber_Image("depth_image_sub", topic_name='/camera/depth_registered/image_raw') rgb_camera_info_sub = ecto_ros.ecto_sensor_msgs.Subscriber_CameraInfo("rgb_camera_info_sub", topic_name='/camera/rgb/camera_info') depth_camera_info_sub = ecto_ros.ecto_sensor_msgs.Subscriber_CameraInfo("depth_camera_info_sub", topic_name='/camera/depth_registered/camera_info') # Converte ros topics to cv types msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud", format=XYZRGB) image2cv = ecto_ros.Image2Mat() depth2cv = ecto_ros.Image2Mat() rgb_info2cv = ecto_ros.CameraInfo2Cv() depth_info2cv = ecto_ros.CameraInfo2Cv() graph += [ cloud_sub['output'] >> msg2cloud[:], rgb_image_sub[:] >> image2cv[:], depth_image_sub[:] >> depth2cv[:], rgb_camera_info_sub[:] >> rgb_info2cv[:], depth_camera_info_sub[:] >> depth_info2cv[:] ] """ rgb_display = highgui.imshow(name='rgb') depth_display = highgui.imshow(name='depth') graph += [image2cv[:] >> rgb_display[:], depth2cv[:] >> depth_display[:], ] # Create pointcloud viewer viewer = CloudViewer("viewer", window_name="Clouds!") graph += [ msg2cloud[:] >> viewer[:] ] """ # Process pointcloud # Cut off some parts cut_x = PassThrough(filter_field_name="x", filter_limit_min=-0.2, filter_limit_max=0.2) cut_y = PassThrough(filter_field_name="y", filter_limit_min=-0.5, filter_limit_max=0.5) cut_z = PassThrough(filter_field_name="z", filter_limit_min=-0.0, filter_limit_max=1.0) graph += [ msg2cloud[:] >> cut_x[:], cut_x[:] >> cut_y[:], cut_y[:] >> cut_z[:] ] # Voxel filter voxel_grid = VoxelGrid("voxel_grid", leaf_size=0.002) graph += [ cut_z[:] >> voxel_grid[:] ] # Find plane and delete it normals = NormalEstimation("normals", k_search=0, radius_search=0.02) graph += [ voxel_grid[:] >> normals[:] ] plane_finder = SACSegmentationFromNormals("planar_segmentation", model_type=SACMODEL_NORMAL_PLANE, eps_angle=0.09, distance_threshold=0.1) plane_deleter = ExtractIndices(negative=True) graph += [ voxel_grid[:] >> plane_finder['input'], normals[:] >> plane_finder['normals'] ] graph += [ voxel_grid[:] >> plane_deleter['input'], plane_finder['inliers'] >> plane_deleter['indices'] ] # Create pointcloud viewer viewer = CloudViewer("viewer", window_name="Clouds!") graph += [ plane_deleter[:] >> viewer[:] ] # Compute vfh vfh_signature = VFHEstimation(radius_search=0.01) graph += [ plane_deleter[:] >> vfh_signature['input'], normals[:] >> vfh_signature['normals'] ] # convert the image to grayscale rgb2gray = imgproc.cvtColor('rgb -> gray', flag=imgproc.Conversion.RGB2GRAY) graph += [image2cv[:] >> rgb2gray[:] ] # get a pose use the dot pattern: there might be a scale ambiguity as this is 3d only poser = OpposingDotPoseEstimator(rows=5, cols=3, pattern_type=calib.ASYMMETRIC_CIRCLES_GRID, square_size=0.04, debug=True) graph += [ image2cv[:] >> poser['color_image'], rgb_info2cv['K'] >> poser['K_image'], rgb2gray[:] >> poser['image'] ] #poser gives us R&T from camera to dot pattern points3d = calib.DepthTo3d() graph += [ depth_info2cv['K'] >> points3d['K'], depth2cv['image'] >> points3d['depth'] ] plane_est = PlaneFinder(min_size=10000) compute_normals = ComputeNormals() # Convert K if the resolution is different (the camera should output that) graph += [ # find the normals depth_info2cv['K'] >> compute_normals['K'], points3d['points3d'] >> compute_normals['points3d'], # find the planes compute_normals['normals'] >> plane_est['normals'], depth_info2cv['K'] >> plane_est['K'], points3d['points3d'] >> plane_est['points3d'] ] pose_filter = object_recognition_capture.ecto_cells.capture.PlaneFilter(); # make sure the pose is centered at the origin of the plane graph += [ depth_info2cv['K'] >> pose_filter['K_depth'], poser['R', 'T'] >> pose_filter['R', 'T'], plane_est['planes', 'masks'] >> pose_filter['planes', 'masks'] ] delta_pose = ecto.If('delta R|T', cell=object_recognition_capture.DeltaRT(angle_thresh=angle_thresh, n_desired=n_desired)) poseMsg = RT2PoseStamped(frame_id='/camera_rgb_optical_frame') graph += [ pose_filter['R', 'T', 'found'] >> delta_pose['R', 'T', 'found'], pose_filter['R', 'T'] >> poseMsg['R', 'T'] ] trainer_ = Trainer() graph += [ pose_filter['R', 'T'] >> trainer_['R', 'T'], vfh_signature['output'] >> trainer_['features'], delta_pose['last'] >> trainer_['last'], delta_pose['novel'] >> trainer_['novel'], plane_deleter[:] >> trainer_['input'] ] novel = delta_pose['novel'] cloud2msg = ecto_pcl_ros.PointCloud2Message("cloud2msg") graph += [ plane_deleter[:] >> cloud2msg[:] ] baggers = dict(points=PointCloudBagger(topic_name='/camera/depth_registered/points'), pose=PoseBagger(topic_name='/camera/pose') ) bagwriter = ecto.If('Bag Writer if R|T', cell=ecto_ros.BagWriter(baggers=baggers, bag=bag_name)) pcd_writer = ecto.If('msd pcd writer', cell = PCDWriter()) graph += [ cloud2msg[:] >> bagwriter['points'], poseMsg['pose'] >> bagwriter['pose'] ] graph += [ plane_deleter['output'] >> pcd_writer['input'] ] delta_pose.inputs.__test__ = True pcd_writer.inputs.__test__ = True graph += [novel >> (bagwriter['__test__'])] graph += [novel >> (pcd_writer['__test__'])] rt2pose = RT2PoseStamped(frame_id = "camera_rgb_optical_frame") rt2pose_filtered = RT2PoseStamped(frame_id = "camera_rgb_optical_frame") posePub = PosePublisher("pose_pub", topic_name='pattern_pose') posePub_filtered = PosePublisher("pose_pub_filtered", topic_name='pattern_pose_filtered') graph += [ poser['R', 'T'] >> rt2pose['R', 'T'], rt2pose['pose'] >> posePub['input'], pose_filter['R', 'T'] >> rt2pose_filtered['R', 'T'], rt2pose_filtered['pose'] >> posePub_filtered['input'] ] plasm = ecto.Plasm() plasm.connect(graph) return (plasm, segmentation_cell) # return segmentation for tuning of parameters.
def read_arguments(parser=None, argv=sys.argv): """ Returns: params, pipeline_params, db_dict, db params: all the pipeline parameters specified in the config file or command line pipeline_params: an array of parameters for each pipeline db_dict: the dictionary of the db parameters db: a db object created from those parameters """ if parser is None: parser = ObjectRecognitionParser() parser.add_argument('-c', '--config_file', help='Config file') parser.add_argument( '--object_ids', help='If set, it overrides the list of object_ids in the config file') parser.add_argument( '--object_names', help='If set, it overrides the list of object names in the config file' ) parser.add_argument( '--visualize', help='If set, it will display some windows with temporary results', default=False, action='store_true') dbtools.add_db_options(parser) try: import ecto_ros ecto_ros.strip_ros_args(argv) except: pass if '--help' in sys.argv or '-h' in sys.argv: args = parser.parse_args() else: args = parser.parse_args() #args=argv) # define the input if args.config_file is None or not os.path.exists(args.config_file): print >> sys.stderr, "The option file does not exist. --help for usage." sys.exit(-1) params = yaml.load(open(args.config_file)) # read some parameters db_params = args_to_db_params(args, params.get('db', {})) # initialize the DB db = dbtools.db_params_to_db(db_params) # read the object_ids object_ids = set() for obj in (args.__dict__, params): ids = obj.get('object_ids', None) names = obj.get('object_names', None) if 'all' in (ids, names): object_ids = set([str(x.id) for x in models.Object.all(db) ]) #unicode without the str() break if 'missing' in (ids, names): tmp_object_ids = set([str(x.id) for x in models.Object.all(db)]) tmp_object_ids_from_names = set( [str(x.object_id) for x in models.Model.all(db)]) object_ids.update( tmp_object_ids.difference(tmp_object_ids_from_names)) if ids and ids != 'missing': object_ids.update(ids[1:-1].split(',')) if names and names != 'missing': for object_name in names[1:-1].split(','): object_ids.update([ str(x.id) for x in models.objects_by_name(db, object_name) ]) # if we got some ids through the command line, just stop here if object_ids: break object_ids = list(object_ids) params['object_ids'] = object_ids pipeline_params = [] for key, value in params.iteritems(): if key.startswith('pipeline'): pipeline_params.append(value) args = vars(args) return params, args, pipeline_params, args['visualize'], db_params
def create_capture_plasm(bag_name, angle_thresh, segmentation_cell, n_desired, orb_template='', res=SXGA_RES, fps=FPS_30, orb_matches=False, preview=False, use_turn_table=True): ''' Creates a plasm that will capture openni data into a bag, using a dot pattern to sparsify views. @param bag_name: A filename for the bag, will write to this file. @param angle_thresh: The angle threshhold in radians to sparsify the views with. ''' graph = [] # Create ros node, subscribing to openni topics argv = sys.argv[:] ecto_ros.strip_ros_args(argv) ecto_ros.init(argv, "object_capture_msd_pcl", False) cloud_sub = ecto_ros.ecto_sensor_msgs.Subscriber_PointCloud2( "cloud_sub", topic_name='/camera/depth_registered/points') rgb_image_sub = ecto_ros.ecto_sensor_msgs.Subscriber_Image( "rgb_image_sub", topic_name='/camera/rgb/image_raw') depth_image_sub = ecto_ros.ecto_sensor_msgs.Subscriber_Image( "depth_image_sub", topic_name='/camera/depth_registered/image_raw') rgb_camera_info_sub = ecto_ros.ecto_sensor_msgs.Subscriber_CameraInfo( "rgb_camera_info_sub", topic_name='/camera/rgb/camera_info') depth_camera_info_sub = ecto_ros.ecto_sensor_msgs.Subscriber_CameraInfo( "depth_camera_info_sub", topic_name='/camera/depth_registered/camera_info') # Converte ros topics to cv types msg2cloud = ecto_pcl_ros.Message2PointCloud("msg2cloud", format=XYZRGB) image2cv = ecto_ros.Image2Mat() depth2cv = ecto_ros.Image2Mat() rgb_info2cv = ecto_ros.CameraInfo2Cv() depth_info2cv = ecto_ros.CameraInfo2Cv() graph += [ cloud_sub['output'] >> msg2cloud[:], rgb_image_sub[:] >> image2cv[:], depth_image_sub[:] >> depth2cv[:], rgb_camera_info_sub[:] >> rgb_info2cv[:], depth_camera_info_sub[:] >> depth_info2cv[:] ] #display the mask display = highgui.imshow(name='Poses') graph += [ image2cv[:] >> display[:], ] # convert the image to grayscale # rgb2gray = imgproc.cvtColor('rgb -> gray', flag=imgproc.Conversion.RGB2GRAY) # graph += [image2cv[:] >> rgb2gray[:] ] # get a pose use the dot pattern: there might be a scale ambiguity as this is 3d only poser = OpposingDotPoseEstimator( rows=5, cols=3, pattern_type=calib.ASYMMETRIC_CIRCLES_GRID, square_size=0.04, debug=True) graph += [ image2cv[:] >> poser['color_image'], rgb_info2cv['K'] >> poser['K_image'], image2cv[:] >> poser['image'] ] plasm = ecto.Plasm() plasm.connect(graph) return (plasm, segmentation_cell ) # return segmentation for tuning of parameters.