Skip to content

Commit

Permalink
updated cells to using PclCell and to reflect changes in pose_corrector
Browse files Browse the repository at this point in the history 
API
  • Loading branch information
Mike Krainin committed Sep 3, 2011
1 parent a32fce4 commit 2e84582
Show file tree
Hide file tree
Showing 6 changed files with 149 additions and 143 deletions.
87 changes: 47 additions & 40 deletions scripts/corrector_test.py
View file
Expand Up @@ -8,7 +8,8 @@
import sys
import math

debug = True
visualize = True
debug_graphs = True
dir = "/wg/stor2a/mkrainin/object_data/perception challenge/object_meshes"
rotation = 10*math.pi/180
translation = 0.03
Expand All @@ -24,40 +25,36 @@
"""
sub_plasm = ecto.Plasm()

#subscribers
sub_cloud = ecto_sensor_msgs.Subscriber_PointCloud2("Cloud2 Subscriber",topic_name="/camera/rgb/points")
sub_info = ecto_sensor_msgs.Subscriber_CameraInfo("Cam Info Subscriber",topic_name="/camera/rgb/camera_info")

#converters (PointCloud2 -> type erased PointCloud -> PointCloud<PointXYZ>)
msg2cloud = ecto_pcl_ros.Message2PointCloud("Cloud2 To Type-Erased",format=ecto_pcl.XYZ)
cloud2typed = ecto_pcl.PointCloud2PointCloudT("Type-Erased To XYZ",format=ecto_pcl.XYZ)
#subscribers/converters
sub_cloud = ecto_sensor_msgs.Subscriber_PointCloud2("Cloud2 Subscriber",
topic_name="/camera/rgb/points")
sub_info = ecto_sensor_msgs.Subscriber_CameraInfo("Cam Info Subscriber",
topic_name="/camera/rgb/camera_info")
msg2cloud = ecto_pcl_ros.Message2PointCloud("Cloud2 To Type-Erased",
format=ecto_pcl.XYZ)

#artificial noise
noise_adder = ecto_corrector.AddNoise("Noise Adder",rotation=rotation,translation=translation)
noise_adder = ecto_corrector.AddNoise("Noise Adder",
rotation=rotation,translation=translation)

#edge detection
edge_detector = ecto_corrector.DepthEdgeDetectorXYZ("Edge Detector",depth_threshold=0.02, \
erode_size=3,open_size=3)
edge_detector = ecto_corrector.DepthEdgeDetector("Edge Detector",
depth_threshold=0.02, erode_size=3,open_size=3)

#model loading
model_loader = ecto_corrector.ModelLoader("Model Loader")

#region of interest
roi = ecto_corrector.ModelROI("ROI",expansion=50,binning=2)
apply_roi = ecto_corrector.ApplyROI("Apply ROI")

#pose correction
beam_corrector = ecto_corrector.CorrectorXYZ("Beam Corrector",window_half=2,sigma_pixel=5.0)
icp_corrector = ecto_corrector.CorrectorXYZ("ICP Corrector",use_icp=True,use_sensor_model=False)

#display
pre_correct_vertices = VerticesPubModule(sub_plasm,node_name+"/pre_correct")
post_beam_vertices = VerticesPubModule(sub_plasm,node_name+"/post_beam")
post_icp_vertices = VerticesPubModule(sub_plasm,node_name+"/post_icp")
# beam_corrector = ecto_corrector.Corrector("Beam Corrector",window_half=2,sigma_pixel=5.0)
icp_corrector = ecto_corrector.Corrector("ICP Corrector",use_icp=True,use_sensor_model=False)

sub_graph = [
#conversion
sub_cloud[:] >> msg2cloud[:],
msg2cloud[:] >> cloud2typed[:],

#artificial noise
model_loader[:] >> noise_adder["model"],
Expand All @@ -66,44 +63,53 @@
noise_adder["out_pose"] >> roi["pose"],
model_loader[:] >> roi["model"],
sub_info[:] >> roi["in_camera_info"],
roi[:] >> apply_roi["info"],
msg2cloud[:] >> apply_roi["input"],

#edge detection
cloud2typed[:] >> edge_detector["cloud"],
roi[:] >> edge_detector["cam_info"],

#pre-correct visualization
noise_adder["out_pose"] >> pre_correct_vertices["pose"],
model_loader["model"] >> pre_correct_vertices["model"],
apply_roi[:] >> edge_detector[:],

#beam correction
# noise_adder["out_pose"] >> beam_corrector["input_pose"],
# model_loader["model"] >> beam_corrector["model"],
# roi["out_camera_info"] >> beam_corrector["camera_info"],
# edge_detector["depth_edges"]>> beam_corrector["depth_edges"],
# cloud2typed[:] >> beam_corrector["cloud"],
#
# #beam-correct visualization
# beam_corrector["output_pose"]>> post_beam_vertices["pose"],
# model_loader["model"] >> post_beam_vertices["model"],
# apply_roi[:] >> beam_corrector["input"],

#icp correction
noise_adder["out_pose"] >> icp_corrector["input_pose"],
model_loader["model"] >> icp_corrector["model"],
roi["out_camera_info"] >> icp_corrector["camera_info"],
edge_detector["depth_edges"]>> icp_corrector["depth_edges"],
cloud2typed[:] >> icp_corrector["cloud"],

#icp-correct visualization
icp_corrector["output_pose"]>> post_icp_vertices["pose"],
model_loader["model"] >> post_icp_vertices["model"],
apply_roi[:] >> icp_corrector["input"],
]

if visualize:
pre_correct_vertices = VerticesPubModule(sub_plasm,node_name+"/pre_correct")
# post_beam_vertices = VerticesPubModule(sub_plasm,node_name+"/post_beam")
post_icp_vertices = VerticesPubModule(sub_plasm,node_name+"/post_icp")

sub_graph += [
#pre-correct visualization
noise_adder["out_pose"] >> pre_correct_vertices["pose"],
model_loader["model"] >> pre_correct_vertices["model"],

#beam-correct visualization
# beam_corrector["output_pose"]>> post_beam_vertices["pose"],
# model_loader["model"] >> post_beam_vertices["model"],

#icp-correct visualization
icp_corrector["output_pose"]>> post_icp_vertices["pose"],
model_loader["model"] >> post_icp_vertices["model"],
]

sub_plasm.connect(sub_graph)
if(debug):
if(debug_graphs):
ecto.view_plasm(sub_plasm)

#conditional executer for correction subplams
executer = ecto_X.Executer(plasm=sub_plasm, niter=1, outputs={}, inputs={"in_pose":noise_adder,"ply_file":model_loader})
executer = ecto_X.Executer(plasm=sub_plasm, niter=1, outputs={},
inputs={"in_pose":noise_adder,"ply_file":model_loader})
correction_subgraph_if = ecto.If('Correction if success',cell=executer)

"""
Expand All @@ -114,7 +120,8 @@
main_plasm = ecto.Plasm()

