processor.py
Go to the documentation of this file.
00001 #
00002 # Copyright (c) 2010, Georgia Tech Research Corporation
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #     * Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     * Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     * Neither the name of the Georgia Tech Research Corporation nor the
00013 #       names of its contributors may be used to endorse or promote products
00014 #       derived from this software without specific prior written permission.
00015 #
00016 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00017 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00020 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00021 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00022 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00023 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00024 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00025 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 #
00027 
00028 #  \author Martin Schuster (Healthcare Robotics Lab, Georgia Tech.)
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 #ros.tf for transforms
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 #from labeling.dumpObj import dumpObj
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     #region of interest:
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 #define it here as it's used in the gaussian_histogram_features
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     #print np.shape(rot), np.shape(points)
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     #line originating at [0,0,0].T
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         #print 'gamma',gamma
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 #class labels:
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 #unused! just appears in filenames. TODO: remove and replace by features_k_nearest_neighbors
00134     feature_radius = 0.03
00135 
00136     classifier_training_size = 900000000 #use all == a very large number
00137     
00138     #define volume of interest
00139     point_of_interest = np.array([0.8,0.0,1.0])
00140     voi_width = 1.4
00141     
00142     #do not label points that are too close to the ground:
00143     ground_exclude_treshold = 0.3
00144     
00145     #use k nearest neighbors for features or all points in a 3cm sphere in NN = None
00146     #50 seems to be a good value, 20 seems to be too small, None gives better results (about 5percent crossvalidation in total) but is way slower
00147     features_k_nearest_neighbors = 50 #None
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     #set reject_zero_ten to False to not remove any points and preserve intensity->3d mapping
00173     # be aware that points filtered by remove-graze-effect have range-values of +/-1000 mapped into 3d-space
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             #save index of scan with points:
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,:] #flip Y axis
00202             
00203         return (pts, scan_indices, np.asarray(intensities)[0])     
00204     
00205     
00206     def truncate_pointcloud_to_voi(self,poi, depth, width, height):
00207         #true/false array returned by get_voi_indices_fancy
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         #self.map_polys = self.map_polys[idx]
00216         #Hack: just truncate the first part which is used as map2d all through the code for now
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         # run RANSAC algorithm
00225         try:
00226             model = ransac.ransac(data,model,
00227                                         #3, 1000, 0.01, len(data_idx)/1.5, # misc. parameters
00228                                         3, 1000, 0.04, len(data_idx)/2.5,
00229                                         debug=False,return_all=False)
00230         except ValueError:
00231             #did not meet fit acceptance criteria
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         #set all planes to zero:
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) #do not translate
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                 #hack: reload and save to prevent pickling of edge and polygon images (=> error)
00268                 self.load_data(self.scan_dataset.id, True)
00269                 self.scans_database.set_internal_pointer_to_dataset(self.scan_dataset.id)
00270                 #set values:
00271                 self.scan_dataset.ground_plane_rotation = rotation
00272                 self.scan_dataset.ground_plane_translation = self.get_groundplane_translation() #call after rotation has been set!
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                     #no ransac was possible -> use dummy:
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     #needs np.matrix as input
00291     def map_image_point_on_3d_plane(self, point, ground_plane_rotation, ground_plane_translation, table_plane_translation):
00292         #just to be sure: copy everything for now (may not be necessary for all variables!)
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         #translate 2d point w.r.t. camera center
00299         #print self.config.cam_centers
00300         #print point
00301         point[0][0] -= self.config.cam_centers[0] 
00302         point[1][0] -= self.config.cam_centers[1] 
00303 
00304         #create camera->3d projection matrix
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         #project 2d point to line from origin in 3d
00309         line = cam_proj_inv * tr.xyzToHomogenous(point)
00310         
00311         #create projection matrix from camera COS to laser COS
00312         
00313         if self.config.device == 'codyRobot':
00314             #transform line into laser COS
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             #transform line into laser COS
00325             line = tr.transform( camera_to_laser, line)
00326         
00327         #rotate line around ground plane
00328         line = ground_plane_rotation * line
00329         
00330         #translate table plane w.r.t. groundplane
00331         table_plane_point = table_plane_translation - ground_plane_translation
00332         
00333         #assume everything is rotated so that resulting table plane is horizontal
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     #from travis scanr:
00344     def map_laser_into_cam_2D(self, pts3d, img, scan_indices, intensities):
00345         # Project self.pts into image, and display it
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         #camPts = np.row_stack((camPts[0:2]/camPts[2],camPts[2]))
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]] #point cloud bounded by camera
00376         #c.A[0]: indices of 3d-points that are visible in image
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         #colormap points in image
00385         #print map[2] #array of indices inside bounds
00386 
00387         x = np.asarray(pts3d)[0][map[2]]
00388         x = x - x.min()
00389         x = x / x.max() * 10 #512 #number of colors
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 #            imNP[map2d[1,i],map2d[0,i],0] = min(255,x[i])
00406 #            imNP[map2d[1,i],map2d[0,i],1] = max(0,255-x[i])
00407 #            imNP[map2d[1,i],map2d[0,i],2] = max(0,512-x[i])
00408     
00409         img_mapped = ut.np2cv(imNP)
00410         return img_mapped
00411     
00412     
00413     def draw_mapped_labels_into_image(self, type):
00414        
00415         #labels_groundtruth = self.map_polys
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                 #if labels[i] == labels_groundtruth[i]:
00440                 cv.cvCircle(img_mapped, cv.cvPoint(map2d[0,i],map2d[1,i]),3, cv.cvScalar(0, 255, 0, 0))
00441                 #else: 
00442                 #    cv.cvCircle(img_mapped, cv.cvPoint(map2d[0,i],map2d[1,i]),3, cv.cvScalar(0, 255, 255, 0))
00443             elif labels[i] == LABEL_CLUTTER:
00444                 #if labels[i] == labels_groundtruth[i]:
00445                 cv.cvCircle(img_mapped, cv.cvPoint(map2d[0,i],map2d[1,i]),3, cv.cvScalar(0, 0, 255, 0))
00446                 #else:
00447                 #    cv.cvCircle(img_mapped, cv.cvPoint(map2d[0,i],map2d[1,i]),3, cv.cvScalar(255, 0, 255, 0))
00448                 
00449         return img_mapped 
00450     
00451 
00452     
00453     def draw_mapped_laser_polygons_into_image(self,map,pts3d,img):
00454         #colormap points in image
00455         #print getTime(), map[2] #array of indices inside bounds
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     #return array that can be used by mayavi for coloring:
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)) # 0 == background/floor/unlabeled/other
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             #first ROI
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 #region of interest for testing 
00540 
00541             #surface second
00542             #always exclude surface edges
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                     #and 255 != self.scan_dataset.cvSurfaceEdgeImage[ximg][yimg]:
00546                     polygon_mapping[i] = LABEL_SURFACE #1 == surface        
00547                                        
00548             #object has highest priority
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 #2 == clutter
00552             
00553         return polygon_mapping    
00554       
00555       
00556     def remove_labels_from_low_points(self):
00557         #do not label points that are too close to the ground:
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     #works
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] #reverse = hflip as scanner starts at bottom
00576         
00577         return ut.np2cv(imgNP)
00578     
00579     
00580     #does not work for now
00581 #    def create_intensity_image2(self, laserscan, bound_indices):
00582 #
00583 #        pos_list, scan_list = laserscan
00584 #        height = len(scan_list)
00585 #        width = len(np.asarray(scan_list[0].intensities)[0])
00586 #        pts_intensity = np.zeros((3,height*width))
00587 #        for i,s in enumerate(scan_list):
00588 #            for j, intensity in enumerate(np.asarray(s.intensities)[0]):
00589 #                pts_intensity[0,i*width+j] = i
00590 #                pts_intensity[1,i*width+j] = j
00591 #                pts_intensity[2,i*width+j] = intensity
00592 #        
00593 #        pts_intensity = pts_intensity[:,bound_indices]
00594 #        print getTime(),  np.min(pts_intensity[0])
00595 #        print getTime(),  np.min(pts_intensity[1]) 
00596 #        pts_intensity[0] = pts_intensity[0] - np.min(pts_intensity[0])
00597 #        pts_intensity[1] = pts_intensity[1] - np.min(pts_intensity[1])
00598 #        print getTime(), pts_intensity
00599 #        
00600 #        imgNP = np.zeros((np.max(pts_intensity[0])+1,np.max(pts_intensity[1])+1))
00601 #        
00602 #        x = pts_intensity
00603 #        n,m = pts_intensity.shape
00604 #        for i in range(m):
00605 #            imgNP[x[0,i]][x[1,i]] = x[2,i]
00606 #        
00607 #        imgNP = imgNP / np.max(imgNP) * 255
00608 #        imgNP = np.asarray(imgNP)
00609 #        imgNP = imgNP[::-1] #reverse = hflip as scanner starts at bottom
00610 #        print getTime(), imgNP
00611 #        return ut.np2cv(imgNP)    
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         #self.map_polys = self.map_poylgons_into_laser(self.map)
00626         #map labels for classification
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         #self.img_intensities = self.create_intensity_image2(self.laserscans[0],self.map[2])
00633         
00634         
00635     #don't create any images but all information needed for the pointcloud
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         #window_name = "Image"
00676         #highgui.cvNamedWindow (window_name, highgui.CV_WINDOW_AUTOSIZE)
00677         #highgui.cvShowImage (window_name, self.img)
00678         
00679         #window_name = "Image Mapped"
00680         #highgui.cvNamedWindow (window_name, highgui.CV_WINDOW_AUTOSIZE)
00681         #highgui.cvShowImage (window_name, self.img_mapped) 
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         #window_name = "Image Intensities"
00688         #highgui.cvNamedWindow (window_name, highgui.CV_WINDOW_AUTOSIZE)
00689         #highgui.cvShowImage (window_name, self.img_intensities)                    
00690         
00691         #self.display_3d()
00692         #highgui.cvWaitKey(0)
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                 #assume that scans from one surface are in order
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                 #for plot
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                 #if dict['labels'][count] == LABEL_CLUTTER:
00805                 #    print 'label',dict['labels'][count], 'fvrange', fv_range
00806                 if dict['labels'][count] == LABEL_SURFACE:
00807                     #colors_plot.append([0,1,0])
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                     #colors_plot.append([1,0,0])   
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             #just needed for clutter height stats!!
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 #in cm
00858        
00859         #clutter_heights = clutter_heights[np.where(clutter_heights > -table_height)]
00860         
00861         print clutter_heights
00862         fig_clutterheights = plt.figure()
00863         plt.title('clutter and surface heights above measured height of table')
00864         # the histogram of the data
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         #hist_table = copy.copy(hist)
00881         #hist_table[2*table_height:] = 0
00882         #hist_clutter = copy.copy(hist)
00883         #hist_clutter[0:table_height*2] = 0
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         #plt.xticks(np.array(range(12))*5-5)        
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         #plt.show()
00893         #return
00894 
00895 
00896 #        h_values_plot += [0]
00897 #        s_values_plot += [0]
00898 #        v_values_plot += [0]
00899 #       i_values_plot += [0]
00900 #       colors_plot += [0]
00901  ##       rgb_plot += [[0,0,0]]
00902  #       mlab.figure(fgcolor=(0, 0, 0), bgcolor=(1, 1, 1))
00903  #       print 'lens',len(h_values_plot),len(s_values_plot),len(v_values_plot),len(i_values_plot),len(colors_plot)
00904  #       plot = mlab.points3d(np.asarray(h_values_plot),np.asarray(s_values_plot), np.asarray(v_values_plot), np.asarray(colors_plot),mode='point',resolution=4,scale_mode='none',scale_factor=0.01)
00905  #       mlab.outline(plot, color=(.7, .7, .7))
00906  #       mlab.xlabel('h')
00907  #       mlab.ylabel('s')
00908  #       mlab.zlabel('v')
00909  #       mlab.colorbar()
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         #for s,h plot probabilities:
00921         #self.plot_s_h_probabilities(s_values_plot[colors_plot == 1], h_values_plot[colors_plot == 1], s_values_plot[colors_plot == 2], h_values_plot[colors_plot == 2])
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         #ut.save_pickle(dict, "s_h_probabilites_values.pkl")
00928 
00929         
00930         x = s_values_plot
00931         y = h_values_plot
00932         xlabel = 's' 
00933         ylabel = 'h'
00934         #self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,colors_plot)
00935         xlabel = 's_in_rgb' 
00936         ylabel = 'h_in_rgb'
00937         #self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,rgb_plot,0.1)  
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         #self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,rgb,0.1)  
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         #self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,rgb,0.1)  
00950         
00951         
00952         x = v_values_plot
00953         y = h_values_plot
00954         xlabel = 'v' 
00955         ylabel = 'h'
00956         #self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,colors_plot)
00957         xlabel = 'v_in_rgb' 
00958         ylabel = 'h_in_rgb'
00959        # self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,rgb_plot,0.1) 
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         #self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,rgb,0.1)  
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         #self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,rgb,0.1) 
00972                 
00973         x = i_values_plot
00974         y = h_values_plot
00975         xlabel = 'i' 
00976         ylabel = 'h'
00977         #self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,colors_plot) 
00978            
00979         x = v_values_plot
00980         y = s_values_plot
00981         xlabel = 'v' 
00982         ylabel = 's'
00983        # self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,colors_plot) 
00984         xlabel = 'v_in_rgb' 
00985         ylabel = 's_in_rgb'
00986         #self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,rgb_plot,0.1)  
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        # self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,rgb,0.1)  
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        # self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,rgb,0.1)       
00999               
01000         x = i_values_plot
01001         y = s_values_plot
01002         xlabel = 'i' 
01003         ylabel = 's'
01004         #self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,colors_plot)   
01005             
01006         x = i_values_plot
01007         y = v_values_plot
01008         xlabel = 'i' 
01009         ylabel = 'v'
01010        # self.polt_color_scatterplots_2d(x,y,xlabel,ylabel,colors_plot)          
01011     
01012         if len(measured_table_heights):
01013             fig_heights = plt.figure()
01014             plt.title('table heights')
01015             # the histogram of the data
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             #plt.show()
01028 
01029         
01030         
01031         #normals_x += [0]
01032         #normals_y += [0]
01033         #normals_z += [0]
01034         #normals_label += [0]
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 #        rand_idx = np.random.randint(0,len(normals_x),size=100000)
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 #        print 'nx',normals_x
01055 #        print 'ny',normals_y
01056 #        print 'nz',normals_z
01057 #        print 'nlabel',normals_label
01058 
01059  
01060 ###plot normals sphere:###        
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         #idx = np.where(normals_label == LABEL_SURFACE)
01069         #nx = normals_x[idx]
01070         #ny = normals_y[idx]
01071         #nz = normals_z[idx]
01072         
01073         #mlab.quiver3d(nx,ny,nz,0.2*nx,0.2*ny,0.2*nz,colormap='jet',scale_factor=0.005, color=(0,1,0))
01074         #idx = np.where(normals_label == LABEL_CLUTTER)
01075         #nx = normals_x[idx]
01076         #ny = normals_y[idx]
01077         #nz = normals_z[idx]
01078         #mlab.quiver3d(nx,ny,nz,-0.2*nx,-0.2*ny,-0.2*nz,colormap='jet',scale_factor=0.005, color=(1,0,0))
01079         #mlab.colorbar()
01080         
01081        
01082 
01083         range_hist_surface_plot /= count_surface
01084         range_hist_clutter_plot /= count_clutter
01085     
01086         #calculate standard deviation:
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       # the width of the bars   
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         #print range_hist_surface_plot
01142         #print range_hist_clutter_plot
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        # print factor
01146         range_hist_surface_plot = np.multiply(range_hist_surface_plot,factor)
01147         #print 'factor',range_hist_surface_plot
01148         #print range_hist_clutter_plot
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         #plt.show()  
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         #TODO: dpi=...
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         #plt.savefig(plt_filename+'.pdf')
01179         #plt.savefig(plt_filename+'.eps')      
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         #hist2d_surface[hist2d_surface == 0.0] = 1
01204         #hist2d_clutter[hist2d_clutter == 0.0] = 1
01205         #hist2d = np.divide(hist2d_surface / len(s_surface) , hist2d_clutter / len(s_clutter))
01206         #print hist2d
01207         #hist2d = hist2d / np.max(hist2d)
01208         #print hist2d
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         #hist_prob_clutter = 1.0 - hist_prob_surface     
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         #prob. too low or too few measurements? -> white
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         #display bounded 3d-points
01260         #mlab.points3d(self.pts3d_bound[0,:].A1,self.pts3d_bound[1,:].A1,self.pts3d_bound[2,:].A1,self.pts3d_bound[0,:].A1,mode='point',scale_factor=0.01,colormap='jet')#,colormap='winter'
01261         
01262         ##mlab.points3d(self.pts3d_bound[0,:].A1,self.pts3d_bound[1,:].A1,self.pts3d_bound[2,:].A1,self.map_polys,mode='point',scale_factor=0.01,colormap='jet')#,colormap='winter'
01263        
01264         #test:
01265         #self.pts3d_bound = self.pts3d
01266        
01267         #test artag
01268         #self.pts3d_bound = tr.transform( self.config.camTlaser, self.pts3d_bound)
01269         
01270         #don't display ROI
01271         #map_polys = np.array(self.map_polys)[a==LABEL_ROI]=0
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             #labels = self.classifiers['all'].postprocess(labels)
01296         elif type == 'baseline':
01297             labels = self.load_Classifier_labels('baseline') #self.classify_baseline_code()
01298         elif type=='h' or type == 's' or type == 'v':
01299             self.map2d = np.asarray(self.map[0][0:2]) #copied from laser to image mapping
01300             #create HSV numpy images:
01301             # compute the hsv version of the image 
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         #draw groundplane:
01327         mlab.imshow([[1,1],[1,1]], extent=[-1, 1,  
01328                                        -1, 1, 
01329                                        0, 0], opacity=.5, colormap='jet')      
01330         
01331         #draw table plane:
01332         #print 'tbp',self.scan_dataset.table_plane_translation
01333         #draw_plane(np.matrix([[0,0,1]]).T, self.scan_dataset.table_plane_translation.T)   
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             #draw camera:
01344             #print 'gpt',self.scan_dataset.ground_plane_translation
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')#,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')#,colormap='winter'
01348         #mlab.orientationaxes()
01349         mean_x = np.mean(self.pts3d_bound[0,:])
01350         mean_y = np.mean(self.pts3d_bound[1,:])
01351         #set camera:
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         #mlab.close() #AttributeError: 'module' object has no attribute 'close' ?????
01360         
01361         
01362     def display_3d(self,type,spheres=False, new_figure = True):
01363         
01364         self.draw_3d(type, spheres, new_figure)
01365         
01366         #draw this and that..:
01367         #mlab.points3d(self.pts3d_bound[0,:].A1,self.pts3d_bound[1,:].A1,self.pts3d_bound[2,:].A1,self.intensities_bound,mode=mode,resolution=4,scale_factor=0.005,scale_mode='none',colormap='winter')#,colormap='winter'
01368         #mlab.points3d(self.pts3d_bound[0,:].A1,self.pts3d_bound[1,:].A1,self.pts3d_bound[2,:].A1,self.pts3d_bound[2,:].A1,mode=mode,resolution=4,scale_factor=0.005,scale_mode='none',colormap='jet')#,colormap='winter'
01369         #mlab.points3d(self.pts3d_bound[0,:].A1,self.pts3d_bound[1,:].A1,self.pts3d_bound[2,:].A1,self.map_polys,mode='sphere',resolution=4,scale_factor=0.0015,scale_mode='none',colormap='jet')#,colormap='winter'
01370         #draw scan indices (coloring)
01371         #mlab.points3d(self.pts3d_bound[0,:].A1,self.pts3d_bound[1,:].A1,self.pts3d_bound[2,:].A1,self.scan_indices_bound,mode='point',scale_factor=0.01,colormap='autumn')#,colormap='winter'
01372         #mlab.points3d(self.pts3d_bound[0,:].A1,self.pts3d_bound[1,:].A1,self.pts3d_bound[2,:].A1,self.scan_indices_bound,mode='sphere',resolution=4,scale_factor=0.0015,scale_mode='none',colormap='autumn')#,colormap='winter'
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             #R[:,0:3] = np.linalg.inv(R[:,0:3])
01387             #draw axis
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     #old, better use quivers instead...
01415     def draw_vector(self, v1, v2, scale, color=(1,1,1)):
01416         #mlab.triangular_mesh([np.array([v1[0]-1,v1[0]+1,v2[0]-1,v2[0]+1])*scale], [np.array([v1[1]-1,v1[1]+1,v2[1]-1,v2[1]+1])*scale], [np.array([v1[2]-1,v1[2]+1,v2[2]-1,v2[2]+1])*scale], [(0,1,2),(1,2,3)],color=color)
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             #axis
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     #needs the ARToolKitPlus with a modified simple example code that reads the image and output the transformation
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         #ut.display_images([img])
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             #sys.exc_info()[0]
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): #all elements are 0
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         # success, 3x4 transformation matrix
01465         print getTime(), 'success'
01466         return R
01467         
01468     #create pointcloud, map laser into image, read artag if present
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         #set reject_zero_ten to False to not remove any points and preserve intensity->3d mapping
01479         # be aware that points filtered by remove-graze-effect have range-values of +/-1000 mapped into 3d-space
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                 #exclude all edges for training
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                 #exclude surfaces edges for testing
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 #                window_name = "edge"+getTime()
01533 #                highgui.cvNamedWindow (window_name, highgui.CV_WINDOW_AUTOSIZE)
01534 #                highgui.cvShowImage (window_name, polygon.cvEdgeImage) 
01535 #                print window_name
01536 #                cvWaitKey()
01537 
01538     #from 2d point in intensity image to 3d:
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     #from 2d point in intensity image to 3d:
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         #print getTime(), 'pixelcount'
01564         print getTime(), self.img_intensities.width * self.img_intensities.height
01565         for i,point2d in enumerate(image_points):
01566             #print getTime(), 'point2d: ', point2d
01567             #point2d = np.round(point2d) #also transforms it into numpy array
01568             #print getTime(), point2d
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         #plot all scene points    
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         #z component should always point upwards
01585         if n[2] < 0:
01586             n = -n
01587         print getTime(), 'plane normal: ' + str(n)
01588         return n, points3d
01589 
01590 
01591     #check if it is a valid point or got set to range=+/-1000 by remove_graze_effect()
01592     def check_3d_plane_point(self, point2d):
01593         #point2d = np.round(point2d)
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 #invalid
01602         print getTime(), point2d, point3d
01603         return True #valid point
01604                 
01605     def rotate_pointcloud_match_groundplane(self):
01606         
01607         if self.scan_dataset.ground_plane_rotation == '':
01608             #fall back to groundplane
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     #        array = np.array(ground_plane_points)
01619     #        mlab.points3d(array[:,0], array[:,1], array[:,2], array[:,2], scale_factor=0.15, scale_mode='none')
01620     #        middle = array.T.mean(axis=1)
01621     #        mlab.quiver3d([middle[0]], [middle[1]], [middle[2]], [n[0,0]], [n[1,0]], [n[2,0]])
01622                    
01623             #rotate:     
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             #rotate:     
01629             self.pts3d = self.scan_dataset.ground_plane_rotation * self.pts3d #rotate_to_plane(n, 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)) #rotate_to_plane(n, 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     #(re)generate only for current test and training set:
01660     def generate_save_features(self, generate_and_save_all_neightborhood_indices = False, regenerate_neightborhood_indices = False):
01661         
01662         #get number of training sets:
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         #loop through all marked datasets
01673         self.scan_dataset = self.scans_database.get_dataset(0)
01674         
01675         state_exclude_edges = False #just 2 alternating states to do both for each scan
01676         
01677         #get size of training set in total
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                 #del train_data
01697                 #del train_labels
01698                 #del nonzero_indices
01699                 #del labels
01700                 
01701             if state_exclude_edges == False:
01702                 state_exclude_edges = True
01703             else:
01704                 state_exclude_edges = False
01705                 #get next one
01706                 self.scan_dataset = self.scans_database.get_next_dataset()
01707                 
01708             
01709 
01710         print getTime(), 'generating features done.'
01711         
01712         
01713     #generate and return features for the current dataset, assume it is loaded and mapped
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] #just surface or clutter for now (index > 0)
01718         #print getTime(), nonzero_indices
01719         #print getTime(), self.map_polys
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         #just select a couple of points only for training:
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 #use all for testing
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) #only for 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             #print getTime(), point3d
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             #print getTime(), 'fv ', fv
01755             #print getTime(), 'tr ',train_data[index], 'label',train_labels[index]
01756 
01757             #if train_labels[count] == LABEL_CLUTTER:
01758             #    print 'label',train_labels[count], 'fvrange', fv
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         #save data:
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     #todo: refactor
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             #else:
01791             #    print 'ERROR: saving classifier for',features,'failed!'
01792 
01793             
01794     def load_Classifiers(self):
01795         for features, classifier in self.classifiers.iteritems():
01796             self.classifiers[features].load()            
01797         
01798         
01799     #if fold != None: just process this specific fold of the crossvalidation and save the results
01800     #if update_postprocess==True: just do postprocessing on 'all', and load the labels for all others
01801     def test_crossvalidation_on_valid_scans(self, fold=None, update_postprocess=False):
01802         print 'start crossvalidation'
01803         #TODO
01804         #group by tables
01805         #should I only use tables with 4 valid scans??
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                 #print 'id',self.scan_dataset.id ,'state',state
01827                 if self.scan_dataset.is_labeled: #only valid labelings
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 #with same dataset
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#only valid after the whole run of the inner loop
01848                         else:
01849                             state = 'after_test_tables'
01850                             continue #with same dataset
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                 #get next one
01858                 self.scan_dataset = self.scans_database.get_next_dataset()
01859             #self.scans_database.save()
01860             
01861             if fold == None or current_fold == fold:
01862                 print getTime(), 'start training, fold', fold
01863 
01864                 if False==update_postprocess:
01865                     #do training:
01866                     self.train_and_save_Classifiers()
01867                     #self.load_Classifiers()
01868                     #do testing:
01869                     testresults_all = self.test_classifiers_on_testset()
01870                 else: #just do postprocessing and load the rest:
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             #debug output:
01880 #            debug_fold_count += 1
01881 #            print 'fold', debug_fold_count
01882 #            self.scan_dataset = self.scans_database.get_dataset(0)
01883 #            while False != self.scan_dataset:
01884 #                if self.scan_dataset.is_labeled: #only valid labelings
01885 #                    print 'table id', self.scan_dataset.surface_id, 'train', self.scan_dataset.is_training_set, 'test', self.scan_dataset.is_test_set
01886 #                self.scan_dataset = self.scans_database.get_next_dataset()
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             #else:
01901             #    print 'ERROR: no data for fold',i,'found!'
01902                 
01903         self.save_testresults_crossvalidation(testresults_crossvalidation)
01904     
01905     def save_testresults_all_crossvalidation_fold(self, testresults_all, fold):
01906         #save results for one fold of the crossvalidation process
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         #save data:
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         #print getTime(), "loading: "+filename
01926         return ut.load_pickle(filename)
01927     
01928     
01929     #assume all data is already present/set/loaded
01930     def load_classifier_and_test_on_dataset(self, features, feature_data):
01931         #needed for display and to get the size of the labels-vector, do before assigning the correct labels!
01932         #(self.pts3d, self.scan_indices, self.intensities) = self.create_pointcloud(self.laserscans)
01933         #self.do_all_point_cloud_mapping()  
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 = {}        #loop through all marked datasets
01949     
01950 #            testerrors_surface = {}
01951 #            testerrors_clutter = {}
01952 #            testerrors_sum_surface_clutter = {}
01953         self.scan_dataset = self.scans_database.get_dataset(0)
01954         #get size of training set in total
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                 #needed for display and to get the size of the labels-vector, do before assigning the correct labels!
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                     #count_surface, count_clutter, count_surface_correct, count_clutter_correct, percent_surface_correct, percent_clutter_correct = testresults
01972                     self.save_classifier_labels(labels, testresults, features)
01973                     testresults_all[features][self.scan_dataset.id] = testresults
01974                             
01975                 #test all+postprocessing:
01976                 features = 'all_post'
01977                 self.classifiers['all'].features = 'all_post' #just for the print-statements
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             #get next one
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 = {}        #loop through all marked datasets
01992 
01993         self.scan_dataset = self.scans_database.get_dataset(0)
01994         #get size of training set in total
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                 #needed for display and to get the size of the labels-vector, do before assigning the correct labels!
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                 #load labels and features
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                 #test all+postprocessing:
02017                 features = 'all_post'
02018                 print 'perform postprocessing on ',self.scan_dataset.id
02019                 self.classifiers['all'].features = 'all_post' #just for the print-statements
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             #get next one
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         #save data:
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         #print testresults_all_reordered
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         #print testresults_all_reordered
02062        
02063         ordered_features = ['range','color','all','all_post','baseline']
02064         #ordered_features = ['range','baseline']
02065         
02066         ids = testresults_all_reordered.keys()
02067         ids.sort()
02068         for id in ids:
02069             results = testresults_all_reordered[id]
02070         #for id, results in testresults_all_reordered.iteritems():
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         #print '\\hline\\hline', '\\hline\\hline' 
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        # print '\\hline\\hline', '\\hline\\hline'         
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             #print '######crossvalidation total: ',features,'#########'
02210             #print testresults_total_crossvalidation[features]
02211             #print 'percent surface correct:',percent_surface_correct,'percent clutter correct:',percent_clutter_correct
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         #offscreen_option_old = mlab.options.offscreen
02228         #mlab.options.offscreen = True #do not display in window
02229         
02230         self.scan_dataset = self.scans_database.get_dataset(0)
02231         #get size of training set in total
02232         while False != self.scan_dataset:
02233             if self.scan_dataset.is_labeled: #former: is_testset
02234                 self.generate_and_save_visualizations(id)
02235             
02236             #get next one
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         #get size of training set in total
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             #get next one
02250             self.scan_dataset = self.scans_database.get_next_dataset()
02251 
02252         #mlab.options.offscreen = offscreen_option_old #reset to old value
02253                 
02254     def test_Classifiers(self):
02255         #loop all feature-sets
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             #needed only for display, do before assigning the correct labels!
02260             labels, testresults = classifier.test()
02261             self.save_classifier_labels(labels, testresults, features)
02262         
02263         #test all+postprocessing:
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         #save data:
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     #as input for histogram/placement statistics/...
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 


laser_camera_segmentation
Author(s): Martin Schuster, Advisor: Prof. Charlie Kemp, Lab: Healthcare Robotics Lab at Georgia Tech
autogenerated on Wed Nov 27 2013 11:56:44