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 from hrl_lib.util import getTime
00031
00032 import hrl_tilting_hokuyo.processing_3d as p3d
00033 import opencv as cv
00034 from opencv import highgui
00035 import hrl_lib.util as ut
00036 import numpy as np,math
00037 import re
00038
00039 import tf
00040
00041 from enthought.mayavi import mlab
00042
00043
00044 from labeling import scans_database
00045
00046 import opencv.highgui as hg
00047 import subprocess
00048
00049 import hrl_lib.transforms as tr
00050
00051
00052
00053
00054 import matplotlib.pyplot as plt
00055
00056 import scipy.stats as stats
00057
00058 import scipy.spatial.kdtree as kdtree
00059
00060 import copy
00061
00062 import ransac
00063
00064
00065
00066 def get_voi_indices_fancy(pts, poi, depth, width, height):
00067 pts = np.asarray(pts)
00068
00069 conditions = np.multiply(np.multiply(np.multiply(np.multiply(np.multiply(pts[0] < poi[0]+depth/2.0, pts[0] > poi[0]-depth/2.0),
00070 pts[1] < poi[1]+width/2.0), pts[1] > poi[1]-width/2.0),
00071 pts[2] < poi[2]+height/2.0), pts[2] > poi[2]-height/2.0)
00072 return conditions
00073
00074
00075 def rotate_to_plane(normal, points):
00076 n = np.copy(normal)
00077 n = n/np.linalg.norm(n)
00078 up = np.matrix([0,0,1]).T
00079 axis = np.matrix(np.cross(up.T, n.T)).T
00080 axis = axis / np.linalg.norm(axis)
00081 angle = np.arccos((up.T * n)[0,0])
00082 rot = tf.transformations.rotation_matrix(angle,axis)
00083 rot = rot[0:3,0:3].T
00084
00085 pts_rotated = np.asarray(rot * np.asmatrix(points))
00086 return pts_rotated
00087
00088 def draw_plane(table_plane_normal, table_plane_point):
00089
00090 table_plane_normal = np.copy(table_plane_normal)
00091 table_plane_normal /= np.linalg.norm(table_plane_normal)
00092
00093
00094 def intersect(line, plane_normal, plane_point):
00095 gamma = (np.dot(plane_normal.T, plane_point)/np.dot(plane_normal.T, line))[0,0]
00096
00097 return gamma * line
00098
00099 p1 = intersect(np.matrix([[0,1,1]]).T,table_plane_normal,table_plane_point)
00100 p2 = intersect(np.matrix([[0,-1,1]]).T,table_plane_normal,table_plane_point)
00101 p3 = intersect(np.matrix([[1,1,1]]).T,table_plane_normal,table_plane_point)
00102 p4 = intersect(np.matrix([[1,-1,1]]).T,table_plane_normal,table_plane_point)
00103
00104 mlab.triangular_mesh([[float(p1[0]),float(p2[0]),float(p3[0]),float(p4[0])]],
00105 [[float(p1[1]),float(p2[1]),float(p3[1]),float(p4[1])]],
00106 [[float(p1[2]),float(p2[2]),float(p3[2]),float(p4[2])]],
00107 [(0,1,2),(1,2,3)], opacity=.2, colormap='gray')
00108
00109
00110 from gaussian_histogram_features import gaussian_histogram_features
00111 from boosted_tree_classifier import boosted_tree_classifier
00112 from baseline_classifier import baseline_classifier
00113
00114
00115 LABEL_NONE = 0
00116 LABEL_SURFACE = 1
00117 LABEL_CLUTTER = 2
00118 LABEL_ROI = 3
00119
00120
00121
00122 class processor(object):
00123 '''
00124 classdocs
00125 '''
00126 pts = None
00127
00128 classifiers = None
00129
00130 features = None
00131
00132 feature_type = 'gaussian_histograms'
00133 feature_neighborhood = 20
00134 feature_radius = 0.03
00135
00136 classifier_training_size = 900000000
00137
00138
00139 point_of_interest = np.array([0.8,0.0,1.0])
00140 voi_width = 1.4
00141
00142
00143 ground_exclude_treshold = 0.3
00144
00145
00146
00147 features_k_nearest_neighbors = 50
00148
00149
00150 def __init__(self, configuration):
00151 '''
00152 Constructor
00153 '''
00154 self.features = gaussian_histogram_features(self)
00155 self.classifiers = {'range' : boosted_tree_classifier(self, 'range'),
00156 'color' : boosted_tree_classifier(self, 'color'),
00157 'all' : boosted_tree_classifier(self, 'all'),
00158 'baseline' : baseline_classifier(self, 'baseline')}
00159
00160 self.config = configuration
00161
00162 self.artag_transformation = np.array([])
00163
00164 self.img_artag = False
00165
00166 try:
00167 self.scans_database = scans_database.scans_database()
00168 self.scans_database.load(self.config.path,'database.pkl')
00169 except IOError:
00170 print 'WARNING: processor::init: no database found none loaded'
00171
00172
00173
00174 def create_pointcloud(self,laserscans, reject_zero_ten=True, get_intensities=True):
00175 pts = np.array([[],[],[]])
00176 intensities = np.array([[]])
00177 scan_indices = np.array([])
00178 for i, laserscan in enumerate(laserscans):
00179 pos_list,scan_list = laserscan
00180
00181 if get_intensities:
00182 new_pts, new_intensities = p3d.generate_pointcloud(pos_list, scan_list,
00183 math.radians(-180), math.radians(180),
00184 l1=self.config.thok_l1, l2=self.config.thok_l2, save_scan=False,
00185 max_dist=np.Inf, min_dist=-np.Inf,get_intensities=True, reject_zero_ten=reject_zero_ten)
00186 intensities = np.hstack((intensities,new_intensities))
00187 else:
00188 new_pts = p3d.generate_pointcloud(pos_list, scan_list,
00189 math.radians(-180), math.radians(180),
00190 l1=self.config.thok_l1, l2=self.config.thok_l2, save_scan=False,
00191 max_dist=np.Inf, min_dist=-np.Inf,get_intensities=False, reject_zero_ten=reject_zero_ten)
00192
00193 pts = np.hstack((pts,new_pts))
00194
00195
00196
00197 n, m = new_pts[0,:].shape
00198 scan_indices = np.hstack((scan_indices,np.ones(m)*i))
00199
00200 if self.config.device == 'codyRobot' or self.config.device == 'dummyScanner':
00201 pts[1,:] = -pts[1,:]
00202
00203 return (pts, scan_indices, np.asarray(intensities)[0])
00204
00205
00206 def truncate_pointcloud_to_voi(self,poi, depth, width, height):
00207
00208 self.pts3d_bound = np.asarray(self.pts3d_bound)
00209 _,m = np.shape(self.pts3d_bound)
00210 idx = get_voi_indices_fancy(self.pts3d_bound, poi, depth, width, height)
00211 self.pts3d_bound = self.pts3d_bound[:,idx]
00212 _,m_new = np.shape(self.pts3d_bound)
00213 print 'truncated pts3d_bound by VOI from',m,'to',m_new,'points'
00214 self.intensities_bound = self.intensities_bound[idx]
00215
00216
00217 self.map = (np.asarray(self.map[0])[:,idx], self.map[1], self.map[2], self.map[3], self.map[4], self.map[5])
00218
00219 def get_ransac_table_plane_from_labels(self):
00220 labels = self.map_polys
00221 model = ransac.PlaneLeastSquaresModel(False)
00222 data_idx = np.where(np.asarray(labels) == LABEL_SURFACE)[0]
00223 data = np.asarray(self.pts3d_bound).T[data_idx]
00224
00225 try:
00226 model = ransac.ransac(data,model,
00227
00228 3, 1000, 0.04, len(data_idx)/2.5,
00229 debug=False,return_all=False)
00230 except ValueError:
00231
00232 return False
00233 print 'ransac: model',model
00234 return model
00235
00236 def calculate_and_save_ground_and_table_transformations_for_all_scans(self, use_RANSAC_table_plane = True):
00237
00238 self.scans_database.add_attribute_to_every_dataset('ground_plane_rotation')
00239 self.scans_database.add_attribute_to_every_dataset('ground_plane_translation')
00240 self.scans_database.add_attribute_to_every_dataset('table_plane_translation')
00241 self.scans_database.save()
00242 self.scan_dataset = self.scans_database.get_dataset(0)
00243 while False != self.scan_dataset:
00244 if self.scan_dataset.is_labeled:
00245
00246 self.load_data(self.scan_dataset.id, False)
00247 self.process_raw_data(False)
00248
00249 rotation = np.matrix([[1,0,0],[0,1,0],[0,0,1]])
00250 rotation = rotate_to_plane(self.scan_dataset.ground_plane_normal, rotation)
00251 ransac_plane = False
00252 if use_RANSAC_table_plane:
00253 ransac_plane = self.get_ransac_table_plane_from_labels()
00254 if False != ransac_plane:
00255 table_plane_point = np.matrix(ransac_plane[0]).T
00256 table_plane_normal = np.matrix(ransac_plane[1]).T
00257 rotation = rotate_to_plane(table_plane_normal, rotation)
00258 table_plane_point = rotate_to_plane(table_plane_normal,table_plane_point)
00259 elif False == use_RANSAC_table_plane:
00260 print "WARNING: Do NOT USE RANSAC FOR TABLE PLANE ESTIMATION!!!!"
00261 print "using 3pts-groundplane and manual labeled surface height instead"
00262 else:
00263 print "!!!!!!!!"
00264 print "ERROR: NO RANSAC PLANE FOUND!!!!"
00265 print "using 3pts-groundplane and manual labeled surface height instead"
00266 print "!!!!!!!!"
00267
00268 self.load_data(self.scan_dataset.id, True)
00269 self.scans_database.set_internal_pointer_to_dataset(self.scan_dataset.id)
00270
00271 self.scan_dataset.ground_plane_rotation = rotation
00272 self.scan_dataset.ground_plane_translation = self.get_groundplane_translation()
00273 if False != ransac_plane:
00274 table_plane_point += self.scan_dataset.ground_plane_translation
00275 self.scan_dataset.table_plane_translation = np.matrix([0.0,0.0,table_plane_point[2]]).T
00276 else:
00277
00278 self.scan_dataset.table_plane_translation = np.matrix([0.0,0.0,float(self.scan_dataset.surface_height) / 100.0]).T
00279 print 'rot', self.scan_dataset.ground_plane_rotation
00280 print 'gp transl',self.scan_dataset.ground_plane_translation
00281 print 'table transl', self.scan_dataset.table_plane_translation
00282 self.scans_database.save()
00283 self.scan_dataset = self.scans_database.get_next_dataset()
00284
00285 def get_mean_table_plane_height_from_labels(self):
00286 idx = np.where(np.asarray(self.map_polys) == LABEL_SURFACE)
00287 return np.mean(self.pts3d_bound[2,idx])
00288
00289
00290
00291 def map_image_point_on_3d_plane(self, point, ground_plane_rotation, ground_plane_translation, table_plane_translation):
00292
00293 point = np.matrix(np.copy(point))
00294 ground_plane_rotation = np.matrix(np.copy(ground_plane_rotation))
00295 ground_plane_translation = np.matrix(np.copy(ground_plane_translation))
00296 table_plane_translation = np.matrix(np.copy(table_plane_translation))
00297
00298
00299
00300
00301 point[0][0] -= self.config.cam_centers[0]
00302 point[1][0] -= self.config.cam_centers[1]
00303
00304
00305 cam_proj_inv = np.copy(self.config.cam_proj_mat)
00306 cam_proj_inv[0:3,0:3] = np.linalg.inv(cam_proj_inv[0:3,0:3])
00307
00308
00309 line = cam_proj_inv * tr.xyzToHomogenous(point)
00310
00311
00312
00313 if self.config.device == 'codyRobot':
00314
00315 import mekabot.coord_frames as mcf
00316 line = mcf.thok0Tglobal(mcf.globalTutmcam0(line,self.image_angle))
00317 elif self.config.device == 'dummyScanner':
00318 import dummyScanner_coord_frames as dcf
00319 line = dcf.thok0Tglobal(dcf.globalTutmcam0(line,self.image_angle))
00320 else:
00321 laser_to_camera = self.config.camTlaser
00322 camera_to_laser = np.copy(laser_to_camera)
00323 camera_to_laser = np.linalg.inv(camera_to_laser)
00324
00325 line = tr.transform( camera_to_laser, line)
00326
00327
00328 line = ground_plane_rotation * line
00329
00330
00331 table_plane_point = table_plane_translation - ground_plane_translation
00332
00333
00334 table_normal_up = np.matrix([[0,0,1]]).T
00335
00336 gamma = (np.dot(table_normal_up.T, table_plane_point)/np.dot(table_normal_up.T, line))[0,0]
00337 point3d = gamma * line
00338
00339 point3d += ground_plane_translation
00340 return point3d
00341
00342
00343
00344 def map_laser_into_cam_2D(self, pts3d, img, scan_indices, intensities):
00345
00346
00347 imgTmp = cv.cvCloneImage(img)
00348 imNP = cv.adaptors.Ipl2NumPy(imgTmp)
00349
00350 if self.config.device == 'codyRobot':
00351 print '...do mapping using mcf transformations for',self.config.device, 'image angle:', math.degrees(self.image_angle)
00352 import mekabot.coord_frames as mcf
00353 XformPts = mcf.utmcam0Tglobal(mcf.globalTthok0(pts3d),self.image_angle)
00354 elif self.config.device == 'dummyScanner':
00355 import dummyScanner_coord_frames as dcf
00356 XformPts = dcf.utmcam0Tglobal(dcf.globalTthok0(pts3d),self.image_angle)
00357 else:
00358 XformPts = tr.transform( self.config.camTlaser, pts3d)
00359
00360
00361 camPts = self.config.cam_proj_mat * tr.xyzToHomogenous(XformPts)
00362 camPts = camPts / camPts[2]
00363
00364 camPts[0] = camPts[0] + self.config.cam_centers[0]
00365 camPts[1] = camPts[1] + self.config.cam_centers[1]
00366 camPts = np.matrix( np.round(camPts), 'int')
00367
00368 conditions = np.concatenate([camPts[0] >= 0,
00369 camPts[0] < imNP.shape[1],
00370 camPts[1] >= 0,
00371 camPts[1] < imNP.shape[0]], 0)
00372 r, c = np.where(np.all(conditions, 0))
00373 camPts_bound = camPts[:, c.A[0]]
00374
00375 pts3d_bound = pts3d[:, c.A[0]]
00376
00377 scan_indices_bound = scan_indices[c.A[0]]
00378 intensities_bound = intensities[c.A[0]]
00379
00380 return camPts_bound, camPts, c.A[0], pts3d_bound, scan_indices_bound, intensities_bound
00381
00382
00383 def draw_mapped_laser_into_image(self,map,pts3d,img):
00384
00385
00386
00387 x = np.asarray(pts3d)[0][map[2]]
00388 x = x - x.min()
00389 x = x / x.max() * 10
00390 x = np.floor(x)
00391 x = np.asarray(np.matrix(x,'int'))[0]
00392 color_list = [(255,255,0),(255,0,0),(0,255,255),(0,255,0),(0,0,255),(0,100,100),(100,100,0),
00393 (100,0,100),(100,200,100),(200,100,100),(100,100,200),(100,0,200),(0,200,100),
00394 (0,100,200),(200,0,100),(100,0,100),(255,152,7) ]
00395
00396 imgTmp = cv.cvCloneImage(img)
00397 imNP = ut.cv2np(imgTmp,format='BGR')
00398
00399 map2d = np.asarray(map[0][0:2])
00400 n,m = map2d.shape
00401 for i in range(0,m):
00402 imNP[map2d[1,i],map2d[0,i],0] = color_list[x[i]][0]
00403 imNP[map2d[1,i],map2d[0,i],1] = color_list[x[i]][1]
00404 imNP[map2d[1,i],map2d[0,i],2] = color_list[x[i]][2]
00405
00406
00407
00408
00409 img_mapped = ut.np2cv(imNP)
00410 return img_mapped
00411
00412
00413 def draw_mapped_labels_into_image(self, type):
00414
00415
00416
00417 if type == 'labels':
00418 labels = self.map_polys
00419 elif type == 'range':
00420 labels = self.load_Classifier_labels('range')
00421 elif type == 'color':
00422 labels = self.load_Classifier_labels('color')
00423 elif type == 'all':
00424 labels = self.load_Classifier_labels('all')
00425 elif type == 'all_post':
00426 labels = self.load_Classifier_labels('all_post')
00427 elif type == 'baseline':
00428 labels = self.load_Classifier_labels('baseline')
00429 else:
00430 print getTime(), 'ERROR: draw_mapped_labels_into_image(): unsupported feature type:', type
00431
00432
00433 img_mapped = cv.cvCloneImage(self.img)
00434
00435 map2d = np.asarray(self.map[0][0:2])
00436 n,m = map2d.shape
00437 for i in range(0,m):
00438 if labels[i] == LABEL_SURFACE:
00439
00440 cv.cvCircle(img_mapped, cv.cvPoint(map2d[0,i],map2d[1,i]),3, cv.cvScalar(0, 255, 0, 0))
00441
00442
00443 elif labels[i] == LABEL_CLUTTER:
00444
00445 cv.cvCircle(img_mapped, cv.cvPoint(map2d[0,i],map2d[1,i]),3, cv.cvScalar(0, 0, 255, 0))
00446
00447
00448
00449 return img_mapped
00450
00451
00452
00453 def draw_mapped_laser_polygons_into_image(self,map,pts3d,img):
00454
00455
00456 color_list = [(255,255,0),(255,0,0),(0,255,255),(0,255,0),(0,0,255),(0,100,100),(100,100,0),
00457 (100,0,100),(100,200,100),(200,100,100),(100,100,200),(100,0,200),(0,200,100),
00458 (0,100,200),(200,0,100),(100,0,100),(255,152,7) ]
00459
00460 imgTmp = cv.cvCloneImage(img)
00461 imNP = ut.cv2np(imgTmp,format='BGR')
00462
00463 map2d = np.asarray(map[0][0:2])
00464 n,m = map2d.shape
00465
00466 for i in range(0,m):
00467
00468 ximg = map2d[1,i]
00469 yimg = map2d[0,i]
00470
00471 imNP[ximg,yimg,0] = 255
00472 imNP[ximg,yimg,1] = 255
00473 imNP[ximg,yimg,2] = 255
00474
00475 for index, polygon in enumerate(self.scan_dataset.polygons):
00476 if len(polygon.get_points()) and polygon.get_type() == 'polygon':
00477
00478 color_index = min(index, len(color_list)-1)
00479 if 255 == polygon.cvImage[ximg][yimg]:
00480 imNP[ximg,yimg,0] = color_list[color_index][0]
00481 imNP[ximg,yimg,1] = color_list[color_index][1]
00482 imNP[ximg,yimg,2] = color_list[color_index][2]
00483
00484 img_mapped = ut.np2cv(imNP)
00485 return img_mapped
00486
00487
00488
00489 def map_poylgons_into_laser(self, map):
00490 n, m = self.pts3d_bound[0,:].shape
00491 polygon_mapping = list(0 for x in range(m))
00492
00493 map2d = np.asarray(map[0][0:2])
00494 n,m = map2d.shape
00495
00496 for i in range(0,m):
00497 ximg = map2d[1,i]
00498 yimg = map2d[0,i]
00499
00500 poly_count = 1
00501 for polygon in self.scan_dataset.polygons:
00502 if len(polygon.get_points()) and polygon.get_type() == 'polygon':
00503 poly_count = poly_count + 1
00504
00505 if 255 == polygon.cvImage[ximg][yimg]:
00506 polygon_mapping[i] = poly_count
00507
00508 return polygon_mapping
00509
00510
00511 def map_poylgon_labels_into_laser(self, map, exclude_edges = False):
00512 n, m = np.shape(self.pts3d_bound)
00513 polygon_mapping = list(0 for x in range(m))
00514
00515 map2d = np.asarray(map[0][0:2])
00516 n,m = map2d.shape
00517
00518 roi_polygons = []
00519 for polygon in self.scan_dataset.polygons:
00520 if len(polygon.get_points()) and polygon.get_type() == 'polygon' and polygon.get_label() == 'roi':
00521 roi_polygons += [polygon]
00522 surface_polygons = []
00523 for polygon in self.scan_dataset.polygons:
00524 if len(polygon.get_points()) and polygon.get_type() == 'polygon' and polygon.get_label() == 'surface':
00525 surface_polygons += [polygon]
00526 object_polygons = []
00527 for polygon in self.scan_dataset.polygons:
00528 if len(polygon.get_points()) and polygon.get_type() == 'polygon' and polygon.get_label() == 'object':
00529 object_polygons += [polygon]
00530
00531 for i in range(0,m):
00532
00533 ximg = map2d[1,i]
00534 yimg = map2d[0,i]
00535
00536
00537 for polygon in roi_polygons:
00538 if 255 == polygon.cvImage[ximg][yimg] and (exclude_edges == False or 255 != self.scan_dataset.cvEdgeImage[ximg][yimg]):
00539 polygon_mapping[i] = LABEL_ROI
00540
00541
00542
00543 for polygon in surface_polygons:
00544 if 255 == polygon.cvImage[ximg][yimg] and (exclude_edges == False or 255 != self.scan_dataset.cvEdgeImage[ximg][yimg]):
00545
00546 polygon_mapping[i] = LABEL_SURFACE
00547
00548
00549 for polygon in object_polygons:
00550 if 255 == polygon.cvImage[ximg][yimg] and (exclude_edges == False or 255 != self.scan_dataset.cvEdgeImage[ximg][yimg]):
00551 polygon_mapping[i] = LABEL_CLUTTER
00552
00553 return polygon_mapping
00554
00555
00556 def remove_labels_from_low_points(self):
00557
00558 idx = np.where(self.pts3d_bound[2,:] < self.ground_exclude_treshold)
00559 self.map_polys = np.asarray(self.map_polys)
00560 self.map_polys[idx] = LABEL_NONE
00561
00562
00563 def create_intensity_image(self, laserscan):
00564
00565 pos_list, scan_list = laserscan
00566
00567 for i,s in enumerate(scan_list):
00568 if 0 == i:
00569 imgNP = s.intensities
00570 else:
00571 imgNP = np.vstack((imgNP,s.intensities))
00572
00573 imgNP = imgNP / np.max(imgNP) * 255
00574 imgNP = np.asarray(imgNP)
00575 imgNP = imgNP[::-1]
00576
00577 return ut.np2cv(imgNP)
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614 def do_all_laser_image_mapping(self, translate = True):
00615 self.map = self.map_laser_into_cam_2D(self.pts3d, self.img, self.scan_indices, self.intensities)
00616 self.pts3d_bound = np.asarray(self.map[3])
00617 self.rotate_pointcloud_match_groundplane()
00618 if translate == True:
00619 self.z_translate_pointcloud_groundplane()
00620 self.scan_indices_bound = self.map[4]
00621 self.intensities_bound = self.map[5]
00622 self.img_mapped = self.draw_mapped_laser_into_image(self.map, self.pts3d, self.img)
00623 self.create_polygon_images()
00624 self.img_mapped_polygons = self.draw_mapped_laser_polygons_into_image(self.map, self.pts3d, self.img)
00625
00626
00627 self.map_polys = self.map_poylgon_labels_into_laser(self.map)
00628 if translate == True:
00629 self.remove_labels_from_low_points()
00630
00631 self.img_intensities = self.create_intensity_image(self.laserscans[0])
00632
00633
00634
00635
00636 def do_all_point_cloud_mapping(self, exclude_edges = False):
00637 self.do_all_point_cloud()
00638 self.do_polygon_mapping(exclude_edges)
00639
00640
00641 def do_polygon_mapping(self, exclude_edges = False):
00642 self.create_polygon_images()
00643 self.map_polys = self.map_poylgon_labels_into_laser(self.map, exclude_edges)
00644 self.remove_labels_from_low_points()
00645
00646 def do_all_point_cloud(self):
00647 self.map = self.map_laser_into_cam_2D(self.pts3d, self.img, self.scan_indices, self.intensities)
00648 self.pts3d_bound = np.asarray(self.map[3])
00649 self.rotate_pointcloud_match_groundplane()
00650 self.z_translate_pointcloud_groundplane()
00651 self.scan_indices_bound = self.map[4]
00652 self.intensities_bound = self.map[5]
00653
00654
00655 def load_data(self, name, reload_database = True):
00656 print getTime(), 'processor: loading ', name
00657 self.load_metadata(name, reload_database)
00658 self.load_raw_data(name)
00659
00660 def load_raw_data(self,name):
00661 self.img = highgui.cvLoadImage(self.config.path+'/data/'+name+'_image.png')
00662 dict = ut.load_pickle(self.config.path+'/data/'+name+'_laserscans.pkl')
00663 self.laserscans = dict['laserscans']
00664 if 'image_angle' in dict:
00665 self.image_angle = dict['image_angle']
00666 else:
00667 self.image_angle = 0
00668
00669 if '' != self.scan_dataset.image_artag_filename:
00670 self.img_artag = highgui.cvLoadImage(self.config.path+'/'+self.scan_dataset.image_artag_filename)
00671 print getTime(), 'read artag image: '+self.config.path+'/'+self.scan_dataset.image_artag_filename
00672
00673
00674 def display_all_data(self):
00675
00676
00677
00678
00679
00680
00681
00682
00683 window_name = "Image Poly Mapped"
00684 highgui.cvNamedWindow (window_name, highgui.CV_WINDOW_AUTOSIZE)
00685 highgui.cvShowImage (window_name, self.img_mapped_polygons)
00686
00687
00688
00689
00690
00691
00692
00693
00694
00695 def display_segmentation_image(self):
00696 self.img_labels = self.draw_mapped_labels_into_image('all_post')
00697 window_name = "Image Labels for classifier 'all_post'"
00698 highgui.cvNamedWindow (window_name, highgui.CV_WINDOW_AUTOSIZE)
00699 highgui.cvShowImage (window_name, self.img_labels)
00700 highgui.cvWaitKey(0)
00701
00702 def save_segmentation_image(self):
00703 self.img_labels = self.draw_mapped_labels_into_image('all_post')
00704 filename = self.config.path+'/data/'+self.scan_dataset.id+'_image_segmentation_all_post.png'
00705 print getTime(), "Saving: "+filename
00706 highgui.cvSaveImage(filename,self.img_labels)
00707
00708 def get_intensity_image(self):
00709 return self.img_intensities
00710
00711 def display_intensities(self):
00712 window_name = "Image Intensities"
00713 highgui.cvNamedWindow (window_name, highgui.CV_WINDOW_AUTOSIZE)
00714 highgui.cvShowImage (window_name, self.img_intensities)
00715
00716
00717 def display_featurevector(self, featurevector):
00718 fv_range = np.asarray(featurevector)[self.features.get_indexvector('range')]
00719
00720 idx = np.asarray(range(len(self.features.get_indexvector('range'))))
00721 plot1 = plt.bar(idx,fv_range,color='b')
00722 print 'fv_range',fv_range
00723 plt.show()
00724
00725
00726 def display_stats(self, global_stats = False):
00727 import colorsys
00728
00729 h_values_plot = []
00730 s_values_plot = []
00731 v_values_plot = []
00732 i_values_plot = []
00733 colors_plot = []
00734 rgb_plot = []
00735
00736 range_hist_surface_plot = np.zeros(len(self.features.get_indexvector('range')))
00737 range_hist_clutter_plot = np.zeros(len(self.features.get_indexvector('range')))
00738
00739 count_surface = 0
00740 count_clutter = 0
00741
00742
00743 normals_x = []
00744 normals_y = []
00745 normals_z = []
00746 normals_label = []
00747
00748 measured_table_heights = []
00749
00750 surface_heights = []
00751 clutter_heights = []
00752
00753
00754 filename = self.get_features_filename()
00755 dict = ut.load_pickle(filename)
00756 loop_condition = True
00757 first_loop_run = True
00758
00759 dataset_count = 0
00760 last_surface_id = -1
00761 while loop_condition == True:
00762 count = 0
00763 if global_stats == True:
00764 if first_loop_run == True:
00765 self.scan_dataset = self.scans_database.get_dataset(0)
00766 first_loop_run = False
00767 else:
00768 print 'next'
00769 self.scan_dataset = self.scans_database.get_next_dataset()
00770
00771 if False == self.scan_dataset:
00772 break
00773
00774 if True != self.scan_dataset.is_labeled:
00775 continue
00776
00777 print 'load',filename
00778 filename = self.get_features_filename()
00779 dict = ut.load_pickle(filename)
00780 dataset_count += 1
00781
00782 if self.scan_dataset.surface_id != last_surface_id:
00783 measured_table_heights += [float(self.scan_dataset.surface_height)]
00784 last_surface_id = self.scan_dataset.surface_id
00785 else:
00786 loop_condition = False
00787
00788
00789 for index in dict['point_indices']:
00790
00791 fv_hs = (dict['features'][count])[self.features.get_indexvector('hsvi')]
00792 if dict['labels'][count] == LABEL_SURFACE or dict['labels'][count] == LABEL_CLUTTER:
00793 h_values_plot.append(fv_hs[0])
00794 s_values_plot.append(fv_hs[1])
00795 v_values_plot.append(fv_hs[2])
00796 i_values_plot.append(fv_hs[3])
00797
00798
00799 rvalue,gvalue,bvalue = colorsys.hsv_to_rgb(fv_hs[0], fv_hs[1], fv_hs[2])
00800
00801 rgb_plot.append([rvalue,gvalue,bvalue])
00802
00803 fv_range = (dict['features'][count])[self.features.get_indexvector('range')]
00804
00805
00806 if dict['labels'][count] == LABEL_SURFACE:
00807
00808 colors_plot.append(1)
00809 range_hist_surface_plot += np.asarray(fv_range)
00810 count_surface += 1
00811 elif dict['labels'][count] == LABEL_CLUTTER:
00812
00813 colors_plot.append(2)
00814 range_hist_clutter_plot += np.asarray(fv_range)
00815 count_clutter += 1
00816
00817
00818
00819 if len(fv_range) > 2:
00820 normals_x += [fv_range[1]]
00821 normals_y += [fv_range[2]]
00822 normals_z += [fv_range[3]]
00823 normals_label += [dict['labels'][count]]
00824
00825 count +=1
00826
00827
00828
00829
00830 print 'load data for ',self.scan_dataset.id
00831 self.load_data(self.scan_dataset.id, False)
00832 (self.pts3d, self.scan_indices, self.intensities) = self.create_pointcloud(self.laserscans)
00833 self.do_all_point_cloud_mapping()
00834
00835
00836 surface_height_sum_this_table = 0
00837 surface_count_this_table = 0
00838 for index, label in enumerate(self.map_polys):
00839 if label == LABEL_SURFACE:
00840 surface_height_sum_this_table += np.asarray(self.pts3d_bound)[2,index]
00841 surface_count_this_table += 1
00842
00843 surface_height_mean_this_table = surface_height_sum_this_table / float(surface_count_this_table)
00844
00845 for index, label in enumerate(self.map_polys):
00846 height = np.asarray(self.pts3d_bound)[2,index]
00847 height -= surface_height_mean_this_table
00848 if label == LABEL_CLUTTER:
00849 clutter_heights += [height]
00850 elif label == LABEL_SURFACE:
00851 surface_heights += [height]
00852
00853 surface_heights = np.asarray(surface_heights)
00854 clutter_heights = np.asarray(clutter_heights)
00855
00856 surface_heights = surface_heights * 100
00857 clutter_heights = clutter_heights * 100
00858
00859
00860
00861 print clutter_heights
00862 fig_clutterheights = plt.figure()
00863 plt.title('clutter and surface heights above measured height of table')
00864
00865 plt.xlabel('height in cm')
00866 plt.ylabel('number of range measurements (points)')
00867 width = 0.35
00868 bins = np.asarray(range(-10,60))
00869 print 'len clutterheights',len(clutter_heights)
00870 surface_heights = surface_heights[np.where(surface_heights < 60)]
00871 surface_heights = surface_heights[np.where(surface_heights > -10)]
00872 clutter_heights = clutter_heights[np.where(clutter_heights < 60)]
00873 clutter_heights = clutter_heights[np.where(clutter_heights > -10)]
00874 print 'len clutterheights',len(clutter_heights)
00875 hist_surface = stats.histogram2(surface_heights,bins)
00876 hist_clutter = stats.histogram2(clutter_heights,bins)
00877 print bins
00878 print hist_surface
00879 print hist_clutter
00880
00881
00882
00883
00884 plot1 = plt.bar(bins,hist_surface, color='g', width=width,linewidth=0.1)
00885 plot1 = plt.bar(bins+width,hist_clutter, color='r', width=width,linewidth=0.1)
00886
00887 plt_filename = self.config.path+'/clutter_heights.png'
00888 print 'saving',plt_filename
00889 plt.savefig(plt_filename)
00890 plt.savefig(self.config.path+'/clutter_heights.pdf')
00891 plt.savefig(self.config.path+'/clutter_heights.eps')
00892
00893
00894
00895
00896
00897
00898
00899
00900
00901
00902
00903
00904
00905
00906
00907
00908
00909
00910
00911 rgb_plot = np.array(rgb_plot)
00912
00913 v_values_plot = np.asarray(v_values_plot)
00914 s_values_plot = np.asarray(s_values_plot)
00915 h_values_plot = np.asarray(h_values_plot)
00916 colors_plot = np.asarray(colors_plot)
00917 rgb_plot = np.asarray(rgb_plot)
00918
00919
00920
00921
00922 dict = {'s_surface':s_values_plot[colors_plot == 1],
00923 'h_surface':h_values_plot[colors_plot == 1],
00924 's_clutter':s_values_plot[colors_plot == 2],
00925 'h_clutter':h_values_plot[colors_plot == 2]
00926 }
00927
00928
00929
00930 x = s_values_plot
00931 y = h_values_plot
00932 xlabel = 's'
00933 ylabel = 'h'
00934
00935 xlabel = 's_in_rgb'
00936 ylabel = 'h_in_rgb'
00937
00938 x = s_values_plot[colors_plot == 1]
00939 y = h_values_plot[colors_plot == 1]
00940 rgb = rgb_plot[colors_plot == 1]
00941 xlabel = 's, surface'
00942 ylabel = 'h, surface'
00943
00944 x = s_values_plot[colors_plot == 2]
00945 y = h_values_plot[colors_plot == 2]
00946 rgb = rgb_plot[colors_plot == 2]
00947 xlabel = 's, clutter'
00948 ylabel = 'h, clutter'
00949
00950
00951
00952 x = v_values_plot
00953 y = h_values_plot
00954 xlabel = 'v'
00955 ylabel = 'h'
00956
00957 xlabel = 'v_in_rgb'
00958 ylabel = 'h_in_rgb'
00959
00960 x = v_values_plot[colors_plot == 1]
00961 y = h_values_plot[colors_plot == 1]
00962 rgb = rgb_plot[colors_plot == 1]
00963 xlabel = 'v_in_rgb_surface'
00964 ylabel = 'h_in_rgb_surface'
00965
00966 x = v_values_plot[colors_plot == 2]
00967 y = h_values_plot[colors_plot == 2]
00968 rgb = rgb_plot[colors_plot == 2]
00969 xlabel = 'v_in_rgb_clutter'
00970 ylabel = 'h_in_rgb_clutter'
00971
00972
00973 x = i_values_plot
00974 y = h_values_plot
00975 xlabel = 'i'
00976 ylabel = 'h'
00977
00978
00979 x = v_values_plot
00980 y = s_values_plot
00981 xlabel = 'v'
00982 ylabel = 's'
00983
00984 xlabel = 'v_in_rgb'
00985 ylabel = 's_in_rgb'
00986
00987 x = v_values_plot[colors_plot == 1]
00988 y = s_values_plot[colors_plot == 1]
00989 rgb = rgb_plot[colors_plot == 1]
00990 xlabel = 'v_in_rgb_surface'
00991 ylabel = 's_in_rgb_surface'
00992
00993 x = v_values_plot[colors_plot == 2]
00994 y = s_values_plot[colors_plot == 2]
00995 rgb = rgb_plot[colors_plot == 2]
00996 xlabel = 'v_in_rgb_clutter'
00997 ylabel = 's_in_rgb_clutter'
00998
00999
01000 x = i_values_plot
01001 y = s_values_plot
01002 xlabel = 'i'
01003 ylabel = 's'
01004
01005
01006 x = i_values_plot
01007 y = v_values_plot
01008 xlabel = 'i'
01009 ylabel = 'v'
01010
01011
01012 if len(measured_table_heights):
01013 fig_heights = plt.figure()
01014 plt.title('table heights')
01015
01016 plt.xlabel('height in cm')
01017 plt.ylabel('number of tables')
01018 print 'table heights', measured_table_heights
01019 hist = np.histogram(measured_table_heights,10)
01020 plot1 = plt.bar(hist[1][0:-1],hist[0],width=3)
01021
01022 plt_filename = self.config.path+'/table_heights.png'
01023 print 'saving',plt_filename
01024 plt.savefig(plt_filename)
01025 plt.savefig(self.config.path+'/table_heights.pdf')
01026 plt.savefig(self.config.path+'/table_heights.eps')
01027
01028
01029
01030
01031
01032
01033
01034
01035 print 'number of points: ', len(normals_x)
01036
01037 normals_x = np.asarray(normals_x)
01038 normals_y = np.asarray(normals_y)
01039 normals_z = np.asarray(normals_z)
01040 normals_label = np.asarray(normals_label)
01041 import random
01042 sampel_size = min(len(normals_label),500000)
01043 rand_idx = np.array(random.sample(xrange(len(normals_label)),sampel_size))
01044
01045 normals_x = normals_x[rand_idx]
01046 normals_y = normals_y[rand_idx]
01047 normals_z = normals_z[rand_idx]
01048 normals_label = normals_label[rand_idx]
01049
01050 normals_x = np.hstack((normals_x,0))
01051 normals_y = np.hstack((normals_y,0))
01052 normals_z = np.hstack((normals_z,0))
01053 normals_label = np.hstack((normals_label,0))
01054
01055
01056
01057
01058
01059
01060
01061 print 'number of normals:',len(normals_x),len(normals_x),len(normals_x),len(normals_label)
01062 mlab.figure(fgcolor=(0, 0, 0), bgcolor=(1, 1, 1))
01063 mlab.points3d(normals_x,normals_y,normals_z,normals_label,resolution=4,scale_factor=0.005,mode='point',scale_mode='none',colormap='jet')
01064 self.plot_axis(0,0,0, np.eye(3),0.5)
01065
01066
01067
01068
01069
01070
01071
01072
01073
01074
01075
01076
01077
01078
01079
01080
01081
01082
01083 range_hist_surface_plot /= count_surface
01084 range_hist_clutter_plot /= count_clutter
01085
01086
01087 range_hist_surface_std = 0
01088 range_hist_clutter_std = 0
01089
01090
01091
01092 loop_condition = True
01093 first_loop_run = True
01094 while loop_condition == True:
01095 count = 0
01096 if global_stats == True:
01097 if first_loop_run == True:
01098 self.scan_dataset = self.scans_database.get_dataset(0)
01099 first_loop_run = False
01100 else:
01101 self.scan_dataset = self.scans_database.get_next_dataset()
01102
01103 if False == self.scan_dataset:
01104 break
01105
01106 if True != self.scan_dataset.is_labeled:
01107 continue
01108
01109 print 'load',filename
01110 filename = self.get_features_filename()
01111 dict = ut.load_pickle(filename)
01112 else:
01113 loop_condition = False
01114
01115 for index in dict['point_indices']:
01116 fv_range = (dict['features'][count])[self.features.get_indexvector('range')]
01117 if dict['labels'][count] == LABEL_SURFACE:
01118 range_hist_surface_std += np.square(np.asarray(fv_range) - range_hist_surface_plot)
01119
01120 elif dict['labels'][count] == LABEL_CLUTTER:
01121 range_hist_clutter_std += np.square(np.asarray(fv_range) - range_hist_clutter_plot)
01122 count +=1
01123
01124 range_hist_surface_std /= count_surface
01125 range_hist_clutter_std /= count_clutter
01126 range_hist_surface_std = np.sqrt(range_hist_surface_std)
01127 range_hist_clutter_std = np.sqrt(range_hist_clutter_std)
01128
01129
01130
01131
01132 width = 0.35
01133 idx = np.asarray(range(len(self.features.get_indexvector('range'))))
01134 print 'surface',range_hist_surface_plot
01135 print 'surface std',range_hist_surface_std
01136 print 'clutter',range_hist_clutter_plot
01137 print 'clutter std',range_hist_clutter_std
01138
01139 fig_range = plt.figure()
01140 plt.title('range features (normalized): mean and standard deviation')
01141
01142
01143 max = np.maximum(np.maximum(np.maximum(range_hist_surface_plot,range_hist_clutter_plot),range_hist_surface_std),range_hist_clutter_std)
01144 factor = np.divide(np.ones(len(range_hist_surface_plot)),max)
01145
01146 range_hist_surface_plot = np.multiply(range_hist_surface_plot,factor)
01147
01148
01149 range_hist_clutter_plot = np.multiply(range_hist_clutter_plot,factor)
01150 range_hist_surface_std = np.multiply(range_hist_surface_std,factor)
01151 range_hist_clutter_std = np.multiply(range_hist_clutter_std,factor)
01152
01153 plot1 = plt.bar(idx,range_hist_surface_plot,width,yerr=range_hist_surface_std,color='g')
01154 plot2 = plt.bar(idx+width,range_hist_clutter_plot,width,yerr=range_hist_clutter_std,color='r')
01155 plt.xticks(idx+width,('zhist','nx','ny','nz','ev1','ev2'))
01156 plt_filename = self.config.path+'/range_features.png'
01157 print 'saving',plt_filename
01158 plt.savefig(plt_filename)
01159 plt.savefig(self.config.path+'/range_features.pdf')
01160 plt.savefig(self.config.path+'/range_features.eps')
01161
01162
01163
01164 print 'creating plots done.'
01165
01166
01167 def polt_color_scatterplots_2d(self,x,y,xlabel,ylabel, colors_plot, alpha=0.02):
01168 fig = plt.figure()
01169 plt.title(xlabel+', '+ylabel)
01170 plt.xlabel(xlabel+' values')
01171 plt.ylabel(ylabel+' values')
01172
01173
01174 plot = plt.scatter(x,y,5,c=colors_plot,linewidths=0, alpha=alpha)
01175 plt_filename = self.config.path+'/colors_'+xlabel+'_'+ylabel
01176 print 'saving',plt_filename,'...',
01177 plt.savefig(plt_filename+'.png')
01178
01179
01180 print 'done'
01181
01182 def plot_s_h_probabilities(self,s_surface, h_surface, s_clutter, h_clutter):
01183 s_surface = np.hstack((s_surface,0))
01184 h_surface = np.hstack((h_surface,0))
01185 s_clutter = np.hstack((s_clutter,0))
01186 h_clutter = np.hstack((h_clutter,0))
01187 s_surface = np.hstack((s_surface,1))
01188 h_surface = np.hstack((h_surface,1))
01189 s_clutter = np.hstack((s_clutter,1))
01190 h_clutter = np.hstack((h_clutter,1))
01191
01192 bins = 256
01193 hist2d_surface = np.histogram2d(s_surface,h_surface,bins=bins)[0]
01194 hist2d_clutter = np.histogram2d(s_clutter,h_clutter,bins=bins)[0]
01195
01196 hist2d_surface = hist2d_surface[:,0:200]
01197 hist2d_clutter = hist2d_clutter[:,0:200]
01198
01199 fig = plt.figure()
01200 plt.xlabel('s values')
01201 plt.ylabel('h values')
01202
01203
01204
01205
01206
01207
01208
01209
01210 hist2d_sum = hist2d_clutter + hist2d_surface
01211 hist2d_sum[hist2d_sum == 0] = 1
01212
01213
01214
01215 hist_prob_surface = np.divide(hist2d_surface,hist2d_sum)
01216 hist_prob_clutter = np.divide(hist2d_clutter,hist2d_sum)
01217
01218
01219 prob_threshold = 0.80
01220 hist_prob_surface[hist_prob_surface < prob_threshold] = 0
01221 hist_prob_clutter[hist_prob_clutter < prob_threshold] = 0
01222
01223 meas_threshold = 5
01224 hist_prob_surface[hist2d_sum < meas_threshold] = 0
01225 hist_prob_clutter[hist2d_sum < meas_threshold] = 0
01226
01227 blue = np.zeros(np.shape(hist_prob_surface))
01228 idx_white = np.multiply(hist_prob_surface == 0, hist_prob_clutter == 0)
01229 blue[idx_white] = 1
01230 hist_prob_surface[idx_white] = 1
01231 hist_prob_clutter[idx_white] = 1
01232
01233 print hist_prob_surface
01234 print hist_prob_clutter
01235
01236 histcolored = np.array([hist_prob_clutter, hist_prob_surface, blue]).T
01237 plt.imshow(histcolored, interpolation='nearest', origin='lower')
01238 plt_filename = self.config.path+'/sh_probabilities'
01239 print 'saving',plt_filename,'...',
01240 plt.savefig(plt_filename+'.png')
01241 plt.savefig(plt_filename+'.pdf')
01242 plt.savefig(plt_filename+'.eps')
01243
01244 import sys
01245 print 'exit'
01246 plt.show()
01247 sys.exit()
01248
01249
01250 def plot_axis(self,x,y, z, directions, scale = 1):
01251 scale *= 2.
01252 mlab.quiver3d([x-directions[0,0]/2.0], [y-directions[1,0]/2.0], [z-directions[1,0]/2.0], [directions[0,0]], [directions[1,0]], [directions[2,0]], scale_factor=scale, color=(1,0,0))
01253 if directions.shape[1] > 1:
01254 mlab.quiver3d([x-directions[0,1]/2.0], [y-directions[1,1]/2.0], [z-directions[2,0]/2.0], [directions[0,1]], [directions[1,1]], [directions[2,1]], scale_factor=scale, color=(0,1,0))
01255 mlab.quiver3d([x-directions[0,2]/2.0], [y-directions[1,2]/2.0], [z-directions[2,2]/2.0], [directions[0,2]], [directions[1,2]], [directions[2,2]], scale_factor=scale, color=(0,0,1))
01256
01257
01258 def draw_3d(self,type,spheres=False, new_figure = True):
01259
01260
01261
01262
01263
01264
01265
01266
01267
01268
01269
01270
01271
01272
01273 mode = 'point'
01274 if spheres:
01275 mode = 'sphere'
01276 if new_figure:
01277 mlab.figure(fgcolor=(0, 0, 0), bgcolor=(1, 1, 1))
01278
01279
01280 print getTime(), type
01281 if type == 'height':
01282 labels = self.pts3d_bound[2,:]
01283 elif type == 'intensities':
01284 labels = self.intensities_bound
01285 elif type == 'labels':
01286 labels = self.map_polys
01287 elif type == 'range':
01288 labels = self.load_Classifier_labels('range')
01289 elif type == 'color':
01290 labels = self.load_Classifier_labels('color')
01291 elif type == 'all':
01292 labels = self.load_Classifier_labels('all')
01293 elif type == 'all_post':
01294 labels = self.load_Classifier_labels('all_post')
01295
01296 elif type == 'baseline':
01297 labels = self.load_Classifier_labels('baseline')
01298 elif type=='h' or type == 's' or type == 'v':
01299 self.map2d = np.asarray(self.map[0][0:2])
01300
01301
01302 image_size = cv.cvGetSize(self.img)
01303 img_h = cv.cvCreateImage (image_size, 8, 1)
01304 img_s = cv.cvCreateImage (image_size, 8, 1)
01305 img_v = cv.cvCreateImage (image_size, 8, 1)
01306 img_hsv = cv.cvCreateImage (image_size, 8, 3)
01307 cv.cvCvtColor (self.img, img_hsv, cv.CV_BGR2HSV)
01308 cv.cvSplit (img_hsv, img_h, img_s, img_v, None)
01309
01310 labels = []
01311
01312 if type=='h':
01313 imNP = ut.cv2np(img_h)
01314 elif type == 's':
01315 imNP = ut.cv2np(img_s)
01316 elif type == 'v':
01317 imNP = ut.cv2np(img_v)
01318
01319
01320 for index in range(len(self.pts3d_bound[2,:].A1)):
01321 labels.append(float(imNP[self.map2d[1,index],self.map2d[0,index]]))
01322
01323
01324 print 'size X', np.size(self.pts3d_bound[0,:]), 'len labels:', len(labels)
01325
01326
01327 mlab.imshow([[1,1],[1,1]], extent=[-1, 1,
01328 -1, 1,
01329 0, 0], opacity=.5, colormap='jet')
01330
01331
01332
01333
01334 if self.scan_dataset.table_plane_translation != '':
01335 print 'self.scan_dataset.table_plane_translation', self.scan_dataset.table_plane_translation
01336 table_pts = self.pts3d_bound[:,self.map_polys == LABEL_SURFACE]
01337 table_height = float(self.scan_dataset.table_plane_translation[2])
01338 if table_pts != []:
01339 mlab.imshow([[1,1],[1,1]], extent=[np.min(table_pts[0,:]), np.max(table_pts[0,:]),
01340 np.min(table_pts[1,:]), np.max(table_pts[1,:]),
01341 table_height, table_height], opacity=.5, colormap='jet')
01342
01343
01344
01345 mlab.points3d([0],[0],[float(self.scan_dataset.ground_plane_translation[2])],[0],mode='sphere',resolution=8,scale_mode='none',scale_factor=0.1,colormap='winter')
01346
01347 mlab.points3d(self.pts3d_bound[0,:],self.pts3d_bound[1,:],self.pts3d_bound[2,:],labels,mode=mode,resolution=8,scale_factor=0.0015,scale_mode='none',scale_factor=0.01,colormap='jet')
01348
01349 mean_x = np.mean(self.pts3d_bound[0,:])
01350 mean_y = np.mean(self.pts3d_bound[1,:])
01351
01352 mlab.view(azimuth=0, elevation=-48, distance=3.75, focalpoint=(mean_x,mean_y,0.85))
01353
01354 def save_3d(self,type,spheres=False):
01355 mlab.clf()
01356 self.draw_3d(type, spheres)
01357 size=(960,800)
01358 mlab.savefig(self.config.path+'/results/'+self.scan_dataset.id+'_'+type+'.png',size=size)
01359
01360
01361
01362 def display_3d(self,type,spheres=False, new_figure = True):
01363
01364 self.draw_3d(type, spheres, new_figure)
01365
01366
01367
01368
01369
01370
01371
01372
01373
01374 print getTime(), 'points:'
01375 print getTime(), self.pts3d_bound[0,:].shape
01376
01377
01378 R = self.artag_transformation
01379 if np.any(R):
01380 print getTime(), 'display ARTag detection coordinates...'
01381 scale = 0.003
01382 self.draw_3d_axis(scale)
01383
01384 R[:,3] = R[:,3] * 0.0005
01385 R = np.vstack((R,np.array([0,0,0,1.0])))
01386
01387
01388 v1 = np.array([0,0,0,1])
01389 v2 = np.array([80*scale,0,0,1])
01390 v1 = np.dot(R,v1)
01391 v2 = np.dot(R,v2)
01392 print getTime(), v2
01393 self.draw_vector(v1[0:3],v2[0:3],scale, (1,0,0))
01394
01395 v1 = np.array([0,0,0,1])
01396 v2 = np.array([0,80*scale,0,1])
01397 v1 = np.dot(R,v1)
01398 v2 = np.dot(R,v2)
01399 print getTime(), v2
01400 self.draw_vector(v1[0:3],v2[0:3],scale,(0,1,0))
01401
01402 v1 = np.array([0,0,0,1])
01403 v2 = np.array([0,0,80*scale,1])
01404 v1 = np.dot(R,v1)
01405 v2 = np.dot(R,v2)
01406 print getTime(), v2
01407 self.draw_vector(v1[0:3],v2[0:3],scale,(0,0,1))
01408
01409
01410 mlab.colorbar()
01411 mlab.show()
01412
01413
01414
01415 def draw_vector(self, v1, v2, scale, color=(1,1,1)):
01416
01417 mlab.triangular_mesh([[v1[0]-1*scale,v1[0]+1*scale,v2[0]-1*scale,v2[0]+1*scale]], [[v1[1]-1*scale,v1[1]+1*scale,v2[1]-1*scale,v2[1]+1*scale]], [[v1[2]-1*scale,v1[2]+1*scale,v2[2]-1*scale,v2[2]+1*scale]], [(0,1,2),(1,2,3)],color=color)
01418 def draw_3d_axis(self, scale):
01419
01420 mlab.triangular_mesh([np.array([0.0,0.0,100.0])*scale], [np.array([-0.3,0.3,0.0])*scale], [np.array([0.0,0.0,0.0])*scale], [(0,1,2)], color=(1,1,1))
01421 mlab.triangular_mesh([np.array([-0.3,0.3,0.0])*scale], [np.array([0.0,0.0,100.0])*scale], [np.array([0.0,0.0,0.0])*scale], [(0,1,2)], color=(1,1,1))
01422 mlab.triangular_mesh([np.array([-0.3,0.3,0.0])*scale], [np.array([0.0,0.0,0.0])*scale], [np.array([0.0,0.0,100.0])*scale], [(0,1,2)], color=(1,1,1))
01423
01424
01425 def read_artag(self, img):
01426
01427 grey = cv.cvCreateImage((img.width, img.height), 8,1)
01428 hg.cvConvertImage(img, grey)
01429
01430 file = open('test.raw', 'wb')
01431
01432 for i in range(grey.height):
01433 for j in range(grey.width):
01434 file.write(chr(grey[i][j]))
01435
01436 file.close()
01437
01438
01439
01440 output = subprocess.Popen(["/home/martin/artags/ARToolKitPlus_2.1.1/bin/simple", ""], stdout=subprocess.PIPE, env={"LD_LIBRARY_PATH": "/home/martin/artags/ARToolKitPlus_2.1.1/lib:/usr/local/lib:"}).communicate()[0]
01441 print getTime(), output
01442
01443 try:
01444 m = re.search('getARMatrix.*\n([0-9\-\.]*) ([0-9\-\.]*) ([0-9\-\.]*) ([0-9\-\.]*).*\n([0-9\-\.]*) ([0-9\-\.]*) ([0-9\-\.]*) ([0-9\-\.]*).*\n([0-9\-\.]*) ([0-9\-\.]*) ([0-9\-\.]*) ([0-9\-\.]*)', output)
01445 except:
01446
01447 print getTime(), "ERROR parsing ARToolKitPlus output"
01448 return np.array([])
01449
01450 if None == m:
01451 print getTime(), "ERROR parsing ARToolKitPlus output"
01452 return np.array([])
01453
01454 R = np.array(m.groups(),dtype=np.float)
01455 R = R.reshape(3,4)
01456
01457 if False == np.any(R):
01458 print getTime(), "ERROR: failed to detect AR tag"
01459 return np.array([])
01460
01461
01462 print getTime(), m.groups()
01463 print getTime(), R
01464
01465 print getTime(), 'success'
01466 return R
01467
01468
01469 def process_raw_data(self, translate = True):
01470 (self.pts3d, self.scan_indices, self.intensities) = self.create_pointcloud(self.laserscans)
01471 self.do_all_laser_image_mapping(translate)
01472
01473 if self.img_artag:
01474 print getTime(), 'calculate artag transformation'
01475 self.artag_transformation = self.read_artag(self.img_artag)
01476
01477 def process_intensities(self):
01478
01479
01480 (self.pts3d, self.scan_indices, self.intensities) = self.create_pointcloud(self.laserscans, False)
01481 self.img_intensities = self.create_intensity_image(self.laserscans[0])
01482
01483 def process_labels(self, features):
01484 (self.pts3d, self.scan_indices, self.intensities) = self.create_pointcloud(self.laserscans)
01485 self.do_all_point_cloud_mapping()
01486 self.img_labels = self.draw_mapped_labels_into_image(features)
01487
01488 def save_mapped_image(self,name):
01489
01490 prefix = self.config.path+'/data/'+name
01491 print getTime(), "Saving: "+prefix+'_image_mapped.png'
01492 highgui.cvSaveImage(prefix+'_image_mapped.png',self.img_mapped)
01493
01494 def save_intensity_image(self, name):
01495 prefix = self.config.path+'/data/'+name
01496 print getTime(), "Saving: "+prefix+'_image_intensities.png'
01497 highgui.cvSaveImage(prefix+'_image_intensities.png',self.img_intensities)
01498 return prefix+'_image_intensities.png'
01499
01500 def save_labels_image(self, features):
01501 filename = self.config.path+'/results/'+self.scan_dataset.id+'_image_labels_'+features+'_'+self.feature_type+'_k'+str(self.feature_neighborhood)+'_r'+str(self.feature_radius)+'.png'
01502 print getTime(), "Saving: "+filename
01503 highgui.cvSaveImage(filename,self.img_labels)
01504 return filename
01505
01506 def load_metadata(self, id, reload_database = True):
01507
01508 if reload_database:
01509 self.scans_database = scans_database.scans_database()
01510 self.scans_database.load(self.config.path,'database.pkl')
01511 self.scan_dataset = self.scans_database.get_dataset_by_id(id)
01512
01513
01514 def create_polygon_images(self):
01515 self.scan_dataset.cvEdgeImage = cv.cvCreateImage(cv.cvSize (self.img.width, self.img.height),8,1)
01516 cv.cvSet(self.scan_dataset.cvEdgeImage, 0)
01517 self.scan_dataset.cvSurfaceEdgeImage = cv.cvCreateImage(cv.cvSize (self.img.width, self.img.height),8,1)
01518 cv.cvSet(self.scan_dataset.cvSurfaceEdgeImage, 0)
01519 for polygon in self.scan_dataset.polygons:
01520 if len(polygon.get_points()):
01521 polygon.cvImage = cv.cvCreateImage(cv.cvSize (self.img.width, self.img.height),8,1)
01522 cv.cvSet(polygon.cvImage, 0)
01523 cv.cvFillPoly(polygon.cvImage,[polygon.get_points()],255)
01524
01525
01526 if len(polygon.get_points()) and polygon.get_type() == 'polygon':
01527 cv.cvPolyLine(self.scan_dataset.cvEdgeImage,[polygon.get_points()], True, 255,30)
01528
01529
01530 if polygon.get_label() == 'surface' and len(polygon.get_points()) and polygon.get_type() == 'polygon':
01531 cv.cvPolyLine(self.scan_dataset.cvSurfaceEdgeImage,[polygon.get_points()], True, 255,10)
01532
01533
01534
01535
01536
01537
01538
01539 def get_3d_point_index(self, image_point):
01540 width = self.img_intensities.width
01541 height = self.img_intensities.height
01542 index = (height - image_point[1]) * width + image_point[0]
01543 return index
01544
01545
01546 def get_3d_point(self, point2d):
01547 index = self.get_3d_point_index(point2d)
01548 return np.asarray(self.pts3d)[:,index]
01549
01550 def get_3d_point_index_in_unrotated(self, point3d):
01551 print 'p3d',point3d
01552 (pts3d, scan_indices, intensities) = self.create_pointcloud(self.laserscans)
01553 img = cv.cvClone(self.img)
01554 map = self.map_laser_into_cam_2D(pts3d, img, scan_indices, intensities)
01555 pts3d_bound = np.asarray(map[3])
01556 self.kdtree2d = kdtree.KDTree(pts3d_bound.T)
01557 indices = self.kdtree2d.query(point3d.T, 1, eps=0, p=2)
01558 print 'indices',indices, 'index',indices[1]
01559 return indices[1]
01560
01561 def get_3d_plane_normal(self, image_points):
01562 points3d = []
01563
01564 print getTime(), self.img_intensities.width * self.img_intensities.height
01565 for i,point2d in enumerate(image_points):
01566
01567
01568
01569 width = self.img_intensities.width
01570 height = self.img_intensities.height
01571 index = (height - point2d[1]) * width + point2d[0]
01572 print getTime(), 'index: ' + str(index)
01573 print getTime(), np.shape(np.asarray(self.pts3d))
01574 points3d.append(np.asarray(self.pts3d)[:,index])
01575
01576
01577 print getTime(), 'image points: ' + str(image_points) + ' points3d: ' + str(points3d)
01578
01579 a = points3d[1] - points3d[0]
01580 b = points3d[2] - points3d[0]
01581 n = np.matrix(np.cross(a,b)).T
01582 n = n / np.linalg.norm(n)
01583
01584
01585 if n[2] < 0:
01586 n = -n
01587 print getTime(), 'plane normal: ' + str(n)
01588 return n, points3d
01589
01590
01591
01592 def check_3d_plane_point(self, point2d):
01593
01594 width = self.img_intensities.width
01595 height = self.img_intensities.height
01596 index = (height - point2d[1]) * width + point2d[0]
01597 point3d = np.asarray(self.pts3d)[:,index]
01598 print getTime(), 'check 3d point: ', point3d
01599
01600 if np.max(np.abs(point3d)) > 100:
01601 return False
01602 print getTime(), point2d, point3d
01603 return True
01604
01605 def rotate_pointcloud_match_groundplane(self):
01606
01607 if self.scan_dataset.ground_plane_rotation == '':
01608
01609 print 'WARNING: no RANSAC groundplane calculated for',self.scan_dataset.id
01610 print 'WARNING: Falling back to 3point-estimation!'
01611
01612 n = self.scan_dataset.ground_plane_normal
01613
01614 if n == '':
01615 print getTime(), 'WARNING: no groundplane defined!'
01616 return False
01617 print getTime(), 'normal: ', n.T
01618
01619
01620
01621
01622
01623
01624 print 'shapes pts3d, pts3d_bound' ,np.shape(self.pts3d), np.shape(self.pts3d_bound)
01625 self.pts3d = rotate_to_plane(n, self.pts3d)
01626 self.pts3d_bound = rotate_to_plane(n, self.pts3d_bound)
01627 else:
01628
01629 self.pts3d = self.scan_dataset.ground_plane_rotation * self.pts3d
01630 print self.scan_dataset.ground_plane_rotation, self.pts3d_bound
01631 self.pts3d_bound = np.asarray(self.scan_dataset.ground_plane_rotation * np.asmatrix(self.pts3d_bound))
01632
01633 return True
01634
01635
01636 def z_translate_pointcloud_groundplane(self):
01637 if self.scan_dataset.ground_plane_translation == '':
01638 print getTime(), 'WARNING: no groundplane translation defined!'
01639 return False
01640 self.pts3d += self.scan_dataset.ground_plane_translation
01641 self.pts3d_bound += self.scan_dataset.ground_plane_translation
01642
01643 def get_groundplane_translation(self):
01644 gpp = self.scan_dataset.ground_plane_three_points
01645 if gpp == '':
01646 print getTime(), 'WARNING: no groundplane defined!'
01647 return False
01648 if self.scan_dataset.ground_plane_rotation == '':
01649 print getTime(), 'WARNING: no groundplane rotation defined!'
01650 return False
01651
01652 gpp = np.asarray(gpp).T
01653 gpp = self.scan_dataset.ground_plane_rotation * np.matrix(gpp)
01654 z_mean = np.mean(gpp[2,:])
01655
01656 return np.matrix([0.0,0.0, -z_mean]).T
01657
01658
01659
01660 def generate_save_features(self, generate_and_save_all_neightborhood_indices = False, regenerate_neightborhood_indices = False):
01661
01662
01663 training_set_count = 0
01664 self.scan_dataset = self.scans_database.get_dataset(0)
01665 while False != self.scan_dataset:
01666 if self.scan_dataset.is_training_set:
01667 training_set_count += 1
01668 self.scan_dataset = self.scans_database.get_next_dataset()
01669
01670 training_size_per_scan = self.classifier_training_size / training_set_count
01671
01672
01673 self.scan_dataset = self.scans_database.get_dataset(0)
01674
01675 state_exclude_edges = False
01676
01677
01678 while False != self.scan_dataset:
01679 if self.scan_dataset.is_labeled:
01680
01681 self.load_raw_data(self.scan_dataset.id)
01682 (self.pts3d, self.scan_indices, self.intensities) = self.create_pointcloud(self.laserscans)
01683 if state_exclude_edges == False:
01684 self.do_all_point_cloud_mapping(False)
01685 print getTime(), 'generating features with edges', self.scan_dataset.id
01686 else:
01687 self.do_all_point_cloud_mapping(True)
01688 print getTime(), 'generating features without edges', self.scan_dataset.id
01689
01690 dict = self.generate_features(training_size_per_scan,generate_and_save_all_neightborhood_indices,regenerate_neightborhood_indices)
01691
01692 filename = self.get_features_filename(state_exclude_edges)
01693 print getTime(), "Saving: "+filename
01694 ut.save_pickle(dict,filename)
01695 del dict
01696
01697
01698
01699
01700
01701 if state_exclude_edges == False:
01702 state_exclude_edges = True
01703 else:
01704 state_exclude_edges = False
01705
01706 self.scan_dataset = self.scans_database.get_next_dataset()
01707
01708
01709
01710 print getTime(), 'generating features done.'
01711
01712
01713
01714 def generate_features(self,training_size_per_scan = 999999999999,generate_and_save_all_neightborhood_indices = False, regenerate_neightborhood_indices = False):
01715
01716 feature_vector_length = len(self.features.get_indexvector('all'))
01717 nonzero_indices = np.nonzero(self.map_polys)[0]
01718
01719
01720 labels = np.array(self.map_polys)[nonzero_indices]
01721 current_set_size_total = len(nonzero_indices)
01722 all_nonzero_indices = copy.copy(nonzero_indices)
01723
01724
01725 if False == self.scan_dataset.is_test_set and training_size_per_scan < current_set_size_total:
01726 training_size_this_scan = min(training_size_per_scan, current_set_size_total)
01727 t_idx = np.random.randint(0,current_set_size_total,size=training_size_this_scan)
01728 nonzero_indices = nonzero_indices[t_idx]
01729 labels = labels[t_idx]
01730 current_set_size = training_size_this_scan
01731 else:
01732 print 'generate on all points in this scan...'
01733 current_set_size = current_set_size_total
01734
01735 print 'generating',current_set_size,'features for', self.scan_dataset.id, ';fvlength',feature_vector_length
01736
01737 if False == generate_and_save_all_neightborhood_indices:
01738 self.features.prepare(self.features_k_nearest_neighbors, nonzero_indices)
01739 else:
01740 self.features.prepare(self.features_k_nearest_neighbors, all_nonzero_indices, True, regenerate_neightborhood_indices)
01741
01742
01743 train_data = np.array(np.zeros((current_set_size,feature_vector_length)))
01744 train_labels = np.array(np.zeros(current_set_size))
01745
01746 count = 0
01747 for index, label in zip(nonzero_indices,labels):
01748
01749
01750 fv = self.features.get_featurevector(index, count)
01751 for fv_index, fv_value in enumerate(fv):
01752 train_data[count][fv_index] = fv_value
01753 train_labels[count] = label
01754
01755
01756
01757
01758
01759
01760 if count % 1024 == 0:
01761 print getTime(), 'label', label, 'fv', fv
01762 if count % 1024 == 0:
01763 print getTime(), 'generate features:', count, 'of', current_set_size, '(',(float(count)/float(current_set_size)*100.0),'%)'
01764
01765 count += 1
01766
01767 dict = {'features' : train_data, 'labels' : train_labels,
01768 'feature_vector_length' : feature_vector_length,
01769 'set_size': current_set_size,
01770 'point_indices': nonzero_indices}
01771 return dict
01772
01773
01774 def get_features_filename(self,state_exclude_edges = False):
01775 if state_exclude_edges == True:
01776 return self.config.path+'/data/'+self.scan_dataset.id+'_features_without_edges_'+self.feature_type+'_k'+str(self.feature_neighborhood)+'_r'+str(self.feature_radius)+'.pkl'
01777 else:
01778 return self.config.path+'/data/'+self.scan_dataset.id+'_features_'+self.feature_type+'_k'+str(self.feature_neighborhood)+'_r'+str(self.feature_radius)+'.pkl'
01779
01780 def train_and_save_Classifiers(self):
01781 for features, classifier in self.classifiers.iteritems():
01782 print getTime(), 'training:', features
01783
01784 classifier.train()
01785
01786 try:
01787 classifier.save()
01788 except SystemError:
01789 print 'ERROR: saving classifier for',features,'failed!'
01790
01791
01792
01793
01794 def load_Classifiers(self):
01795 for features, classifier in self.classifiers.iteritems():
01796 self.classifiers[features].load()
01797
01798
01799
01800
01801 def test_crossvalidation_on_valid_scans(self, fold=None, update_postprocess=False):
01802 print 'start crossvalidation'
01803
01804
01805
01806
01807 tables_per_fold = 1
01808
01809 testresults_crossvalidation = []
01810 last_fold_first_test_table_id = None
01811
01812 last_table_id = None
01813
01814 last_fold_last_test_table_id = None
01815
01816 current_fold = 0
01817
01818 debug_fold_count = 0
01819 while last_fold_last_test_table_id != last_table_id or last_fold_last_test_table_id == None:
01820 self.scan_dataset = self.scans_database.get_dataset(0)
01821 skip_test_table_count=0
01822 current_fold_test_table_count = 0
01823 state = 'before_test_tables'
01824 print '#####################################'
01825 while False != self.scan_dataset:
01826
01827 if self.scan_dataset.is_labeled:
01828 if state == 'before_test_tables':
01829 if (last_fold_first_test_table_id == self.scan_dataset.surface_id or skip_test_table_count > 0) and last_table_id != self.scan_dataset.surface_id:
01830 skip_test_table_count += 1
01831 if last_fold_first_test_table_id != None and skip_test_table_count <= tables_per_fold:
01832 self.scan_dataset.is_test_set = False
01833 self.scan_dataset.is_training_set = True
01834 else:
01835 state = 'test_tables'
01836 last_fold_first_test_table_id = self.scan_dataset.surface_id
01837 continue
01838
01839 elif state == 'test_tables':
01840
01841 if last_table_id != self.scan_dataset.surface_id:
01842 current_fold_test_table_count += 1
01843
01844 if current_fold_test_table_count <= tables_per_fold:
01845 self.scan_dataset.is_test_set = True
01846 self.scan_dataset.is_training_set = False
01847 last_fold_last_test_table_id = self.scan_dataset.surface_id
01848 else:
01849 state = 'after_test_tables'
01850 continue
01851
01852 elif state == 'after_test_tables':
01853 self.scan_dataset.is_test_set = False
01854 self.scan_dataset.is_training_set = True
01855
01856 last_table_id = self.scan_dataset.surface_id
01857
01858 self.scan_dataset = self.scans_database.get_next_dataset()
01859
01860
01861 if fold == None or current_fold == fold:
01862 print getTime(), 'start training, fold', fold
01863
01864 if False==update_postprocess:
01865
01866 self.train_and_save_Classifiers()
01867
01868
01869 testresults_all = self.test_classifiers_on_testset()
01870 else:
01871 testresults_all = self.update_test_postprocessing_on_testset()
01872
01873 testresults_crossvalidation += [testresults_all]
01874 if fold != None:
01875 self.save_testresults_all_crossvalidation_fold(testresults_all, fold)
01876
01877 current_fold = current_fold + 1
01878
01879
01880
01881
01882
01883
01884
01885
01886
01887 if fold == None:
01888 self.save_testresults_crossvalidation(testresults_crossvalidation)
01889
01890 return testresults_crossvalidation
01891
01892 def collect_and_save_testresults_crossvalidation(self, folds):
01893 testresults_crossvalidation = []
01894 for i in range(folds):
01895 try:
01896 testresults_all = self.load_testresults_all_crossvalidation_fold(i)
01897 testresults_crossvalidation += [testresults_all]
01898 except IOError:
01899 print 'ERROR: no data for fold',i,'found!'
01900
01901
01902
01903 self.save_testresults_crossvalidation(testresults_crossvalidation)
01904
01905 def save_testresults_all_crossvalidation_fold(self, testresults_all, fold):
01906
01907 filename = self.config.path+'/'+'testresults_all_crossvalidation_fold_'+str(fold)+'.pkl'
01908 print getTime(), "Saving: "+filename
01909 ut.save_pickle(testresults_all,filename)
01910
01911 def load_testresults_all_crossvalidation_fold(self, fold):
01912 filename = self.config.path+'/'+'testresults_all_crossvalidation_fold_'+str(fold)+'.pkl'
01913 print getTime(), "loading: "+filename
01914 return ut.load_pickle(filename)
01915
01916
01917 def save_testresults_crossvalidation(self, testresults_crossvalidation):
01918
01919 filename = self.config.path+'/'+'testresults_crossvalidation.pkl'
01920 print getTime(), "Saving: "+filename
01921 ut.save_pickle(testresults_crossvalidation,filename)
01922
01923 def load_testresults_crossvalidation(self):
01924 filename = self.config.path+'/'+'testresults_crossvalidation.pkl'
01925
01926 return ut.load_pickle(filename)
01927
01928
01929
01930 def load_classifier_and_test_on_dataset(self, features, feature_data):
01931
01932
01933
01934 if features == 'all_post':
01935 self.classifiers['all'].load()
01936 self.classifiers['all'].test(feature_data)
01937 labels, testresults = self.classifiers['all'].test_postprocess()
01938 self.save_classifier_labels(labels, testresults, 'all_post')
01939 else:
01940 self.classifiers[features].load()
01941 labels, testresults = self.classifiers[features].test(feature_data)
01942 self.save_classifier_labels(labels, testresults, features)
01943 return labels, testresults
01944
01945
01946 def test_classifiers_on_testset(self):
01947
01948 testresults_all = {}
01949
01950
01951
01952
01953 self.scan_dataset = self.scans_database.get_dataset(0)
01954
01955 for features, classifier in self.classifiers.iteritems():
01956 testresults_all[features] = {}
01957 testresults_all['all_post'] = {}
01958
01959 while False != self.scan_dataset:
01960 if self.scan_dataset.is_test_set:
01961
01962 self.load_data(self.scan_dataset.id, False)
01963
01964 (self.pts3d, self.scan_indices, self.intensities) = self.create_pointcloud(self.laserscans)
01965 self.do_all_point_cloud_mapping()
01966
01967 for features, classifier in self.classifiers.iteritems():
01968 print getTime(), 'start testing on ',features,'id',self.scan_dataset.id
01969
01970 labels, testresults = classifier.test()
01971
01972 self.save_classifier_labels(labels, testresults, features)
01973 testresults_all[features][self.scan_dataset.id] = testresults
01974
01975
01976 features = 'all_post'
01977 self.classifiers['all'].features = 'all_post'
01978 labels, testresults = self.classifiers['all'].test_postprocess()
01979 self.save_classifier_labels(labels, testresults, features)
01980 self.classifiers['all'].features = 'all'
01981 testresults_all[features][self.scan_dataset.id] = testresults
01982
01983
01984 self.scan_dataset = self.scans_database.get_next_dataset()
01985
01986 self.save_testresults_all(testresults_all)
01987 return testresults_all
01988
01989 def update_test_postprocessing_on_testset(self):
01990
01991 testresults_all = {}
01992
01993 self.scan_dataset = self.scans_database.get_dataset(0)
01994
01995 for features, classifier in self.classifiers.iteritems():
01996 testresults_all[features] = {}
01997 testresults_all['all_post'] = {}
01998
01999 while False != self.scan_dataset:
02000 if self.scan_dataset.is_test_set:
02001 print '0'
02002 self.load_data(self.scan_dataset.id, False)
02003
02004 (self.pts3d, self.scan_indices, self.intensities) = self.create_pointcloud(self.laserscans)
02005 self.do_all_point_cloud_mapping()
02006 print '1'
02007
02008
02009 for features, classifier in self.classifiers.iteritems():
02010 print getTime(), 'start testing on ',features,'id',self.scan_dataset.id
02011 self.classifiers[features].test_labels = self.load_Classifier_labels(features)
02012 self.classifiers[features].test_feature_dict = ut.load_pickle(self.get_features_filename())
02013 testresults = classifier.test_results(self.classifiers[features].test_feature_dict,self.classifiers[features].test_labels)
02014 testresults_all[features][self.scan_dataset.id] = testresults
02015
02016
02017 features = 'all_post'
02018 print 'perform postprocessing on ',self.scan_dataset.id
02019 self.classifiers['all'].features = 'all_post'
02020 labels, testresults = self.classifiers['all'].test_postprocess()
02021 self.save_classifier_labels(labels, testresults, features)
02022 self.classifiers['all'].features = 'all'
02023 testresults_all[features][self.scan_dataset.id] = testresults
02024
02025
02026 self.scan_dataset = self.scans_database.get_next_dataset()
02027
02028 self.save_testresults_all(testresults_all)
02029 return testresults_all
02030
02031
02032 def save_testresults_all(self, testresults_all):
02033
02034 filename = self.config.path+'/'+'testresults_all.pkl'
02035 print getTime(), "Saving: "+filename
02036 ut.save_pickle(testresults_all,filename)
02037
02038 def load_testresults_all(self):
02039 filename = self.config.path+'/'+'testresults_all.pkl'
02040 print getTime(), "loading: "+filename
02041 return ut.load_pickle(filename)
02042
02043 def print_testresults_all_latex(self, testresults_all):
02044
02045 testresults_total = {}
02046 for features, results in testresults_all.iteritems():
02047 testresults_total[features] = {'count_surface' : 0, 'count_clutter' : 0, 'count_surface_correct' : 0, 'count_clutter_correct' : 0}
02048
02049 testresults_all_reordered = {}
02050 for features, results in testresults_all.iteritems():
02051 for id, testresults in results.iteritems():
02052 testresults_all_reordered[id] = {}
02053 break
02054
02055
02056
02057 for features, results in testresults_all.iteritems():
02058 for id, testresults in results.iteritems():
02059 testresults_all_reordered[id][features] = testresults
02060
02061
02062
02063 ordered_features = ['range','color','all','all_post','baseline']
02064
02065
02066 ids = testresults_all_reordered.keys()
02067 ids.sort()
02068 for id in ids:
02069 results = testresults_all_reordered[id]
02070
02071 first = True
02072 for features in ordered_features:
02073 testresults = results[features]
02074 if first == True:
02075 print '\\multirow{5}{*}{',id.replace('_','-'),'}'
02076 first = False
02077 count_surface, count_clutter, count_surface_correct, count_clutter_correct, percent_surface_correct, percent_clutter_correct = testresults
02078
02079 testresults_total[features]['count_surface'] += count_surface
02080 testresults_total[features]['count_clutter'] += count_clutter
02081 testresults_total[features]['count_surface_correct'] += count_surface_correct
02082 testresults_total[features]['count_clutter_correct'] += count_clutter_correct
02083 if features != 'baseline':
02084 name = features
02085 else:
02086 name = 'baseline algo'
02087 if features == 'all_post':
02088 print '&\\bf{',name.replace('_','-'),'}&\\bf{',count_surface,'}&\\bf{',count_surface_correct,'}&\\bf{',count_clutter, \
02089 '}&\\bf{',count_clutter_correct,'}&\\bf{%(ps)02.2f}&\\bf{%(pc)02.2f}\\\\' % {'ps':percent_surface_correct, 'pc':percent_clutter_correct}
02090 else:
02091 print '&',name.replace('_','-'),'&',count_surface,'&',count_surface_correct,'&',count_clutter, \
02092 '&',count_clutter_correct,'&%(ps)02.2f&%(pc)02.2f\\\\' % {'ps':percent_surface_correct, 'pc':percent_clutter_correct}
02093 if features != 'baseline':
02094 print '\\cline{2-8}'
02095 else:
02096 print '\\hline\\hline'
02097
02098 print '\\hline\\hline'
02099
02100 first = True
02101 for features in ordered_features:
02102 total_counts = testresults_total[features]
02103
02104 count_surface = total_counts['count_surface']
02105 count_clutter = total_counts['count_clutter']
02106 count_surface_correct = total_counts['count_surface_correct']
02107 count_clutter_correct = total_counts['count_clutter_correct']
02108 percent_surface_correct = float(count_surface_correct)/float(count_surface) * 100
02109 percent_clutter_correct = float(count_clutter_correct)/float(count_clutter) * 100
02110 if first == True:
02111 print '\multirow{5}{*}{total}'
02112 first = False
02113 if features != 'baseline':
02114 name = features
02115 else:
02116 name = 'baseline algo'
02117 if features == 'all_post':
02118 print '&\\bf{',name.replace('_','-'),'}&\\bf{',count_surface,'}&\\bf{',count_surface_correct,'}&\\bf{',count_clutter, \
02119 '}&\\bf{',count_clutter_correct,'}&\\bf{%(ps)02.2f}&\\bf{%(pc)02.2f}\\\\' % {'ps':percent_surface_correct, 'pc':percent_clutter_correct}
02120 else:
02121 print '&',name.replace('_','-'),'&',count_surface,'&',count_surface_correct,'&',count_clutter, \
02122 '&',count_clutter_correct,'&%(ps)02.2f&%(pc)02.2f\\\\' % {'ps':percent_surface_correct, 'pc':percent_clutter_correct}
02123 if features != 'baseline':
02124 print '\\cline{2-8}'
02125 else:
02126 print '\\hline\\hline'
02127
02128 return testresults_total
02129
02130 def print_testrestults_crossvalidation_latex(self, testresults_crossvalidation):
02131
02132 ordered_features = ['range','color','all','all_post','baseline']
02133 testresults_total_crossvalidation = {}
02134 for features in ordered_features:
02135 testresults_total_crossvalidation[features] = {'count_surface' : 0, 'count_clutter' : 0, 'count_surface_correct' : 0, 'count_clutter_correct' : 0}
02136
02137
02138 latex_clearpage = False
02139
02140 for testresults_all in testresults_crossvalidation:
02141 print r"""\begin{table*}[p]
02142 \centering
02143
02144 \caption{Classification results }
02145 \label{tab:results}
02146 %\resizebox{1.80\columnwidth}{!} {
02147 \begin{tabular}{|c|c||c|c|c|c||c|c|}
02148 \hline
02149 \tbthc{Dataset}&\tbth{Features /}&\tbthc{\#points surface}&\tbth{\#points surface}&\tbthc{\#points clutter}&\tbth{\#points clutter}&\tbth{\% surface}&\tbth{\% clutter}
02150 \\ &\tbth{Algorithm}&&\tbth{correct}&&\tbth{correct}&\tbth{correct}&\tbth{correct}
02151 \\\hline"""
02152 testresults_total = self.print_testresults_all_latex(testresults_all)
02153 print r"""\end{tabular}
02154 %}
02155 \end{table*}"""
02156 if latex_clearpage:
02157 print r"""\clearpage"""
02158 latex_clearpage = False
02159 else:
02160 latex_clearpage = True
02161 for features in ordered_features:
02162 testresults_total_crossvalidation[features]['count_surface'] += testresults_total[features]['count_surface']
02163 testresults_total_crossvalidation[features]['count_clutter'] += testresults_total[features]['count_clutter']
02164 testresults_total_crossvalidation[features]['count_surface_correct'] += testresults_total[features]['count_surface_correct']
02165 testresults_total_crossvalidation[features]['count_clutter_correct'] += testresults_total[features]['count_clutter_correct']
02166
02167
02168
02169 print r"""\begin{table*}[p]
02170 \centering
02171 \caption{Classification results crossvalidation total}
02172 \label{tab:results}
02173 %\resizebox{1.80\columnwidth}{!} {
02174 \begin{tabular}{|c|c||c|c|c|c||c|c|}
02175 \hline
02176 \tbthc{Dataset}&\tbth{Features /}&\tbthc{\#points surface}&\tbth{\#points surface}&\tbthc{\#points clutter}&\tbth{\#points clutter}&\tbth{\% surface}&\tbth{\% clutter}
02177 \\ &\tbth{Algorithm}&&\tbth{correct}&&\tbth{correct}&\tbth{correct}&\tbth{correct}
02178 \\\hline"""
02179 first = True
02180 for features in ordered_features:
02181 count_surface = testresults_total_crossvalidation[features]['count_surface']
02182 count_clutter = testresults_total_crossvalidation[features]['count_clutter']
02183 count_surface_correct = testresults_total_crossvalidation[features]['count_surface_correct']
02184 count_clutter_correct = testresults_total_crossvalidation[features]['count_clutter_correct']
02185 percent_surface_correct = float(count_surface_correct)/float(count_surface) * 100
02186 percent_clutter_correct = float(count_clutter_correct)/float(count_clutter) * 100
02187
02188 if first == True:
02189 print '\multirow{5}{*}{crossvalidation}'
02190 first = False
02191 if features != 'baseline':
02192 name = features
02193 else:
02194 name = 'baseline algo'
02195 if features == 'all_post':
02196 print '&\\bf{',name.replace('_','-'),'}&\\bf{',count_surface,'}&\\bf{',count_surface_correct,'}&\\bf{',count_clutter, \
02197 '}&\\bf{',count_clutter_correct,'}&\\bf{%(ps)02.2f}&\\bf{%(pc)02.2f}\\\\' % {'ps':percent_surface_correct, 'pc':percent_clutter_correct}
02198 else:
02199 print '&',name.replace('_','-'),'&',count_surface,'&',count_surface_correct,'&',count_clutter, \
02200 '&',count_clutter_correct,'&%(ps)02.2f&%(pc)02.2f\\\\' % {'ps':percent_surface_correct, 'pc':percent_clutter_correct}
02201 if features != 'baseline':
02202 print '\\cline{2-8}'
02203 else:
02204 print '\\hline\\hline'
02205
02206 print r"""\end{tabular}
02207 %}
02208 \end{table*}"""
02209
02210
02211
02212
02213
02214 def generate_and_save_visualizations(self, id):
02215 feature_list = self.classifiers.keys()
02216 feature_list += ['labels', 'all_post']
02217
02218 self.load_data(id, False)
02219 self.process_raw_data()
02220
02221 for features in feature_list:
02222 self.img_labels = self.draw_mapped_labels_into_image(features)
02223 self.save_labels_image(features)
02224 self.save_3d(features, True)
02225
02226 def generate_and_save_visualizations_all(self):
02227
02228
02229
02230 self.scan_dataset = self.scans_database.get_dataset(0)
02231
02232 while False != self.scan_dataset:
02233 if self.scan_dataset.is_labeled:
02234 self.generate_and_save_visualizations(id)
02235
02236
02237 self.scan_dataset = self.scans_database.get_next_dataset()
02238
02239
02240 def generate_and_save_visualizations_height(self):
02241 self.scan_dataset = self.scans_database.get_dataset(0)
02242
02243 while False != self.scan_dataset:
02244 if self.scan_dataset.is_test_set:
02245 self.load_data(self.scan_dataset.id, False)
02246 self.process_raw_data()
02247 self.save_3d('height', True)
02248
02249
02250 self.scan_dataset = self.scans_database.get_next_dataset()
02251
02252
02253
02254 def test_Classifiers(self):
02255
02256 (self.pts3d, self.scan_indices, self.intensities) = self.create_pointcloud(self.laserscans)
02257 self.do_all_point_cloud_mapping()
02258 for features, classifier in self.classifiers.iteritems():
02259
02260 labels, testresults = classifier.test()
02261 self.save_classifier_labels(labels, testresults, features)
02262
02263
02264 features = 'all_post'
02265 labels, testresults = self.classifiers['all'].test_postprocess()
02266 self.save_classifier_labels(labels, testresults, features)
02267
02268 def get_point_label(self, index):
02269 return self.map_polys[index]
02270
02271
02272 def save_classifier_labels(self, labels, testresults, features):
02273
02274 dict = {'labels' : labels, 'testresults' : testresults}
02275 filename = self.get_Classifier_labels_filename(features)
02276 print getTime(), "Saving: "+filename
02277 ut.save_pickle(dict,filename)
02278 print '...saving labels: len:', len(labels), 'testresults:',testresults
02279
02280 def load_Classifier_labels(self, features):
02281 filename = self.get_Classifier_labels_filename(features)
02282 print 'loading', filename
02283 dict = ut.load_pickle(filename)
02284 print '...loading labels: len:', len(dict['labels']), 'testresults:',dict['testresults']
02285 return dict['labels']
02286
02287 def get_Classifier_labels_filename(self, features):
02288 return self.config.path+'/data/'+self.scan_dataset.id+'_labels_Classifier_'+features+'_'+self.feature_type+'_k'+str(self.feature_neighborhood)+'_r'+str(self.feature_radius)+'.pkl'
02289
02290
02291
02292 def save_all_labeled_pointclouds(self):
02293
02294 self.scan_dataset = self.scans_database.get_dataset(0)
02295 while False != self.scan_dataset:
02296 if self.scan_dataset.is_labeled:
02297 self.load_data(self.scan_dataset.id, False)
02298 self.process_raw_data()
02299
02300 dict = {'points': self.pts3d_bound,
02301 'labels': self.map_polys,
02302 'intensities': self.intensities_bound,
02303 'surface_height': self.scan_dataset.surface_height}
02304
02305 filename = self.config.path+'/data/labeled_pointclouds/pointcloud_'+self.scan_dataset.id+'.pkl'
02306 print 'saving', filename
02307 ut.save_pickle(dict, filename)
02308
02309 self.scan_dataset = self.scans_database.get_next_dataset()
02310
02311