#triggering
sub_image = ecto_sensor_msgs.Subscriber_Image("Image Subscriber",topic_name="/camera/rgb/image_color")
sub_image = ecto_sensor_msgs.Subscriber_Image("Image Subscriber",
topic_name="/camera/rgb/image_color")
img2mat = ecto_ros.Image2Mat("Image to Mat", swap_rgb=True)
show_triggers = {'d_key':ord('d')}
show = ecto_opencv.highgui.imshow("imshow",waitKey=10,triggers=show_triggers)
Expand All @@ -135,10 +142,10 @@
trigger_and["out"] >> correction_subgraph_if["__test__"],

#correction subgraph
tod_detector_if["ply_file","pose"] >> correction_subgraph_if["ply_file","in_pose"] ,
tod_detector_if["ply_file","pose"]>>correction_subgraph_if["ply_file","in_pose"],
]
main_plasm.connect(main_graph)
if(debug):
if(debug_graphs):
ecto.view_plasm(main_plasm)


Expand Down
58 changes: 29 additions & 29 deletions scripts/depth_edge_test.py
View file
@@ -1,40 +1,40 @@
#!/usr/bin/env python

import ecto, ecto_ros, ecto_pcl, ecto_pcl_ros, ecto_sensor_msgs, ecto_opencv.highgui
import ecto_corrector
import ecto, ecto_ros
from ecto_pcl import NiConverter
from ecto_openni import Capture, ResolutionMode, Device
from ecto_opencv.highgui import imshow
from ecto_corrector import DepthEdgeDetector

import sys

debug = True

if __name__ == "__main__":
ecto_ros.init(sys.argv, "depth_edge_test")

#subscribers
sub_cloud = ecto_sensor_msgs.Subscriber_PointCloud2("Cloud2 Subscriber",topic_name="/camera/rgb/points")
sub_info = ecto_sensor_msgs.Subscriber_CameraInfo("Cam Info Subscriber",topic_name="/camera/rgb/camera_info")

#converters (PointCloud2 -> type erased PointCloud -> PointCloud<PointXYZ>)
msg2cloud = ecto_pcl_ros.Message2PointCloud("Cloud2 To Type-Erased",format=ecto_pcl.XYZ)
cloud2typed = ecto_pcl.PointCloud2PointCloudT("Type-Erased To XYZ",format=ecto_pcl.XYZ)
#openni
device = Capture('ni device', rgb_resolution=ResolutionMode.VGA_RES,
depth_resolution=ResolutionMode.VGA_RES,
rgb_fps=30, depth_fps=30,
device_number=0,
registration=True,
synchronize=False,
device=Device.KINECT
)
cloud_generator = NiConverter('cloud_generator')
graph = [ device[:] >> cloud_generator[:], ]

