import numpy as np import matplotlib.pyplot as plt import cv2 def to_real(i, mm, n): u = (mm[1]-mm[0])/n u0 = u/2 return mm[1]-u*i - u0 def to_index(a, N, mm): a_max = mm[1] a_min = mm[0] return int(np.clip(np.floor(N*(a_max-a)/(a_max-a_min)), 0, N-1)) def grid_cell_to_map_cell(i,j, n_bel, n_map): x = to_real(i, [-1.0,1.0], n_bel) y = to_real(j, [-1.0,1.0], n_bel) I = to_index(x, n_map, [-1.0,1.0]) J = to_index(y, n_map, [-1.0,1.0]) return I,J def create_circular_mask(h, w, center=None, radius=None, angle=None, thick=0): # img = np.random.randint(0,2,(4,224,224)) # print (img.shape) # mask = create_circular_mask(224,224, center = (100,100), radius = 20) # img[3,~mask] = 0 # plt.imshow(img[3,:,:]) # plt.show() if center is None: # use the middle of the image center = [int(w/2), int(h/2)] if radius is None: # use the smallest distance between the center and image walls radius = min(center[0], center[1], w-center[0], h-center[1]) Y, X = np.ogrid[:h, :w] dist_from_center = np.sqrt((X - center[0])**2 + (Y-center[1])**2) if angle is None: mask = (dist_from_center <= radius) else: angle_from_center = np.arctan2(Y-center[1],X-center[0]) mask = (dist_from_center <= radius) & (np.abs(np.unwrap(angle_from_center-angle)) * dist_from_center <= thick) #(angle_from_center < angle+0.15/(dist_from_center+0.01)) & (angle_from_center > angle-0.15/(dist_from_center+0.01)) return mask def square_clock(x, n): width = x.shape[2] height = x.shape[1] quater = n//4-1 #even/odd even = 1 - quater % 2 side = quater+2+even N = side*max(width,height) img = np.zeros((N,N)) for i in range(n): s = (i+n//8)%n if s < n//4: org = (0, n//4-s) elif s < n//2: org = (s-n//4+even, 0) elif s < 3*n//4: org = (n//4+even, s-n//2+even) else: org = (n//4-(s-3*n//4), n//4+even) ox = org[0]*height oy = org[1]*width img[ox:ox+height, oy:oy+width] = x[i,:,:] del x return img, side def square_array(img, n): output = np.zeros((n*img.shape[1], n*img.shape[2]),np.float32) for i in range(n): for j in range(n): output[i*n:(i+1)*n,j*n:(j+1)*n]=img[i*n+j] return output def wrap(phase): # wrap into [-pi, pi] phase = ( phase + np.pi) % (2 * np.pi ) - np.pi return phase def wrap_2pi(phase): # wrap into [0, 2*pi] phase = ( phase) % (2 * np.pi ) return phase def distort_map(img, rows, cols): # done making a map for LM : self.map_for_LM # try erode and dilate kernel = np.array([[0,1,1,1,0], [1,1,1,1,1], [1,1,1,1,1], [1,1,1,1,1], [0,1,1,1,0]], np.uint8) # expand img boarder = img-cv2.erode(img,np.ones((3,3),np.uint8),iterations=1) img1 = np.clip(boarder,0,1) # leave portion img1 = np.clip(img1 * (np.random.rand(rows,cols) > 0.5).astype(int) + img-boarder, 0, 1) img1 = cv2.dilate(img1, kernel, iterations = 1) # shrink img2 = cv2.erode(img1, kernel, iterations = 1) # img2 = cv2.morphologyEx(img2, cv2.MORPH_CLOSE, kernel) img3 = img2 n = np.random.randint(5) for _ in range(n): img3 = img3 * (np.random.rand(rows,cols) > 0.33).astype(int) #img3 = img2*np.random.randint(2,size=[224,224]) img3 = cv2.dilate(img3, kernel, iterations = 2) img3 = cv2.erode(img3, kernel, iterations = 2) return img3 def fill_outer_rim(img, rows, cols): for i in range(rows): j = 0 while img[i,j] == 0: # print (i,j,img[i,j]) img[i,j] = 1.0 j = j+1 if j>min(i,rows-i): break for i in range(rows): j = -1 while img[i,j] == 0: # print (i,j,img[i,j]) img[i,j] = 1.0 j = j-1 if j < -min(i,rows-i): break for j in range(cols): i = 0 while img[i,j] == 0: # print (i,j,img[i,j]) img[i,j] = 1.0 i = i + 1 if i > min(j, cols-j): break for j in range(cols): i = -1 while img[i,j] == 0: # print (i,j,img[i,j]) img[i,j] = 1.0 i = i - 1 if i < -min(j, cols-j): break return img def transform(s,g,t,q): d = g-s theta = np.arctan2(d[1],d[0]) R = np.array( [[np.cos(theta), np.sin(theta)], [-np.sin(theta), np.cos(theta)]]) b=t-g q=wrap(q-theta) return np.append(R.dot(b),q) def control_law(fwd_err,lat_err,ang_err, t_elapse): lin_gain = 0.8 rot_gain = 2.0 #k time_gain = 0.5 min_ang_vel = - 0.01 * np.sign(ang_err) eps = 0.05 max_lin_vel = 0.3 max_lat_err = 0.05 min_offset_lin_vel = -0.03 * np.sign(fwd_err) lin_vel = -lin_gain*fwd_err+min_offset_lin_vel lin_vel = np.clip(lin_vel, -max_lin_vel, min(max_lin_vel,t_elapse*time_gain)) slide_var = wrap(ang_err+np.arctan(rot_gain*lat_err)) if np.abs(slide_var) > np.arctan(rot_gain*max_lat_err): lin_vel = 0.0 ang_gain = 0.5 #0.4+np.abs(lin_vel)*0.5 #b1+w_bar ang_vel = - rot_gain*lin_vel*np.sin(ang_err)/(1.0+(rot_gain*lat_err)**2) \ - ang_gain*np.clip(slide_var/eps,-1.0,1.0) \ + min_ang_vel return lin_vel, ang_vel def define_tf(src, tgt): # src = observed pose from source coordinates # tgt = observed pose from target coordinates # if you need robot pose from map coordinates given that from odom coordinates # get map_T_robot = map_T_odom * odom_T_robot: you need map_T_odom # map_T_odom = define_tf(map, odom) src_T_obs = np.zeros((3,3), dtype=np.float32) tgt_T_obs = np.zeros((3,3), dtype=np.float32) src_T_obs[-1,-1] = 1.0 tgt_T_obs[-1,-1] = 1.0 theta = src[2] src_T_obs[:2,:2] = np.array( [[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]) src_T_obs[:2,2] = np.array([src[0], src[1]],dtype=np.float32).transpose() theta = tgt[2] tgt_T_obs[:2,:2] = np.array( [[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]) tgt_T_obs[:2,2] = np.array([tgt[0], tgt[1]],dtype=np.float32).transpose() # print (tgt_T_obs) src_T_tgt = np.dot(src_T_obs, np.linalg.inv(tgt_T_obs)) #src_T_tgt = np.dot(src_T_obs, inv_tf(tgt_T_obs)) return src_T_tgt def inv_tf(T): R = T[:2, :2] p = T[:2, 2] out = np.zeros_like(T) out[:2,:2]=R.transpose() out[:2, 2]=-np.dot(R.transpose(), p) out[2,2] = 1.0 return out def tuple_to_hg(p): T = np.zeros((3,3), dtype=np.float32) T[-1,-1] = 1.0 theta = p[2] T[:2,:2] = np.array( [[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]) T[:2,2] = np.array([p[0], p[1]],dtype=np.float32).transpose() return T def hg_to_tuple(T): theta = np.arctan2(T[1,0], T[0,0]) x = T[0,2] y = T[1,2] return (x,y,theta) if __name__ == "__main__": img = (np.random.rand(224,224)>0.75).astype(int) print (img.shape) # angle = np.pi*np.random.rand() x,y = np.random.randint(0, 224, 2) angle = np.arctan2(y-112,x-112)+np.pi/2 mask = create_circular_mask(224,224, center = (x,y), radius = 20, angle=angle, thick=3) img[~mask] = 0 plt.imshow(img[:,:]) plt.title('angle=%s'%np.rad2deg(angle)) plt.plot(x,y,'o') plt.show()