def save_data(self, name, metadata=True, angle=None):
dict = {
'laserscans': self.laserscans,
'l1': self.config.thok_l1,
'l2': self.config.thok_l2,
'image_angle': angle
}
prefix = self.config.path + '/data/' + name
print "Saving: " + prefix + '_laserscans.pkl'
ut.save_pickle(dict, prefix + '_laserscans.pkl')
print "Saving: " + prefix + '_image.png'
highgui.cvSaveImage(prefix + '_image.png', self.img)
if metadata:
# save metadata to database:
database = scans_database.scans_database()
database.load(self.config.path, 'database.pkl')
dataset = scan_dataset.scan_dataset()
dataset.id = name
dataset.scan_filename = 'data/' + name + '_laserscans.pkl'
dataset.image_filename = 'data/' + name + '_image.png'
database.add_dataset(dataset)
database.save()
return name
def get_test_data():
cfg = configuration.configuration("../data/ROS_test_client/", "dummyScanner")
pc = processor.processor(cfg)
pc.scan_dataset = scan_dataset()
pc.scan_dataset.image_artag_filename = ""
pc.scan_dataset.table_plane_translation = np.matrix([0, 0, 0]).T
# approximate height of hokuyo above the ground, assume it to be fixed for now.
pc.scan_dataset.ground_plane_translation = np.matrix([0, 0, 1.32]).T
pc.scan_dataset.ground_plane_rotation = ""
# pc.scan_dataset.is_test_set = True
pc.scan_dataset.is_labeled = True
pc.scan_dataset.id = "ROStest"
pc.load_raw_data(pc.scan_dataset.id)
image_size = cv.cvGetSize(pc.img)
# don't do any mapping to not change the pts3d!
# create point cloud from raw range points
(pc.pts3d, pc.scan_indices, pc.intensities) = pc.create_pointcloud(pc.laserscans, True, True)
# region of interest in image pixels
polygon = label_object()
polygon.set_label("surface")
width_min = image_size.width * 0.4
width_max = image_size.width * 0.95
height_min = image_size.height * 0.3
height_max = image_size.height * 0.95
polygon.add_point((width_min, height_min))
polygon.add_point((width_min, height_max))
polygon.add_point((width_max, height_max))
polygon.add_point((width_max, height_min))
return pc.pts3d, pc.intensities, None, pc.img, pc.image_angle, polygon
def get_test_data():
cfg = configuration.configuration('../data/ROS_test_client/',
'dummyScanner')
pc = processor.processor(cfg)
pc.scan_dataset = scan_dataset()
pc.scan_dataset.image_artag_filename = ''
pc.scan_dataset.table_plane_translation = np.matrix([0, 0, 0]).T
#approximate height of hokuyo above the ground, assume it to be fixed for now.
pc.scan_dataset.ground_plane_translation = np.matrix([0, 0, 1.32]).T
pc.scan_dataset.ground_plane_rotation = ''
#pc.scan_dataset.is_test_set = True
pc.scan_dataset.is_labeled = True
pc.scan_dataset.id = 'ROStest'
pc.load_raw_data(pc.scan_dataset.id)
image_size = cv.cvGetSize(pc.img)
#don't do any mapping to not change the pts3d!
#create point cloud from raw range points
(pc.pts3d, pc.scan_indices,
pc.intensities) = pc.create_pointcloud(pc.laserscans, True, True)
#region of interest in image pixels
polygon = label_object()
polygon.set_label('surface')
width_min = image_size.width * 0.4
width_max = image_size.width * 0.95
height_min = image_size.height * 0.3
height_max = image_size.height * 0.95
polygon.add_point((width_min, height_min))
polygon.add_point((width_min, height_max))
polygon.add_point((width_max, height_max))
polygon.add_point((width_max, height_min))
return pc.pts3d, pc.intensities, None, pc.img, pc.image_angle, polygon
def create_default_scan_dataset(unique_id=DATASET_ID, z_above_floor=1.32):
dataset = scan_dataset()
dataset.table_plane_translation = np.matrix([0, 0, 0]).T
# approximate height of hokuyo above the ground, assume it to be fixed for now.
dataset.ground_plane_translation = np.matrix([0, 0, z_above_floor]).T
dataset.ground_plane_rotation = '' #expects a 3x3 numpy matrix
#pc.scan_dataset.is_test_set = True
dataset.is_labeled = IS_LABELED
dataset.id = unique_id
return dataset
def create_default_scan_dataset(unique_id = DATASET_ID, z_above_floor=1.32):
dataset = scan_dataset()
dataset.table_plane_translation = np.matrix([0,0,0]).T
# approximate height of hokuyo above the ground, assume it to be fixed for now.
dataset.ground_plane_translation = np.matrix([0,0,z_above_floor]).T
dataset.ground_plane_rotation = '' #expects a 3x3 numpy matrix
#pc.scan_dataset.is_test_set = True
dataset.is_labeled = IS_LABELED
dataset.id = unique_id
return dataset
def test_segmentation():
rospy.wait_for_service('segment_pointcloud')
try:
pts3d, intensities, labels, image, image_angle, polygon = get_test_data(
)
ROS_pointcloud = convert_pointcloud_to_ROS(pts3d, intensities, labels)
ROS_image = convert_cvimage_to_ROS(image)
imageSize = cv.cvGetSize(image)
ROS_polygon = convert_polygon_to_ROS(polygon)
segment_pointcloud = rospy.ServiceProxy('segment_pointcloud',
Segmentation)
request = SegmentationRequest()
request.imageWidth = 41
request.pointcloud = ROS_pointcloud
request.image = ROS_image
request.imageWidth = imageSize.width
request.imageHeight = imageSize.height
request.imageAngle = image_angle
request.regionOfInterest2D = ROS_polygon
request.laserHeightAboveGround = 1.32
request.numberOfPointsToClassify = 1000 #-1 == all
response = segment_pointcloud(request)
pts3d_bound, intensities, labels = convert_ROS_pointcloud_to_pointcloud(
response.pointcloud)
cfg = configuration.configuration('../data/ROS_test_client/',
'dummyScanner')
pc = processor.processor(cfg)
pc.scan_dataset = scan_dataset()
pc.scan_dataset.image_artag_filename = ''
pc.scan_dataset.table_plane_translation = np.matrix([0, 0, 0]).T
pc.scan_dataset.ground_plane_translation = np.matrix(
[0, 0, request.laserHeightAboveGround]).T
pc.scan_dataset.ground_plane_rotation = ''
pc.scan_dataset.is_labeled = True
pc.scan_dataset.id = 'ROStest'
pc.image_angle = ''
pc.pts3d_bound = pts3d_bound
pc.map_polys = labels
pc.scan_dataset.ground_plane_normal = np.matrix([0., 0., 1.]).T
from enthought.mayavi import mlab
pc.display_3d('labels')
mlab.show()
return response
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def segment_pointcloud(request):
#convert data from ROS
pts3d, intensities, labels = convert_ROS_pointcloud_to_pointcloud(
request.pointcloud)
cvimage = convert_ROS_image_to_cvimage(request.image, request.imageWidth,
request.imageHeight)
polygon = convert_ROS_polygon_to_polygon(request.regionOfInterest2D)
polygon.set_label('surface')
print polygon, polygon.get_points()
#create processor and configuration
#cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/ROS_server_test', 'codyRobot')
cfg = configuration.configuration('../data/ROS_server', 'dummyScanner')
pc = processor.processor(cfg)
pc.scan_dataset = scan_dataset()
pc.scan_dataset.image_artag_filename = ''
pc.scan_dataset.table_plane_translation = np.matrix([0, 0, 0]).T
pc.scan_dataset.ground_plane_translation = np.matrix(
[0, 0, request.laserHeightAboveGround]).T
pc.scan_dataset.ground_plane_rotation = ''
#pc.scan_dataset.is_test_set = True
pc.scan_dataset.is_labeled = True
pc.scan_dataset.id = 'ROStest'
pc.img = cvimage
pc.image_angle = request.imageAngle
pc.pts3d = pts3d
pc.intensities = intensities
pc.scan_indices = np.zeros(len(intensities))
pc.scan_dataset.polygons.append(polygon)
pc.do_all_point_cloud()
pc.do_polygon_mapping()
pc.scan_dataset.ground_plane_normal = np.matrix([0., 0., 1.]).T
if request.numberOfPointsToClassify == -1:
n = 999999999999
else:
n = request.numberOfPointsToClassify
feature_data = pc.generate_features(n, False, True)
labels, testresults = pc.load_classifier_and_test_on_dataset(
'all_post', feature_data)
response = SegmentationResponse()
response.pointcloud = convert_pointcloud_to_ROS(pc.pts3d_bound,
pc.intensities_bound,
labels)
return response
def test_segmentation():
rospy.wait_for_service("segment_pointcloud")
try:
pts3d, intensities, labels, image, image_angle, polygon = get_test_data()
ROS_pointcloud = convert_pointcloud_to_ROS(pts3d, intensities, labels)
ROS_image = convert_cvimage_to_ROS(image)
imageSize = cv.cvGetSize(image)
ROS_polygon = convert_polygon_to_ROS(polygon)
segment_pointcloud = rospy.ServiceProxy("segment_pointcloud", Segmentation)
request = SegmentationRequest()
request.imageWidth = 41
request.pointcloud = ROS_pointcloud
request.image = ROS_image
request.imageWidth = imageSize.width
request.imageHeight = imageSize.height
request.imageAngle = image_angle
request.regionOfInterest2D = ROS_polygon
request.laserHeightAboveGround = 1.32
request.numberOfPointsToClassify = 1000 # -1 == all
response = segment_pointcloud(request)
pts3d_bound, intensities, labels = convert_ROS_pointcloud_to_pointcloud(response.pointcloud)
cfg = configuration.configuration("../data/ROS_test_client/", "dummyScanner")
pc = processor.processor(cfg)
pc.scan_dataset = scan_dataset()
pc.scan_dataset.image_artag_filename = ""
pc.scan_dataset.table_plane_translation = np.matrix([0, 0, 0]).T
pc.scan_dataset.ground_plane_translation = np.matrix([0, 0, request.laserHeightAboveGround]).T
pc.scan_dataset.ground_plane_rotation = ""
pc.scan_dataset.is_labeled = True
pc.scan_dataset.id = "ROStest"
pc.image_angle = ""
pc.pts3d_bound = pts3d_bound
pc.map_polys = labels
pc.scan_dataset.ground_plane_normal = np.matrix([0.0, 0.0, 1.0]).T
from enthought.mayavi import mlab
pc.display_3d("labels")
mlab.show()
return response
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def slot_import_image(self):
fileName = QtGui.QFileDialog.getOpenFileName(self,"Open Image", self.path, "Image Files (*.png)")
print "Import image into new dataset:" + fileName
name = ut.formatted_time()
new_dataset = scan_dataset.scan_dataset()
new_dataset.id = name
new_dataset.image_filename = 'data/'+name+'_image.png'
shutil.copy(fileName,self.path+'/'+new_dataset.image_filename)
self.scans_database.add_dataset(new_dataset)
#proceed to new dataset:
while True == self.slot_next_dataset():
pass
def segment_pointcloud(request):
#convert data from ROS
pts3d, intensities, labels = convert_ROS_pointcloud_to_pointcloud(request.pointcloud)
cvimage = convert_ROS_image_to_cvimage(request.image, request.imageWidth, request.imageHeight)
polygon = convert_ROS_polygon_to_polygon(request.regionOfInterest2D)
polygon.set_label('surface')
print polygon, polygon.get_points()
#create processor and configuration
#cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/ROS_server_test', 'codyRobot')
cfg = configuration.configuration('../data/ROS_server', 'dummyScanner')
pc = processor.processor(cfg)
pc.scan_dataset = scan_dataset()
pc.scan_dataset.image_artag_filename = ''
pc.scan_dataset.table_plane_translation = np.matrix([0,0,0]).T
pc.scan_dataset.ground_plane_translation = np.matrix([0,0,request.laserHeightAboveGround]).T
pc.scan_dataset.ground_plane_rotation = ''
#pc.scan_dataset.is_test_set = True
pc.scan_dataset.is_labeled = True
pc.scan_dataset.id = 'ROStest'
pc.img = cvimage
pc.image_angle = request.imageAngle
pc.pts3d = pts3d
pc.intensities = intensities
pc.scan_indices = np.zeros(len(intensities))
pc.scan_dataset.polygons.append(polygon)
pc.do_all_point_cloud()
pc.do_polygon_mapping()
pc.scan_dataset.ground_plane_normal = np.matrix([0.,0.,1.]).T
if request.numberOfPointsToClassify == -1:
n = 999999999999
else:
n = request.numberOfPointsToClassify
feature_data = pc.generate_features(n, False, True)
labels, testresults = pc.load_classifier_and_test_on_dataset('all_post', feature_data)
response = SegmentationResponse()
response.pointcloud = convert_pointcloud_to_ROS(pc.pts3d_bound, pc.intensities_bound, labels)
return response
def slot_import_image(self):
fileName = QtGui.QFileDialog.getOpenFileName(self, "Open Image",
self.path,
"Image Files (*.png)")
print "Import image into new dataset:" + fileName
name = ut.formatted_time()
new_dataset = scan_dataset.scan_dataset()
new_dataset.id = name
new_dataset.image_filename = 'data/' + name + '_image.png'
shutil.copy(fileName, self.path + '/' + new_dataset.image_filename)
self.scans_database.add_dataset(new_dataset)
#proceed to new dataset:
while True == self.slot_next_dataset():
pass
def save_data(self,name, metadata=True, angle = None):
dict = {'laserscans' : self.laserscans,
'l1': self.config.thok_l1, 'l2': self.config.thok_l2,
'image_angle' : angle}
prefix = self.config.path+'/data/'+name
print "Saving: "+prefix+'_laserscans.pkl'
ut.save_pickle(dict,prefix+'_laserscans.pkl')
print "Saving: "+prefix+'_image.png'
highgui.cvSaveImage(prefix+'_image.png',self.img)
if metadata:
# save metadata to database:
database = scans_database.scans_database()
database.load(self.config.path,'database.pkl')
dataset = scan_dataset.scan_dataset()
dataset.id = name
dataset.scan_filename = 'data/'+name+'_laserscans.pkl'
dataset.image_filename = 'data/'+name+'_image.png'
database.add_dataset(dataset)
database.save()
return name
# OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
# \author Martin Schuster (Healthcare Robotics Lab, Georgia Tech.)
import roslib
roslib.load_manifest('laser_camera_segmentation')
from labeling import label_object, scan_dataset, scans_database
scans_database = scans_database.scans_database()
scans_database.path = '/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling'
scans_database.filename = 'database.pkl'
dataset = scan_dataset.scan_dataset()
dataset.title = 'emtpy'
dataset.id = '0'
dataset.surface_id = '-1'
dataset.scan_filename = ''
dataset.image_filename = ''
scans_database.add_dataset(dataset)
#dataset = scan_dataset.scan_dataset()
#dataset.name = 'test2'
#dataset.scan_filename = ''
#dataset.image_filename = 'data/2009Oct01_112328_image.png'
#scans_database.add_dataset(dataset)
scans_database.save()
# OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
# ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
# \author Martin Schuster (Healthcare Robotics Lab, Georgia Tech.)
import roslib
roslib.load_manifest("laser_camera_segmentation")
from labeling import label_object, scan_dataset, scans_database
scans_database = scans_database.scans_database()
scans_database.path = "/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling"
scans_database.filename = "database.pkl"
dataset = scan_dataset.scan_dataset()
dataset.title = "emtpy"
dataset.id = "0"
dataset.surface_id = "-1"
dataset.scan_filename = ""
dataset.image_filename = ""
scans_database.add_dataset(dataset)
# dataset = scan_dataset.scan_dataset()
# dataset.name = 'test2'
# dataset.scan_filename = ''
# dataset.image_filename = 'data/2009Oct01_112328_image.png'
# scans_database.add_dataset(dataset)
scans_database.save()