00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 import time
00034 import hrl_lib.util as ut
00035 import opencv as cv
00036 import opencv.highgui as hg
00037 import pycessing as pyc
00038 import transforms as tr
00039 import numpy as np,math
00040 import tilting_hokuyo.processing_3d as p3d
00041
00042
00043 def scale(image, s):
00044 scaled = cv.cvCreateImage(cv.cvSize(int(image.width * s), int(image.height * s)), image.depth, image.nChannels)
00045 cv.cvResize(image, scaled, cv.CV_INTER_AREA)
00046 return scaled
00047
00048 class Callib():
00049 def __init__(self, transFunc, seeds, deltas, names, pts, img, cam_proj_mat, cam_centers, zoom, id):
00050 '''
00051 transFunc => takes in param vector, returns homogeneous Xform
00052 seeds => initial param vector (1XM array)
00053 deltas => step sizes in param vector quantities (1XM array)
00054 pts => 3D points to be mapped into img (3XN array)
00055 img => an OpenCV image from the camera
00056 '''
00057
00058 self.dataset_id = id
00059
00060
00061 imgTmp = cv.cvCloneImage(img)
00062 imNP = cv.adaptors.Ipl2NumPy(imgTmp)
00063 self.height = imNP.shape[0] * zoom
00064 self.width = imNP.shape[1] * zoom
00065 self.zoom = zoom
00066
00067 pyc.size(self.width + 200, self.height)
00068
00069 self.pts = pts
00070 self.img = img
00071 self.vals = seeds
00072 self.selected = np.zeros( self.vals.shape[0] )
00073 self.selected[0] = 1.0
00074 self.dels = deltas
00075 self.names = names
00076 self.transFunc = transFunc
00077 self.cam_proj_mat = cam_proj_mat
00078 self.cam_centers = cam_centers
00079 self.display_laser = True
00080
00081
00082 def reDraw(self):
00083 pyc.background(255)
00084 pyc.lightSpecular(255*30/640, 255*30/640, 255*30/640)
00085 pyc.directionalLight(255,255,255,0,0,1)
00086 pyc.specular(102, 102, 102)
00087
00088
00089 imgTmp = cv.cvCloneImage(self.img)
00090 imNP = cv.adaptors.Ipl2NumPy(scale(imgTmp, self.zoom))
00091
00092 color_list = [(255,255,0),(255,0,0),(0,255,255),(0,255,0),(0,0,255),(0,100,100),(100,100,0),
00093 (100,0,100),(100,200,100),(200,100,100),(100,100,200),(100,0,200),(0,200,100),
00094 (0,100,200),(200,0,100),(100,0,100),(255,152,7) ]
00095
00096 XformPts = tr.transform( self.transFunc(self.vals), self.pts )
00097 camPts = self.cam_proj_mat * tr.xyzToHomogenous(XformPts)
00098 camPts = camPts / camPts[2]
00099 camPts[0] = (camPts[0] + self.cam_centers[0]) * self.zoom
00100 camPts[1] = (camPts[1] + self.cam_centers[1]) * self.zoom
00101 camPts = np.matrix( np.round(camPts), 'int')
00102
00103 conditions = np.concatenate([camPts[0] >= 0,
00104 camPts[0] < imNP.shape[1],
00105 camPts[1] >= 0,
00106 camPts[1] < imNP.shape[0]], 0)
00107 r, c = np.where(np.all(conditions, 0))
00108 camPts_bound = camPts[:, c.A[0]]
00109 x = np.asarray(self.pts[0])[0][c.A[0]]
00110 x = x - x.min()
00111 x = x / x.max() * 256
00112 x = np.floor(x)
00113 x = np.asarray(np.matrix(x,'int'))[0]
00114 if self.display_laser:
00115 map2d = np.asarray(camPts_bound[0:2])
00116 n,m = map2d.shape
00117 for i in range(0,m):
00118 imNP[map2d[1,i],map2d[0,i], :] = [x[i],256-x[i],128+x[i]/2]
00119 imgTmp = cv.adaptors.NumPy2Ipl(imNP)
00120
00121
00122 im = cv.adaptors.Ipl2PIL(imgTmp)
00123
00124
00125 pyc.image(im, 0,0, self.width, self.height)
00126
00127
00128
00129
00130 pyc.textSize(10)
00131 for i, val in enumerate(self.vals):
00132 if np.nonzero(self.selected)[0] == i:
00133 print 'x',
00134 print '%8.4f ' % self.vals[i],
00135 pval = '%7s: %8.4f' % (self.names[i], self.vals[i])
00136 pyc.text(pval, self.width + 15, 20 + 20*i, 0)
00137 if np.nonzero(self.selected)[0] == i:
00138 pyc.fill(255,0,0)
00139 pyc.quad(self.width+4.0, 15.0 + 20.0*i - 7.0,
00140 self.width+4.0, 15.0 + 20.0*i + 7.0,
00141 self.width+13.0, 15.0 + 20.0*i,
00142 self.width+13.0, 15.0 + 20.0*i)
00143 print '\n'
00144
00145 self.move(pyc.escape_handler(pyc.draw()))
00146
00147 def move(self, events):
00148 if len(events) > 0:
00149 for event in events:
00150 currselect = np.nonzero( self.selected )[0]
00151 if event.type == pyc.KEYDOWN:
00152 if event.key == pyc.K_DOWN:
00153 self.selected[currselect] = 0
00154 self.selected[ ut.bound(currselect + 1, 0, self.selected.shape[0]-1) ] = 1
00155 if event.key == pyc.K_UP:
00156 self.selected[currselect] = 0
00157 self.selected[ ut.bound(currselect - 1, 0, self.selected.shape[0]-1) ] = 1
00158 if event.key == pyc.K_LEFT:
00159 self.vals[currselect] -= self.dels[currselect]
00160 if event.key == pyc.K_RIGHT:
00161 self.vals[currselect] += self.dels[currselect]
00162 if event.key == pyc.K_SPACE:
00163 self.display_laser = not self.display_laser
00164 if event.key == pyc.K_RETURN:
00165 self.display3d()
00166
00167
00168 return events
00169
00170 def display3d(self):
00171 import laser_camera_segmentation.processor as processor
00172 import laser_camera_segmentation.configuration as configuration
00173 print 'display in 3d...'
00174 cfg = configuration.configuration('/home/martin/robot1_data/usr/martin/laser_camera_segmentation/labeling')
00175 cfg.cam_vec = np.array(self.vals)
00176 import scanr_transforms as trs
00177 cfg.camTlaser = trs.camTlaser(cfg.cam_vec)
00178
00179 pc = processor.processor(cfg)
00180 pc.load_data(self.dataset_id)
00181 pc.process_raw_data()
00182 pc.display_3d('labels', False)
00183
00184
00185 if __name__ == '__main__':
00186 import optparse
00187 p = optparse.OptionParser()
00188 p.add_option('-c', action='store', type='string', dest='image',
00189 default='xxcalib.png',
00190 help='camera image')
00191 p.add_option('-p', action='store', type='string', dest='point_cloud',
00192 default='xxcalib.pkl',
00193 help='pickle file containing a point cloud matrix expressed in the laser\'s frame')
00194 p.add_option('-n', action='store', type='string', dest='camera_name',
00195 default='eleUTM',
00196 help='name of camera as specified in camera_config.py')
00197
00198 opt, args = p.parse_args()
00199 image_filename = opt.image
00200 point_cloud_filename = opt.point_cloud
00201
00202
00203 def cameraTlaser(vec):
00204 x, y, z, r1, r2, r3, r4 = vec
00205 disp = np.matrix([x, y, z]).T
00206 rot1 = tr.Rx(math.radians(r1))
00207 rot2 = tr.Rz(math.radians(r2))
00208 rot3 = tr.Rx(math.radians(r3))
00209 rot4 = tr.Rz(math.radians(r4))
00210 rt = rot4 * rot3 * rot2 * rot1
00211 laserTcam = tr.composeHomogeneousTransform(rt, disp)
00212 trans = tr.invertHomogeneousTransform(laserTcam)
00213 return trans
00214
00215
00216
00217
00218
00219 seeds = np.array([ -0.087, 0.105 , 0.01, 89.8, 89.8, 90.0, 0])
00220
00221
00222 deltas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1, 0.1])
00223
00224 names = ['x_disp', 'y_disp', 'z_disp', 'rotX', 'rotZ', 'rotX', 'rotZ']
00225
00226 img = hg.cvLoadImage(image_filename)
00227 raw_laser_scans = ut.load_pickle(point_cloud_filename)
00228
00229 poses, scans = raw_laser_scans['laserscans'][0]
00230 points_cloud_laser = p3d.generate_pointcloud(poses, scans, math.radians(-180), math.radians(180),
00231 0, .035, max_dist=5, min_dist=.2)
00232
00233
00234
00235
00236 import webcam_config as cc
00237 cp = cc.webcam_parameters['DesktopWebcam']
00238 fx = cp['focal_length_x_in_pixels']
00239 fy = cp['focal_length_y_in_pixels']
00240 cam_proj_mat = np.matrix([[fx, 0, 0, 0],
00241 [0, fy, 0, 0],
00242 [0, 0, 1, 0]])
00243
00244 cam_centers = ( cp['optical_center_x_in_pixels'], cp['optical_center_y_in_pixels'] )
00245
00246 c = Callib(cameraTlaser, seeds, deltas, names, points_cloud_laser, img, cam_proj_mat, cam_centers, 1/1.0)
00247
00248 while True:
00249 c.reDraw()