def display3d(self):
import laser_camera_segmentation.processor as processor
import laser_camera_segmentation.configuration as configuration
print 'display in 3d...'
cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling')
cfg.cam_vec = np.array(self.vals)
import scanr_transforms as trs
cfg.camTlaser = trs.camTlaser(cfg.cam_vec)
pc = processor.processor(cfg)
pc.load_data(self.dataset_id)
pc.process_raw_data()
pc.display_3d('labels', False)
def display3d(self):
import laser_camera_segmentation.processor as processor
import laser_camera_segmentation.configuration as configuration
print 'display in 3d...'
cfg = configuration.configuration(
'/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling'
)
cfg.cam_vec = np.array(self.vals)
import scanr_transforms as trs
cfg.camTlaser = trs.camTlaser(cfg.cam_vec)
pc = processor.processor(cfg)
pc.load_data(self.dataset_id)
pc.process_raw_data()
pc.display_3d('labels', False)
def __init__(self, path = '../data/', device = 'desktopScanner'):
'''
set default values
'''
self.path = path
self.pointcloud_max_dist = 5.0
self.pointcloud_min_dist = 0.1
self.device = device
if device == 'desktopScanner':
import webcam_config as cc
self.webcam_id = 1
#most code from travis scanr-class:
# Initialize webcam
self.cam_name = 'DesktopWebcam'
cp = cc.webcam_parameters[self.cam_name]
fx = cp['focal_length_x_in_pixels']
fy = cp['focal_length_y_in_pixels']
self.cam_proj_mat = np.matrix([[fx, 0, 0, 0],
[0, fy, 0, 0],
[0, 0, 1, 0]])
self.cam_centers = ( cp['optical_center_x_in_pixels'], cp['optical_center_y_in_pixels'] )
# cam_vec came from a previous laser_cam_callibration
#self.cam_vec = np.array([0.8, 0.9, -1.7, 3.1, 0.061, 0.032, -0.035 ])
#self.cam_vec = np.array([1.2000, 1.2000 , -1.4000 , 3.6000 , 0.0600 , 0.0330 ,-0.0200])
#self.cam_vec = np.array([0.9000 , 0.8000 , -2.2000 , 3.1000 , 0.0620 , 0.0320, -0.0270 ])
self.cam_vec = np.array([ 1.8000 , 1.7000 , -2.6000 , 4.7500 , 0.0620 , 0.0320 , -0.0270 ])
#self.cam_vec = np.array([ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ])
self.cam_deltas = np.array([0.1, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001 ])
self.cam_names = ['Ry_0', 'Rz_0', 'Rx_-90', 'Rz_-90', 'dx', 'dy', 'dz']
import scanr_transforms as trs
self.camTlaser = trs.camTlaser(self.cam_vec)
self.scanner_metal_plate_offset = 0.05 #TODO
# Initialize THOK
self.thok_l1 = 0
self.thok_l2 = 0.035
self.thok_tilt_angles = (math.radians(40.0),math.radians(-40.0))
self.thok_devname = '/dev/robot/desktopServos'
self.thok_servonum = 19
self.thok_hoknum = 0
self.thok_scan_speed = math.radians(5.0)
elif device == 'codyRobot':
import hrl_camera.camera_config as cc
self.webcam_id = 0
#values from equilibrium_point_control/lpi.py
self.cam_name = 'mekabotUTM'
cp = cc.camera_parameters[self.cam_name]
fx = cp['focal_length_x_in_pixels']
fy = cp['focal_length_y_in_pixels']
self.cam_proj_mat = np.matrix([[fx, 0, 0, 0],
[0, fy, 0, 0],
[0, 0, 1, 0]])
self.cam_centers = ( cp['optical_center_x_in_pixels'], cp['optical_center_y_in_pixels'] )
#self.camTlaser = mcf.utmcam0Tglobal(mcf.globalTthok0(m),self.image_angle)
# Initialize THOK
self.thok_l1 = 0
self.thok_l2 = -0.055
self.thok_tilt_angles = (math.radians(40.0),math.radians(-40.0))
self.thok_devname = '/dev/robot/servo0'
self.thok_servonum = 5
self.thok_hoknum = 0
self.thok_scan_speed = math.radians(10.0) #speed=10 in lpi
elif device == 'dummyScanner': #just for testing/demonstration without dependencies outside of gt-ros-pkgk
self.webcam_id = 0
#values from equilibrium_point_control/lpi.py
self.cam_name = 'dummyUTM'
import opencv as cv
#values copied from Cody
cp = {'calibration_image_width' : 640.0,
'calibration_image_height' : 480.0,
'focal_length_x_in_pixels' : 362.381,
'focal_length_y_in_pixels' : 362.260,
'optical_center_x_in_pixels' : 275.630,
'optical_center_y_in_pixels' : 267.914,
'lens_distortion_radial_1' : -0.270544,
'lens_distortion_radial_2' : 0.0530850,
'lens_distortion_tangential_1' : 0,
'lens_distortion_tangential_2' : 0,
'opencv_bayer_pattern' : cv.CV_BayerBG2BGR,
'color': True,
'uid': 8520228
}
fx = cp['focal_length_x_in_pixels']
fy = cp['focal_length_y_in_pixels']
self.cam_proj_mat = np.matrix([[fx, 0, 0, 0],
[0, fy, 0, 0],
[0, 0, 1, 0]])
self.cam_centers = ( cp['optical_center_x_in_pixels'], cp['optical_center_y_in_pixels'] )
#self.camTlaser = mcf.utmcam0Tglobal(mcf.globalTthok0(m),self.image_angle)
# Initialize THOK
self.thok_l1 = 0
self.thok_l2 = -0.055
self.thok_tilt_angles = (math.radians(40.0),math.radians(-40.0))
self.thok_devname = '/dev/robot/servo0'
self.thok_servonum = 5
self.thok_hoknum = 0
self.thok_scan_speed = math.radians(10.0) #speed=10 in lpi
else:
print 'ERROR: unknown device',device
def __init__(self, path = '../data/', device = 'desktopScanner', tf_msg = None):
'''
set default values
'''
self.path = path
self.pointcloud_max_dist = 5.0
self.pointcloud_min_dist = 0.1
self.device = device
if device == 'PR2' or device == 'PR2example':
self.cam_name = 'wide_stereo/right/image_rect_color'
fx = 428.48 #focal lengths in pixels
fy = 428.35
self.cam_proj_mat = np.matrix([[fx, 0, 0, 0],
[0, fy, 0, 0],
[0, 0, 1, 0]])
cx = 323.4 #in pixels
cy = 242.9 #in pixels
self.cam_centers = (cx, cy)
self.cam_image_height= 480 #px
self.cam_image_width= 640 #px
#Transform properties will depend on wide_stereo_optical_frame
# -to-base_footprint TF since the PR2
#can move its head relative to the tilting hokuyo.
#
######### EXAMPLE result from TF on PR2 ##############
# header: --
# frame_id: /base_footprint
# child_frame_id: /wide_stereo_optical_frame
# transform:
# translation:
x = 0.111181322026
y= 0.0201393251186 #-0.09 #Using right camera is shifted over by 9cm.
z= 1.39969502374 #+0.051 #- 1.32 #****MY data was in BASE_LINK FRAME??
# rotation: (rotation same as optical frame of Gazebo_R and Gazebo_L_ optical)
rx= -0.625821685412
ry= 0.66370971141
rz= -0.30689909515
rw= 0.271384565597
#euler_angles = transformations.euler_from_quaternion([rx,ry,rz,rw])
#In euler: (-132, -1.4, -94) in degrees.
#####################################################
#kill soon
# Initialize THOK
self.thok_l1 = 0
self.thok_l2 = -0.055
self.thok_tilt_angles = (math.radians(40.0),math.radians(-40.0))
self.thok_devname = '/dev/robot/servo0'
self.thok_servonum = 5
self.thok_hoknum = 0
self.thok_scan_speed = math.radians(10.0) #speed=10 in lpi
if device == 'PR2' and tf_msg:
#requires an appropriate TF message (type=TransformStamped) called tf_msg.
#Condition: header.frame_id = '/base_footprint', child_frame_id = '/wide_stereo_optical_frame'
t = tf_msg.transform.translation
r = tf_msg.transform.rotation
(x,y,z) = (t.x, t.y, t.z)
(rx, ry, rz, rw) = (r.x, r.y, r.z, r.w)
T = transformations.translation_matrix([x,y,z])
R = transformations.quaternion_matrix([rx,ry,rz,rw])
print 'R=',R
print 'T=',T
M = np.matrix(R);
M[:3,3] = np.matrix(T)[:3,3]
print 'M=',M
#hack
M = np.linalg.inv(M)
self.camTlaser = M
# (wrong) TRmatrix = [[-0.06939527, -0.66415251, 0.74436936, 0.11118132],
# [-0.99730322, 0.02832033, -0.06770713, -0.06986067],
# [ 0.02388707, -0.74706051, -0.66432673, 1.39969502],
# [ 0. , 0. , 0. , 1. ]]
#Result is a 4x4 array: [ R | t ]
# [ 0 | 1 ]
# np.array([[ 0.74436936, 0.06939527, 0.66415251, 0. ],
# [-0.06770713, 0.99730322, -0.02832033, 0. ],
# [-0.66432673, -0.02388707, 0.74706051, 0. ],
# [ 0. , 0. , 0. , 1. ]])
elif device == 'desktopScanner':
import scanr_transforms as trs
self.webcam_id = 1
#From webcam_config definition formerly in robot1-->hrl_lib
# Parameter definitions for camera used on desktopScanner
webcam_parameters = {
'DesktopWebcam':
{
'calibration_image_width' : 960.0,
'calibration_image_height' : 720.0,
'focal_length_x_in_pixels' : 794.985,
'focal_length_y_in_pixels' : 797.122,
'optical_center_x_in_pixels' : 491.555,
'optical_center_y_in_pixels' : 344.289,
'lens_distortion_radial_1' : 0.0487641,
'lens_distortion_radial_2' : -0.128722,
'lens_distortion_tangential_1' : 0,
'lens_distortion_tangential_2' : 0,
'opencv_bayer_pattern' : None,
'color': True,
}
}
#most code from travis scanr-class:
# Initialize webcam
self.cam_name = 'DesktopWebcam'
cp = webcam_parameters[self.cam_name]
fx = cp['focal_length_x_in_pixels']
fy = cp['focal_length_y_in_pixels']
self.cam_proj_mat = np.matrix([[fx, 0, 0, 0],
[0, fy, 0, 0],
[0, 0, 1, 0]])
self.cam_centers = ( cp['optical_center_x_in_pixels'], cp['optical_center_y_in_pixels'] )
self.cam_deltas = np.array([0.1, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001 ])
self.cam_names = ['Ry_0', 'Rz_0', 'Rx_-90', 'Rz_-90', 'dx', 'dy', 'dz']
self.cam_vec = np.array([ 1.8000 , 1.7000 , -2.6000 , 4.7500 , 0.0620 , 0.0320 , -0.0270 ])
self.camTlaser = trs.camTlaser(self.cam_vec)
self.scanner_metal_plate_offset = 0.05 #TODO
# Initialize THOK
self.thok_l1 = 0
self.thok_l2 = 0.035
self.thok_tilt_angles = (math.radians(40.0),math.radians(-40.0))
self.thok_devname = '/dev/robot/desktopServos'
self.thok_servonum = 19
self.thok_hoknum = 0
self.thok_scan_speed = math.radians(5.0)
elif device == 'codyRobot' or device == 'dummyScanner':
#just for testing/demonstration without dependencies outside of gt-ros-pkg
self.webcam_id = 0
#values from equilibrium_point_control/lpi.py
self.cam_name = 'mekabotUTM' #also: 'dummyUTM'
#Values copied from Cody
#Please update with current values if they are expected to have changed.
cp = {'calibration_image_width' : 640.0,
'calibration_image_height' : 480.0,
'focal_length_x_in_pixels' : 362.381,
'focal_length_y_in_pixels' : 362.260,
'optical_center_x_in_pixels' : 275.630,
'optical_center_y_in_pixels' : 267.914,
'lens_distortion_radial_1' : -0.270544,
'lens_distortion_radial_2' : 0.0530850,
'lens_distortion_tangential_1' : 0,
'lens_distortion_tangential_2' : 0,
'opencv_bayer_pattern' : 48, #same as cv.CV_BayerBG2BGR
'color': True,
'uid': 8520228
}
fx = cp['focal_length_x_in_pixels']
fy = cp['focal_length_y_in_pixels']
self.cam_proj_mat = np.matrix([[fx, 0, 0, 0],
[0, fy, 0, 0],
[0, 0, 1, 0]])
self.cam_centers = ( cp['optical_center_x_in_pixels'], cp['optical_center_y_in_pixels'] )
#self.camTlaser = mcf.utmcam0Tglobal(mcf.globalTthok0(m),self.image_angle)
# Initialize THOK
self.thok_l1 = 0
self.thok_l2 = -0.055
self.thok_tilt_angles = (math.radians(40.0),math.radians(-40.0))
self.thok_devname = '/dev/robot/servo0'
self.thok_servonum = 5
self.thok_hoknum = 0
self.thok_scan_speed = math.radians(10.0) #speed=10 in lpi
else:
print '[configuration] ERROR: configuration.py: Device "%s" not recognized:' %( device )
print 'Exiting. Cannot fetch transformation and camera properties for this device.'
def __init__(self, path='../data/', device='desktopScanner'):
'''
set default values
'''
self.path = path
self.pointcloud_max_dist = 5.0
self.pointcloud_min_dist = 0.1
self.device = device
if device == 'desktopScanner':
import webcam_config as cc
self.webcam_id = 1
#most code from travis scanr-class:
# Initialize webcam
self.cam_name = 'DesktopWebcam'
cp = cc.webcam_parameters[self.cam_name]
fx = cp['focal_length_x_in_pixels']
fy = cp['focal_length_y_in_pixels']
self.cam_proj_mat = np.matrix([[fx, 0, 0, 0], [0, fy, 0, 0],
[0, 0, 1, 0]])
self.cam_centers = (cp['optical_center_x_in_pixels'],
cp['optical_center_y_in_pixels'])
# cam_vec came from a previous laser_cam_callibration
#self.cam_vec = np.array([0.8, 0.9, -1.7, 3.1, 0.061, 0.032, -0.035 ])
#self.cam_vec = np.array([1.2000, 1.2000 , -1.4000 , 3.6000 , 0.0600 , 0.0330 ,-0.0200])
#self.cam_vec = np.array([0.9000 , 0.8000 , -2.2000 , 3.1000 , 0.0620 , 0.0320, -0.0270 ])
self.cam_vec = np.array(
[1.8000, 1.7000, -2.6000, 4.7500, 0.0620, 0.0320, -0.0270])
#self.cam_vec = np.array([ 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ])
self.cam_deltas = np.array(
[0.1, 0.1, 0.1, 0.1, 0.001, 0.001, 0.001])
self.cam_names = [
'Ry_0', 'Rz_0', 'Rx_-90', 'Rz_-90', 'dx', 'dy', 'dz'
]
import scanr_transforms as trs
self.camTlaser = trs.camTlaser(self.cam_vec)
self.scanner_metal_plate_offset = 0.05 #TODO
# Initialize THOK
self.thok_l1 = 0
self.thok_l2 = 0.035
self.thok_tilt_angles = (math.radians(40.0), math.radians(-40.0))
self.thok_devname = '/dev/robot/desktopServos'
self.thok_servonum = 19
self.thok_hoknum = 0
self.thok_scan_speed = math.radians(5.0)
elif device == 'codyRobot':
import hrl_camera.camera_config as cc
self.webcam_id = 0
#values from equilibrium_point_control/lpi.py
self.cam_name = 'mekabotUTM'
cp = cc.camera_parameters[self.cam_name]
fx = cp['focal_length_x_in_pixels']
fy = cp['focal_length_y_in_pixels']
self.cam_proj_mat = np.matrix([[fx, 0, 0, 0], [0, fy, 0, 0],
[0, 0, 1, 0]])
self.cam_centers = (cp['optical_center_x_in_pixels'],
cp['optical_center_y_in_pixels'])
#self.camTlaser = mcf.utmcam0Tglobal(mcf.globalTthok0(m),self.image_angle)
# Initialize THOK
self.thok_l1 = 0
self.thok_l2 = -0.055
self.thok_tilt_angles = (math.radians(40.0), math.radians(-40.0))
self.thok_devname = '/dev/robot/servo0'
self.thok_servonum = 5
self.thok_hoknum = 0
self.thok_scan_speed = math.radians(10.0) #speed=10 in lpi
elif device == 'dummyScanner': #just for testing/demonstration without dependencies outside of gt-ros-pkgk
self.webcam_id = 0
#values from equilibrium_point_control/lpi.py
self.cam_name = 'dummyUTM'
import opencv as cv
#values copied from Cody
cp = {
'calibration_image_width': 640.0,
'calibration_image_height': 480.0,
'focal_length_x_in_pixels': 362.381,
'focal_length_y_in_pixels': 362.260,
'optical_center_x_in_pixels': 275.630,
'optical_center_y_in_pixels': 267.914,
'lens_distortion_radial_1': -0.270544,
'lens_distortion_radial_2': 0.0530850,
'lens_distortion_tangential_1': 0,
'lens_distortion_tangential_2': 0,
'opencv_bayer_pattern': cv.CV_BayerBG2BGR,
'color': True,
'uid': 8520228
}
fx = cp['focal_length_x_in_pixels']
fy = cp['focal_length_y_in_pixels']
self.cam_proj_mat = np.matrix([[fx, 0, 0, 0], [0, fy, 0, 0],
[0, 0, 1, 0]])
self.cam_centers = (cp['optical_center_x_in_pixels'],
cp['optical_center_y_in_pixels'])
#self.camTlaser = mcf.utmcam0Tglobal(mcf.globalTthok0(m),self.image_angle)
# Initialize THOK
self.thok_l1 = 0
self.thok_l2 = -0.055
self.thok_tilt_angles = (math.radians(40.0), math.radians(-40.0))
self.thok_devname = '/dev/robot/servo0'
self.thok_servonum = 5
self.thok_hoknum = 0
self.thok_scan_speed = math.radians(10.0) #speed=10 in lpi
else:
print 'ERROR: unknown device', device
