def declare_cells(p):
return {'main': create_source(*('image_pipeline', 'OpenNISubscriber'), **p)}
default=0,
help='The video device number. Default: %(default)s')
parser.add_argument('--width', dest='width', default=640)
parser.add_argument('--height', dest='height', default=480)
parser.add_argument('--preview', action='store_true')
#ecto options
scheduler_options(parser.add_argument_group('Scheduler Options'))
args = parser.parse_args()
if not os.path.exists(args.output):
os.makedirs(args.output)
#camera = VideoCapture(video_device=1,width=int(args.width),height=int(args.height))
source = create_source(
package_name='image_pipeline',
source_type='OpenNISource') #optionally pass keyword args here...
depth_saver = ImageSaver(
filename_format=os.path.join(args.output, 'frame_%010d_depth.png'))
image_saver = ImageSaver(
filename_format=os.path.join(args.output, 'frame_%010d_image.png'))
#mono_saver = ImageSaver(filename_format=os.path.join(args.output, 'frame_%010d_mono.png'))
plasm = ecto.Plasm()
plasm.connect(
# camera['image'] >> imshow(name='Mono')['image'],
source['depth'] >> imshow(name='Depth')['image'],
source['image'] >> imshow(name='RGB')['image'])
if not args.preview:
plasm.connect(
import argparse
parser = argparse.ArgumentParser(description='Find a plane in an RGBD image.')
scheduler_options(parser.add_argument_group('Scheduler'))
options = parser.parse_args()
return options
if __name__ == '__main__':
options = parse_args()
plasm = ecto.Plasm()
#setup the input source, grayscale conversion
from ecto_openni import SXGA_RES, FPS_15
source = create_source('image_pipeline','OpenNISource',image_mode=SXGA_RES,image_fps=FPS_15)
rgb2gray = cvtColor('Grayscale', flag=Conversion.RGB2GRAY)
plane_finder = PlaneFinder(l=600, nu=100)
depth_to_3d = DepthTo3d()
plasm.connect(source['image'] >> rgb2gray ['image'])
#connect up the pose_est
connections = [ source['image'] >> plane_finder['image'],
source['depth_raw'] >> depth_to_3d['depth'],
source['K'] >> depth_to_3d['K'],
depth_to_3d['points3d'] >> plane_finder['points3d'],
source['K'] >> plane_finder['K'] ]
connections += [plane_finder['image'] >> imshow(name='hulls')[:],
source['image'] >> imshow(name='original')[:]]
plasm.connect(connections)
def create_capture_plasm(bag_name, angle_thresh, segmentation_cell, n_desired=72,
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 = []
# try several parameter combinations
source = create_source('image_pipeline', 'OpenNISource', outputs_list=['K', 'K', 'camera', 'image',
'depth', 'points3d',
'mask_depth'], res=res, fps=fps)
# convert the image to grayscale
rgb2gray = imgproc.cvtColor('rgb -> gray', flag=imgproc.Conversion.RGB2GRAY)
graph += [source['image'] >> rgb2gray[:] ]
# Find planes
plane_est = PlaneFinder(min_size=10000)
compute_normals = ComputeNormals()
graph += [ # find the normals
source['K', 'points3d'] >> compute_normals['K', 'points3d'],
# find the planes
compute_normals['normals'] >> plane_est['normals'],
source['K', 'points3d'] >> plane_est['K', 'points3d'] ]
if orb_template:
# find the pose using ORB
poser = OrbPoseEstimator(directory=orb_template, show_matches=orb_matches)
graph += [ source['image', 'K', 'mask_depth', 'points3d'] >> poser['color_image', 'K', 'mask', 'points3d'],
rgb2gray[:] >> poser['image']
]
else:
# 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 += [ source['image', 'K'] >> poser['color_image', 'K'],
rgb2gray[:] >> poser['image'] ]
# filter the previous pose and resolve the scale ambiguity using 3d
pose_filter = object_recognition_capture.ecto_cells.capture.PlaneFilter();
# make sure the pose is centered at the origin of the plane
graph += [ source['K'] >> pose_filter['K'],
poser['R', 'T'] >> pose_filter['R', 'T'],
plane_est['planes', 'masks'] >> pose_filter['planes', 'masks'] ]
# draw the found pose
pose_drawer = calib.PoseDrawer('Pose Draw')
display = highgui.imshow(name='Poses')
graph += [ pose_filter['found'] >> pose_drawer['trigger'],
poser['debug_image'] >> pose_drawer['image'],
source['K'] >> pose_drawer['K'],
pose_filter['R', 'T'] >> pose_drawer['R', 'T'],
pose_drawer['output'] >> display[:] ]
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'] ]
# publish the source data
rgbMsg = Mat2Image(frame_id='/camera_rgb_optical_frame', swap_rgb=True)
depthMsg = Mat2Image(frame_id='/camera_rgb_optical_frame')
graph += [ source['depth'] >> depthMsg[:],
source['image'] >> rgbMsg[:] ]
# mask out the object
masker = segmentation_cell
graph += [ source['points3d'] >> masker['points3d'],
plane_est['masks', 'planes'] >> masker['masks', 'planes'],
pose_filter['T'] >> masker['T'] ]
# publish the mask
maskMsg = Mat2Image(frame_id='/camera_rgb_optical_frame')
graph += [ masker['mask'] >> maskMsg[:] ]
camera2cv = CameraModelToCv()
cameraMsg = Cv2CameraInfo(frame_id='/camera_rgb_optical_frame')
graph += [source['camera'] >> camera2cv['camera'],
camera2cv['K', 'D', 'image_size'] >> cameraMsg['K', 'D', 'image_size']
]
#display the mask
mask_display = object_recognition_capture.MaskDisplay()
mask_display_highgui = highgui.imshow(name='mask')
graph += [
masker['mask'] >> mask_display['mask'],
source['image'] >> mask_display['image'],
mask_display['image'] >> mask_display_highgui['image'],
]
if not preview:
baggers = dict(image=ImageBagger(topic_name='/camera/rgb/image_color'),
depth=ImageBagger(topic_name='/camera/depth/image'),
mask=ImageBagger(topic_name='/camera/mask'),
pose=PoseBagger(topic_name='/camera/pose'),
image_ci=CameraInfoBagger(topic_name='/camera/rgb/camera_info'),
depth_ci=CameraInfoBagger(topic_name='/camera/depth/camera_info'),
)
bagwriter = ecto.If('Bag Writer if R|T',
cell=ecto_ros.BagWriter(baggers=baggers, bag=bag_name)
)
graph += [
rgbMsg[:] >> bagwriter['image'],
depthMsg[:] >> bagwriter['depth'],
cameraMsg[:] >> (bagwriter['image_ci'], bagwriter['depth_ci']),
poseMsg['pose'] >> bagwriter['pose'],
maskMsg[:] >> bagwriter['mask'],
]
novel = delta_pose['novel']
if use_turn_table:
table = TurnTable(angle_thresh=angle_thresh)
ander = ecto.And()
graph += [
table['trigger'] >> (delta_pose['__test__'], ander['in2']),
delta_pose['novel'] >> ander['in1'],
]
novel = ander['out']
else:
delta_pose.inputs.__test__ = True
graph += [novel >> (bagwriter['__test__'])]
plasm = ecto.Plasm()
plasm.connect(graph)
return (plasm, segmentation_cell) # return segmentation for tuning of parameters.
def declare_cells(self, p):
return {'main': create_source(*('image_pipeline', 'BagReader'), **p)}
import argparse
parser = argparse.ArgumentParser(description='Find a plane in an RGBD image.')
scheduler_options(parser.add_argument_group('Scheduler'))
options = parser.parse_args()
return options
if __name__ == '__main__':
options = parse_args()
plasm = ecto.Plasm()
#setup the input source, grayscale conversion
from ecto_openni import SXGA_RES, FPS_15, VGA_RES, FPS_30
source = create_source('image_pipeline','OpenNISource',image_mode=VGA_RES,image_fps=FPS_30)
rgb2gray = cvtColor('Grayscale', flag=Conversion.RGB2GRAY)
depth_to_3d = DepthTo3d()
compute_normals = {}
draw_normals = {}
normal_types = [ RgbdNormalsTypes.LINEMOD, RgbdNormalsTypes.FALS, RgbdNormalsTypes.SRI ]
normal_types = [ RgbdNormalsTypes.FALS]
for type in normal_types:
compute_normals[type] = ComputeNormals(method=type)
draw_normals[type] = DrawNormals(step=20)
#plasm.connect(source['image'] >> rgb2gray ['image'])
#connect up the pose_est
connections = [ source['depth'] >> depth_to_3d['depth'],
source['K_depth'] >> depth_to_3d['K'],
def declare_cells(self, p):
return {"main": create_source(*("image_pipeline", "OpenNISource"), **p)}
def create_capture_plasm(bag_name, angle_thresh, segmentation_cell, n_desired=72,
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 = []
# try several parameter combinations
source = create_source('image_pipeline', 'OpenNISource', outputs_list=['K_depth', 'K_image', 'camera', 'image',
'depth', 'points3d',
'mask_depth'], res=res, fps=fps)
# convert the image to grayscale
rgb2gray = imgproc.cvtColor('rgb -> gray', flag=imgproc.Conversion.RGB2GRAY)
graph += [source['image'] >> rgb2gray[:] ]
# Find planes
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
source['K_depth', 'points3d'] >> compute_normals['K', 'points3d'],
# find the planes
compute_normals['normals'] >> plane_est['normals'],
source['K_depth', 'points3d'] >> plane_est['K', 'points3d'] ]
if orb_template:
# find the pose using ORB
poser = OrbPoseEstimator(directory=orb_template, show_matches=orb_matches)
graph += [ source['image', 'K_image', 'mask_depth', 'points3d'] >> poser['color_image', 'K_image', 'mask', 'points3d'],
rgb2gray[:] >> poser['image']
]
else:
# 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 += [ source['image', 'K_image'] >> poser['color_image', 'K_image'],
rgb2gray[:] >> poser['image'] ]
# filter the previous pose and resolve the scale ambiguity using 3d
pose_filter = object_recognition_capture.ecto_cells.capture.PlaneFilter();
# make sure the pose is centered at the origin of the plane
graph += [ source['K_depth'] >> pose_filter['K_depth'],
poser['R', 'T'] >> pose_filter['R', 'T'],
plane_est['planes', 'masks'] >> pose_filter['planes', 'masks'] ]
# draw the found pose
pose_drawer = calib.PoseDrawer('Pose Draw')
display = highgui.imshow(name='Poses')
graph += [ pose_filter['found'] >> pose_drawer['trigger'],
poser['debug_image'] >> pose_drawer['image'],
source['K_image'] >> pose_drawer['K'],
pose_filter['R', 'T'] >> pose_drawer['R', 'T'],
pose_drawer['output'] >> display[:] ]
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'] ]
# publish the source data
rgbMsg = Mat2Image(frame_id='/camera_rgb_optical_frame', swap_rgb=True)
depthMsg = Mat2Image(frame_id='/camera_rgb_optical_frame')
graph += [ source['depth'] >> depthMsg[:],
source['image'] >> rgbMsg[:] ]
# mask out the object
masker = segmentation_cell
graph += [ source['points3d'] >> masker['points3d'],
plane_est['masks', 'planes'] >> masker['masks', 'planes'],
pose_filter['T'] >> masker['T'] ]
# publish the mask
maskMsg = Mat2Image(frame_id='/camera_rgb_optical_frame')
graph += [ masker['mask'] >> maskMsg[:] ]
camera2cv = CameraModelToCv()
cameraMsg = Cv2CameraInfo(frame_id='/camera_rgb_optical_frame')
graph += [source['camera'] >> camera2cv['camera'],
camera2cv['K', 'D', 'image_size'] >> cameraMsg['K', 'D', 'image_size']
]
#display the mask
mask_display = object_recognition_capture.MaskDisplay()
mask_display_highgui = highgui.imshow(name='mask')
graph += [
masker['mask'] >> mask_display['mask'],
source['image'] >> mask_display['image'],
mask_display['image'] >> mask_display_highgui['image'],
]
if not preview:
baggers = dict(image=ImageBagger(topic_name='/camera/rgb/image_color'),
depth=ImageBagger(topic_name='/camera/depth/image'),
mask=ImageBagger(topic_name='/camera/mask'),
pose=PoseBagger(topic_name='/camera/pose'),
image_ci=CameraInfoBagger(topic_name='/camera/rgb/camera_info'),
depth_ci=CameraInfoBagger(topic_name='/camera/depth/camera_info'),
)
bagwriter = ecto.If('Bag Writer if R|T',
cell=ecto_ros.BagWriter(baggers=baggers, bag=bag_name)
)
graph += [
rgbMsg[:] >> bagwriter['image'],
depthMsg[:] >> bagwriter['depth'],
cameraMsg[:] >> (bagwriter['image_ci'], bagwriter['depth_ci']),
poseMsg['pose'] >> bagwriter['pose'],
maskMsg[:] >> bagwriter['mask'],
]
novel = delta_pose['novel']
if use_turn_table:
table = TurnTable(angle_thresh=angle_thresh)
ander = ecto.And()
graph += [
table['trigger'] >> (delta_pose['__test__'], ander['in2']),
delta_pose['novel'] >> ander['in1'],
]
novel = ander['out']
else:
delta_pose.inputs.__test__ = True
graph += [novel >> (bagwriter['__test__'])]
plasm = ecto.Plasm()
plasm.connect(graph)
return (plasm, segmentation_cell) # return segmentation for tuning of parameters.
parser = argparse.ArgumentParser(description='Saves images from a connect on keystroke.')
parser.add_argument('-o,--output', dest='output', help='The output directory. Default: %(default)s', default='./images/%s'%(time.strftime('%Y-%b-%d-%H.%M.%S')))
parser.add_argument('-d,--device', dest='device', default=0, help='The video device number. Default: %(default)s')
parser.add_argument('--width',dest='width',default=640)
parser.add_argument('--height',dest='height',default=480)
parser.add_argument('--preview', action='store_true')
#ecto options
scheduler_options(parser.add_argument_group('Scheduler Options'))
args = parser.parse_args()
if not os.path.exists(args.output):
os.makedirs(args.output)
#camera = VideoCapture(video_device=1,width=int(args.width),height=int(args.height))
source = create_source(package_name='image_pipeline', source_type='OpenNISource') #optionally pass keyword args here...
depth_saver = ImageSaver(filename_format=os.path.join(args.output, 'frame_%010d_depth.png'))
image_saver = ImageSaver(filename_format=os.path.join(args.output, 'frame_%010d_image.png'))
#mono_saver = ImageSaver(filename_format=os.path.join(args.output, 'frame_%010d_mono.png'))
plasm = ecto.Plasm()
plasm.connect(
# camera['image'] >> imshow(name='Mono')['image'],
source['depth'] >> imshow(name='Depth')['image'],
source['image'] >> imshow(name='RGB')['image']
)
if not args.preview:
plasm.connect(
source['image'] >> image_saver['image'],
source['depth'] >> depth_saver['image'],
parser = argparse.ArgumentParser(description="Find a plane in an RGBD image.")
scheduler_options(parser.add_argument_group("Scheduler"))
options = parser.parse_args()
return options
if __name__ == "__main__":
options = parse_args()
plasm = ecto.Plasm()
# setup the input source, grayscale conversion
from ecto_openni import SXGA_RES, FPS_15
source = create_source("image_pipeline", "OpenNISource", image_mode=SXGA_RES, image_fps=FPS_15)
rgb2gray = cvtColor("Grayscale", flag=Conversion.RGB2GRAY)
plane_finder = PlaneFinder(l=600, nu=100)
depth_to_3d = DepthTo3d()
plasm.connect(source["image"] >> rgb2gray["image"])
# connect up the pose_est
connections = [
source["image"] >> plane_finder["image"],
source["depth_raw"] >> depth_to_3d["depth"],
source["K"] >> depth_to_3d["K"],
depth_to_3d["points3d"] >> plane_finder["points3d"],
source["K"] >> plane_finder["K"],
]
connections += [plane_finder["image"] >> imshow(name="hulls")[:], source["image"] >> imshow(name="original")[:]]
def declare_cells(p):
return {'main': create_source(*('image_pipeline', 'BagReader'), **p)}
