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.
