Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 from classifier import classifier
00031
00032 import hrl_lib.util as ut
00033 import numpy as np
00034
00035 from hrl_lib.util import getTime
00036
00037 import processor
00038
00039 class baseline_classifier(classifier):
00040 '''
00041 classdocs
00042 '''
00043
00044
00045
00046
00047
00048
00049
00050
00051 def test(self, feature_data = None):
00052
00053 print getTime(), 'test on:', self.processor.scan_dataset.id
00054
00055 if feature_data == None:
00056 filename = self.processor.get_features_filename()
00057 dict = ut.load_pickle(filename)
00058 else:
00059 dict = feature_data
00060
00061 baseline_labels = self.classify_baseline_code()
00062
00063 return baseline_labels, self.test_results(dict, baseline_labels)
00064
00065
00066 def classify_baseline_code(self):
00067 import hrl_tilting_hokuyo.processing_3d as p3d
00068 import hrl_tilting_hokuyo.occupancy_grid_3d as og3d
00069 import hrl_tilting_hokuyo.display_3d_mayavi as d3m
00070 pt = np.matrix(self.processor.point_of_interest).T
00071
00072 width_half = self.processor.voi_width / 2.0
00073 brf = pt+np.matrix([-width_half,-width_half,-width_half]).T
00074 tlb = pt+np.matrix([width_half, width_half, width_half]).T
00075 resolution = np.matrix([0.1,0.1,0.0025]).T
00076 max_dist = 15
00077 min_dist = -15
00078 gr = og3d.occupancy_grid_3d(brf,tlb,resolution)
00079 print 'filling grid...'
00080 gr.fill_grid(self.processor.pts3d_bound)
00081 print '...filled.'
00082 gr.to_binary(1)
00083 l = gr.find_plane_indices(assume_plane=True,hmin=0.3,hmax=2)
00084 z_min = min(l)*gr.resolution[2,0]+gr.brf[2,0]
00085 z_max = max(l)*gr.resolution[2,0]+gr.brf[2,0]
00086
00087 pts = np.asarray(self.processor.pts3d_bound)
00088 conditions_surface = np.multiply(pts[2,:] > z_min, pts[2,:] < z_max)
00089 print 'cf',conditions_surface
00090 conditions_clutter = np.invert(conditions_surface)
00091 conditions_surface = np.multiply(conditions_surface, np.array(self.processor.map_polys) > 0)
00092 print 'cf',conditions_surface
00093 idx_surface = np.where(conditions_surface)
00094 conditions_clutter = np.multiply(conditions_clutter, np.array(self.processor.map_polys) > 0)
00095 idx_clutter = np.where(conditions_clutter)
00096
00097 n, m = np.shape(self.processor.pts3d_bound)
00098 print n,m
00099 labels = np.zeros(m)
00100 print np.shape(labels), labels
00101 print np.shape(idx_surface), idx_surface
00102 labels[idx_surface] = processor.LABEL_SURFACE
00103 labels[idx_clutter] = processor.LABEL_CLUTTER
00104
00105 print labels
00106
00107 return labels
00108
00109