00001 import roslib; roslib.load_manifest('hai_sandbox')
00002 import rospy
00003 import cv
00004 import numpy as np
00005 import feature_extractor_fpfh.srv as fsrv
00006 import hrl_lib.image3d as i3d
00007 import hrl_lib.rutils as ru
00008 import hrl_lib.prob as pr
00009 import hrl_lib.tf_utils as tfu
00010 import pdb
00011
00012
00013
00014
00015 def sample_points(weights, N):
00016 assert(weights.shape[0] >= N)
00017
00018 M = weights.shape[0]
00019 weights = weights / np.sum(weights)
00020 r = np.random.rand() * (1.0/M)
00021 c = weights[0,0]
00022 i = 0
00023 Xt = []
00024
00025 indices = np.sort(np.random.permutation(np.arange(1, M+1))[0:N]).tolist()
00026 for m in indices:
00027 U = r + (m - 1) * (1.0/M)
00028 while U > c:
00029 i = i + 1
00030 c = c + weights[i,0]
00031 Xt.append(i)
00032 return Xt
00033
00034
00035 def test_sample_points():
00036 w = np.matrix([.1,.4,.5]).T
00037 count = [0., 0., 0.]
00038
00039 for i in range(6000.):
00040 idx = sample_points(w, 2)
00041 for x in idx:
00042 count[x] += 1
00043 print np.matrix(count) / np.sum(count)
00044
00045
00046 def intensity_pyramid_feature(point2d_image, np_image_arr, win_size, multipliers, flatten=True):
00047 invalid_location = False
00048 local_intensity = []
00049 for multiplier in multipliers:
00050 if multiplier == 1:
00051 features = i3d.local_window(point2d_image, np_image_arr, win_size, flatten=flatten)
00052 else:
00053 features = i3d.local_window(point2d_image, np_image_arr, win_size*multiplier,
00054 resize_to=win_size, flatten=flatten)
00055 if features == None:
00056 invalid_location = True
00057 break
00058 else:
00059 local_intensity.append(features)
00060 if invalid_location:
00061 return None
00062 else:
00063 if flatten:
00064 return np.row_stack(local_intensity)
00065 else:
00066 return local_intensity
00067
00068 class Subsampler:
00069 def __init__(self):
00070 self.proxy = rospy.ServiceProxy('subsample', fsrv.SubsampleCalc)
00071
00072 def subsample(self, points3d, frame='base_link'):
00073 req = fsrv.SubsampleCalcRequest()
00074 req.input = ru.np_to_pointcloud(points3d, frame)
00075 res = self.proxy(req)
00076 return ru.pointcloud_to_np(res.output)
00077
00078
00079 class IntensityCloudFeatureExtractor:
00080
00081 def __init__(self, pointcloud_bl, cvimage_mat, expected_loc_bl, distance_feature_points,
00082 image_T_bl, camera_calibration, params):
00083
00084 self.pointcloud_bl = pointcloud_bl
00085 self.cvimage_mat = cvimage_mat
00086 self.expected_loc_bl = expected_loc_bl
00087 self.distance_feature_points = distance_feature_points
00088
00089 self.image_T_bl = image_T_bl
00090 self.camera_calibration = camera_calibration
00091 self.params = params
00092 self.subsampler_service = Subsampler()
00093 self.sizes = None
00094
00095 def get_sizes(self):
00096 return self.sizes
00097
00098 def _subsample(self):
00099 rospy.loginfo('Subsampling using PCL')
00100 rospy.loginfo('before %s' % str(self.pointcloud_bl.shape))
00101 self.pc_sub_samp_bl = self.subsampler_service.subsample(self.pointcloud_bl)
00102 rospy.loginfo('after %s' % str(self.pc_sub_samp_bl.shape))
00103
00104 def _sample_points(self):
00105 rospy.loginfo('Sampling points')
00106
00107 gaussian = pr.Gaussian(self.expected_loc_bl, \
00108 np.matrix([[self.params.uncertainty_x**2, 0, 0], \
00109 [0, self.params.uncertainty_y**2, 0], \
00110 [0, 0, self.params.uncertainty_z**2]]))
00111
00112 pdf = gaussian.pdf_mat()
00113 probs = np.matrix(pdf(self.pc_sub_samp_bl))
00114
00115
00116 n_samples = min(self.params.n_samples, self.pc_sub_samp_bl.shape[1])
00117 pt_indices = list(set(sample_points(probs.T, n_samples)))
00118
00119
00120 sampled_pts3d_bl = self.pc_sub_samp_bl[:, pt_indices]
00121 sampled_pts3d_image = tfu.transform_points(self.image_T_bl, sampled_pts3d_bl)
00122 sampled_pts2d = self.camera_calibration.project(sampled_pts3d_image)
00123 sampled_pix2d = np.matrix(np.round(sampled_pts2d))
00124
00125
00126 x = sampled_pix2d[0,:]
00127 y = sampled_pix2d[1,:]
00128 good_pts = np.where((x >= 0) + (x < self.camera_calibration.w) \
00129 + (y >= 0) + (y < self.camera_calibration.h))[1].A1
00130
00131 sampled_pts3d_bl = sampled_pts3d_bl[:, good_pts]
00132 sampled_pix2d = sampled_pix2d[:, good_pts]
00133
00134 rospy.loginfo('got %s good points' % str(sampled_pix2d.shape[1]))
00135 return sampled_pts3d_bl, sampled_pix2d
00136
00137 def feature_vec_at(self, point3d_bl, point2d_image):
00138 fea_calculated = []
00139
00140
00141 distance_feas = None
00142 if self.distance_feature_points != None:
00143 distance_feas = np.power(np.sum(np.power(self.distance_feature_points - point3d_bl, 2), 0), .5).T
00144 fea_calculated.append(distance_feas)
00145
00146
00147 intensity = intensity_pyramid_feature(point2d_image, np.asarray(self.cvimage_mat),
00148 self.params.win_size, self.params.win_multipliers, True)
00149
00150 if intensity == None:
00151 return None
00152 else:
00153 fea_calculated.append(intensity)
00154
00155 if self.sizes == None:
00156 self.sizes = {}
00157 if distance_feas != None:
00158 self.sizes['distance'] = distance_feas.shape[0]
00159 self.sizes['intensity'] = intensity.shape[0]
00160
00161 return fea_calculated
00162
00163 def extract_features(self):
00164 self._subsample()
00165 sampled_pts3d_bl, sampled_pix2d = self._sample_points()
00166 features_l = []
00167 pts_with_features = []
00168
00169 rospy.loginfo('Extracting features')
00170 for i in range(sampled_pts3d_bl.shape[1]):
00171 features = self.feature_vec_at(sampled_pts3d_bl[:,i], sampled_pix2d[:,i])
00172 if features != None:
00173 features_l.append(features)
00174 pts_with_features.append(i)
00175 if i % 500 == 0:
00176 rospy.loginfo(i)
00177
00178 features_by_type = zip(*features_l)
00179 xs = np.row_stack([np.column_stack(f) for f in features_by_type])
00180 rospy.loginfo('Finished feature extraction.')
00181 return xs, sampled_pix2d[:, pts_with_features], sampled_pts3d_bl[:, pts_with_features]
00182
00183