00001 import roslib; roslib.load_manifest('hrl_lib')
00002
00003 import cv
00004 import numpy as np
00005 import hrl_lib.tf_utils as tfu
00006 import hrl_opencv.blob as blob
00007 import copy
00008 import pdb
00009
00010
00011
00012
00013
00014 def calc_normal(points3d, p=np.matrix([-1,0,0.]).T):
00015
00016 u, s, vh = np.linalg.svd(np.cov(points3d))
00017 u = np.matrix(u)
00018
00019
00020 if (u[:,2].T * p)[0,0] < 0:
00021 normal = -u[:,2]
00022 else:
00023 normal = u[:,2]
00024 return normal
00025
00026
00027
00028
00029
00030
00031 def indices_of_points_in_frame(points, cal_obj):
00032 valid_indices = np.where(np.multiply(np.multiply(points[0,:] > 0, points[0,:] < cal_obj.w-.6),
00033 np.multiply(points[1,:] > 0, points[1,:] < cal_obj.h-.6)))[1].A1
00034 return valid_indices
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 def local_window(location, bw_image, winsize, resize_to=None, flatten=True):
00047 loc = np.matrix(np.round(location), dtype='int')
00048 start = loc - winsize
00049 patch_size = winsize*2+1
00050 img_width = bw_image.shape[1]
00051 img_height = bw_image.shape[0]
00052
00053 r = blob.Rect(start[0,0], start[1,0], patch_size, patch_size).keep_inside(0, img_width-1, 0, img_height-1)
00054 if r.width < patch_size or r.height < patch_size:
00055 return None
00056 else:
00057
00058
00059
00060 subrect = bw_image[r.y:r.y+r.height, r.x:r.x+r.width, :]
00061
00062
00063
00064 if resize_to != None:
00065 rescaled = np.zeros((resize_to*2+1, resize_to*2+1, subrect.shape[2]), dtype='uint8')
00066 cv.Resize(cv.fromarray(subrect), cv.fromarray(rescaled), cv.CV_INTER_LINEAR)
00067 subrect = rescaled
00068
00069 if flatten:
00070
00071
00072
00073
00074
00075
00076 try:
00077 intensity = np.matrix(np.reshape(subrect, (subrect.shape[0]*subrect.shape[1]*subrect.shape[2], 1))) / 255.
00078 except TypeError, e:
00079
00080 intensity = np.matrix(np.reshape(subrect, (subrect.shape[0]*subrect.shape[1]*subrect.shape[2], 1))) / 255.
00081
00082
00083
00084
00085
00086
00087 else:
00088 intensity = subrect
00089
00090 return intensity
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101 def combine_scan_and_image_laser_frame(points_in_laser_frame, image_T_laser, image, cal_obj):
00102 points_in_image_frame = tfu.transform_points(image_T_laser, points_in_laser_frame)
00103 return combine_scan_and_image(points_in_image_frame, image, cal_obj)
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113 def combine_scan_and_image(points_in_image_frame, imagea, cal_obj):
00114 p2d = cal_obj.project(points_in_image_frame)
00115 valid_indicies = indices_of_points_in_frame(p2d, cal_obj)
00116
00117 vp2d = p2d[:, valid_indicies]
00118 vp2d = np.matrix(np.round(vp2d), dtype='int')
00119
00120 vpoints = points_in_image_frame[:, valid_indicies]
00121
00122
00123 intensity_channels = imagea[vp2d[1,:].A1, vp2d[0,:].A1, :]
00124
00125 return vpoints, (np.matrix(intensity_channels).T / 255.), vp2d
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135 def select_rect(center, w, h, depth, points):
00136 limits = [[center[0,0]-w, center[0,0]+w],
00137 [center[1,0]-h, center[1,0]+h],
00138 [center[2,0]-depth, center[2,0]+depth]]
00139 return select_volume(limits, points), limits
00140
00141
00142
00143
00144
00145
00146
00147 def select_volume(limits, points):
00148 xlim, ylim, zlim = limits
00149 xlim_sat = np.multiply(points[0, :] > xlim[0], points[0, :] < xlim[1])
00150 ylim_sat = np.multiply(points[1, :] > ylim[0], points[1, :] < ylim[1])
00151 zlim_sat = np.multiply(points[2, :] > zlim[0], points[2, :] < zlim[1])
00152 selected = np.multiply(np.multiply(xlim_sat, ylim_sat), zlim_sat)
00153 if np.sum(selected) <= 0:
00154 return None
00155 return points[:, np.where(selected)[1].A1]
00156
00157
00158
00159
00160
00161
00162