def declare_cells(_p):
"""
Implement the virtual function from the base class
Only cells from which something is forwarded have to be declared
"""
cells = {}
cells['throttle'] = ecto.TrueEveryN("Throttle", n=1)
cells['mat2image'] = ecto_ros.Mat2Image("Mat2Image")
cells['subscribed_mat2image'] = ecto.If(
"Subscribed Mat2Image",
input_tendril_name="has_subscribers",
cell=cells['mat2image'])
cells['throttled_mat2image'] = ecto.If(
"Throttled Mat2Image",
input_tendril_name="throttle",
cell=cells['subscribed_mat2image'])
cells['publisher'] = ecto_sensor_msgs.Publisher_Image(
"Publisher",
topic_name="/images/candidate_beans",
queue_size=2,
latched=True)
cells['throttled_publisher'] = ecto.If("Throttled Publisher",
input_tendril_name="throttle",
cell=cells['publisher'])
subscribers_input, subscribers_output = \
ecto.EntangledPair(value=cells['throttled_mat2image'].inputs.at('has_subscribers'), source_name="Has Subscribers Input", sink_name="Has Subscribers Output")
cells['subscribers_input'] = subscribers_input
cells['subscribers_output'] = subscribers_output
return cells
def test_If():
plasm = ecto.Plasm()
g = ecto_test.Generate("Generator", step=1.0, start=1.0)
If = ecto.If(cell=g)
truer = ecto.TrueEveryN(n=3, count=3)
plasm.connect(truer['flag'] >> If['__test__'])
plasm.execute(niter=27)
assert g.outputs.out == 9 #should have only called execute 9 times.
def test_nested_if():
plasm = ecto.Plasm()
g = ecto_test.Generate("Generator", step=1.0, start=1.0)
inside_if = ecto.If(input_tendril_name="on_threes", cell=g)
outside_if = ecto.If(input_tendril_name="on_twos", cell=inside_if)
truer_on_threes = ecto.TrueEveryN(n=3,count=0)
truer_on_twos = ecto.TrueEveryN(n=2,count=0)
plasm.connect([
truer_on_threes['flag'] >> outside_if['on_threes'],
truer_on_twos['flag'] >> outside_if['on_twos']
])
#for x in range(0,18):
# plasm.execute(niter=1)
# print("No of times executed: %s of %s" % (g.outputs.out, x))
# executes on the very first iteration (count = 0) and once every 3*2 iterations thereafter
plasm.execute(niter=18)
assert g.outputs.out == 3 # should have only called execute 3 times.
plasm.execute(niter=1)
assert g.outputs.out == 4 # should have executed once more
parser.add_argument('-i,--input',
dest='input',
help='The input dir. %(default)s',
default='./images')
scheduler_options(parser.add_argument_group('Scheduler'))
options = parser.parse_args()
return options
options = parse_args()
images = ImageReader(path=options.input)
orb_m = ORB(n_features=5000, n_levels=10)
rgb2gray = cvtColor(flag=Conversion.RGB2GRAY)
_stats = ORBstats()
stats = ecto.If(cell=_stats)
stats.inputs.__test__ = False
accum = DescriptorAccumulator()
plasm = ecto.Plasm()
plasm.connect(
images['image'] >> rgb2gray['image'],
rgb2gray['image'] >> orb_m['image'],
orb_m['descriptors'] >> accum[:],
accum[:] >> stats['descriptors'],
)
run_plasm(options, plasm, locals=vars())
_stats.process()
hist = _stats.outputs.distances.toarray()
np.savetxt('hist.txt', hist)
plasm.connect(ir_reader['image'] >> (ir_detector[:], ir_drawer['input']),
rgb_reader['image'] >> (rgb_detector[:], rgb_drawer['input']),
ir_detector['found', 'out'] >> ir_drawer['found', 'points'],
rgb_detector['found', 'out'] >> rgb_drawer['found', 'points'],
rgb_drawer[:] >> rgb_display[:],
ir_drawer[:] >> imshow(name='Depth points')[:])
#triggers
ander = ecto.And(ninput=2)
plasm.connect(
rgb_detector['found'] >> ander['in1'],
ir_detector['found'] >> ander['in2'],
)
left_points, right_points = ecto.If('Left Points',
cell=Points2DAccumulator()), ecto.If(
'Right Points',
cell=Points2DAccumulator())
object_points = ecto.If('Object Points', cell=Points3DAccumulator())
calibrator_ = StereoCalibration()
calibrator = ecto.If('calibrator', cell=calibrator_)
plasm.connect(
rgb_detector['out'] >> left_points['points'],
rgb_detector['ideal'] >> object_points['points'],
ir_detector['out'] >> right_points['points'], ander[:] >>
(left_points['__test__'], right_points['__test__'],
object_points['__test__']),
left_points['stacked'] >> calibrator['points_left'],
object_points['stacked'] >> calibrator['points_object'],
help='The output directory. Default: %(default)s',
default='./images')
#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)
source = create_source(
package_name='image_pipeline',
source_type='OpenNISource') #optionally pass keyword args here...
depth_saver = ecto.If(
'depth saver',
cell=ImageSaver(
filename_format=os.path.join(args.output, 'depth%04d.png')))
image_saver = ecto.If(
'rgb saver',
cell=ImageSaver(
filename_format=os.path.join(args.output, 'image%04d.png')))
display = imshow(name='RGB', triggers=dict(save=ord('s')))
plasm = ecto.Plasm()
plasm.connect(
source['image'] >> (display['image'], image_saver['image']),
source['depth'] >> (imshow(name='Depth')['image'], depth_saver['image']),
display['save'] >> (image_saver['__test__'], depth_saver['__test__']))
run_plasm(args, plasm, locals=vars())
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 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.
#!/usr/bin/env python
import ecto
from ecto_opencv.highgui import VideoCapture, imshow, FPSDrawer, ImageSaver
video_cap = VideoCapture(video_device=0)
fps = FPSDrawer()
video_display = imshow(name='video_cap', triggers=dict(save=ord('s')))
saver = ecto.If(
cell=ImageSaver('saver', filename_format='ecto_image_%05d.jpg', start=1))
plasm = ecto.Plasm()
plasm.connect(
video_cap['image'] >> fps['image'],
fps['image'] >> video_display['image'],
video_display['save'] >> saver['__test__'],
fps['image'] >> saver['image'],
)
if __name__ == '__main__':
from ecto.opts import doit
doit(plasm, description='Save images from video stream.')
#!/usr/bin/env python
import ecto
from ecto_opencv.highgui import VideoCapture, FPSDrawer, imshow, ImageSaver
from ecto_opencv.imgproc import BilateralFilter
video_cap = VideoCapture(video_device=0)
fps = FPSDrawer()
video_display = imshow(
'imshow',
name='video_cap',
triggers=dict(save=ord('s')),
)
saver = ecto.If(
cell=ImageSaver('saver', filename_format='bilateral_%05d.jpg', start=1))
bl_begin = None
bl_end = None
plasm = ecto.Plasm()
#Build up a chain of bilateral filters.
for i in range(1, 10):
next = BilateralFilter(d=-1, sigmaColor=10, sigmaSpace=5)
if bl_begin == None: # beginning case
bl_begin = next
bl_end = next
continue
plasm.connect(bl_end[:] >> next[:])
bl_end = next
for x in files:
if '.png' in x:
num = x.split('_')[1].split('.')[0]
if num.isdigit():
return int(num) + 1
return 0
rgb_start = init_image_dir('rgb')
ir_start = init_image_dir('ir')
print 'rgb start', rgb_start, 'ir_start', ir_start
if (rgb_start != ir_start):
print 'please ensure that the images in ir, and rgb are aligned lexographically'
sys.exit(1)
#saving images
rgb_saver = ecto.If(
cell=ImageSaver(filename_format='rgb/rgb_%04d.png', start=rgb_start))
ir_saver = ecto.If(
cell=ImageSaver(filename_format='ir/ir_%04d.png', start=ir_start))
plasm.connect(rgb2gray[:] >> rgb_saver['image'],
conversion[:] >> ir_saver['image'], ander[:] >>
(rgb_saver['__test__'], ir_saver['__test__']))
sched = ecto.schedulers.Singlethreaded(plasm)
while sched.execute(niter=5) == 0:
#swap it modes
next_mode, capture.params.stream_mode = capture.params.stream_mode, next_mode
print sched.stats()