#detection
edge_detector = ecto_corrector.DepthEdgeDetectorXYZ("Edge Detector",depth_threshold=0.02, \
erode_size=3,open_size=3)

#drawing
im_drawer = ecto_opencv.highgui.imshow("Drawer",name="depth edges", waitKey=10)

graph = [
sub_cloud[:] >> msg2cloud[:],
msg2cloud[:] >> cloud2typed[:],
cloud2typed[:] >> edge_detector["cloud"],
sub_info[:] >> edge_detector["cam_info"],
edge_detector[:] >> im_drawer[:]
]

plasm = ecto.Plasm()
plasm.connect(graph)
#detection
edge_detector = DepthEdgeDetector("Edge Detector",depth_threshold=0.02, \
erode_size=3,open_size=3)
graph += [cloud_generator[:] >> edge_detector["input"], ]

#drawing
im_drawer = imshow("Drawer",name="depth edges", waitKey=10)
graph += [edge_detector[:] >> im_drawer[:]]

plasm = ecto.Plasm()
plasm.connect(graph)

if __name__ == "__main__":

if(debug):
ecto.view_plasm(plasm)
Expand Down
83 changes: 47 additions & 36 deletions scripts/segmenter_test.py
View file
@@ -1,48 +1,59 @@
#!/usr/bin/env python

import ecto, ecto_ros, ecto_pcl, ecto_pcl_ros, ecto_sensor_msgs, ecto_opencv.highgui
import ecto_corrector
import ecto, ecto_ros
from ecto_openni import Capture, ResolutionMode, Device
from ecto_pcl import NiConverter, NormalEstimation, KDTREE_ORGANIZED_INDEX
from ecto_corrector import Segmenter
from ecto_opencv.highgui import imshow

import sys

debug = False

#openni capture
device = Capture('ni device', rgb_resolution=ResolutionMode.VGA_RES,
depth_resolution=ResolutionMode.VGA_RES,
rgb_fps=30, depth_fps=30,
device_number=0,
registration=True,
synchronize=False,
device=Device.KINECT
)
cloud_generator = NiConverter('cloud_generator')
graph = [ device[:] >> cloud_generator[:] ]

#normals
normals = NormalEstimation("Normals", k_search=0, radius_search=0.006,
spatial_locator=KDTREE_ORGANIZED_INDEX)
graph += [ cloud_generator[:] >> normals[:] ]

#segmentation
segmenter = Segmenter("Segmenter",pixel_step=2,
depth_threshold=0.0015, #0.0015
normal_threshold=0.98, #0.96
curvature_threshold=10) #not using curvature threshold
graph += [ cloud_generator[:] >> segmenter["input"],
normals[:] >> segmenter["normals"], ]

#drawing
im_drawer = imshow("Drawer",name="segments", waitKey=10)
graph += [ segmenter[:] >> im_drawer[:] ]


plasm = ecto.Plasm()
plasm.connect(graph)

if __name__ == "__main__":
ecto_ros.init(sys.argv, "segmenter_test")

#subscribers
sub_cloud = ecto_sensor_msgs.Subscriber_PointCloud2("Cloud2 Subscriber",topic_name="/camera/rgb/points")

#converters (PointCloud2 -> type erased PointCloud -> PointCloud<PointXYZ>)
msg2cloud = ecto_pcl_ros.Message2PointCloud("Cloud2 To Type-Erased",format=ecto_pcl.XYZ)
#cloud2typed = ecto_pcl.PointCloud2PointCloudT("Type-Erased To XYZ",format=ecto_pcl.XYZ)

#normals
normals = ecto_pcl.NormalEstimation("Normals", k_search=0, radius_search=0.006, spatial_locator=ecto_pcl.KDTREE_ORGANIZED_INDEX)
#normals = ecto_pcl.NormalEstimation("Normals", k_search=20, radius_search=0, spatial_locator=ecto_pcl.KDTREE_FLANN)

#segmentation
segmenter = ecto_corrector.Segmenter("Segmenter",pixel_step=2,
depth_threshold=0.0015, #0.0015
normal_threshold=0.98, #0.96
curvature_threshold=10) #not using curvature threshold

#drawing
im_drawer = ecto_opencv.highgui.imshow("Drawer",name="segments", waitKey=10)

graph = [
sub_cloud[:] >> msg2cloud[:],
msg2cloud[:] >> normals[:],
msg2cloud[:] >> segmenter["input"],
normals[:] >> segmenter["normals"],
segmenter[:] >> im_drawer[:]
]

plasm = ecto.Plasm()
plasm.connect(graph)

if(debug):
ecto.view_plasm(plasm)

sched = ecto.schedulers.Threadpool(plasm)
sched.execute()

if debug:
sched.execute_async()
from IPython.Shell import IPShellEmbed
ipshell = IPShellEmbed()
ipshell()

else:
sched.execute()

0 comments on commit 2e84582

Please sign in to comment.