def get_dist(a, b, p): u = np.sum((p - a) * (b - a)) l = np.sum((b - a) * (b - a)) t = u / l x = a + (b - a) * t dist = np.sum((p - x) * (p - x)) if dist > 200.0: return 50.0 else: return t
def match(tester,dataset): codex = np.array(dataset) t = np.array(tester) for i in range(codex.shape()[0]): codex[i,:] = codex[i,:] - t data = np.sum(codex * codex, axis=1) data = np.sqrt(data) if codex.shape()[0] == 1: return data,1 ind = np.argmin(data) return data[ind][0],ind + 1
def get_nearest(feature_list, feature): min_dist = 10000 name = '' vec = [] for n, v in feature_list: dist = np.sum((v - feature) * (v - feature)) if dist < min_dist: min_dist = dist name = n vec = v return name, min_dist, vec
def spatial_filter(img, kernel, dxy=None): """ Convolves `img` with `kernel` assuming a stride of 1. If `img` is linear, `dxy` needs to hold the dimensions of the image. """ # Make sure that the image is 2D _img = np.array(img) if dxy is None: dx, dy = _img.shape() isInt = type(img[0:0]) isFlat = False else: dx, dy = dxy if dx * dy != _img.size(): raise TypeError("Dimensions do not match number of `img` elements") isInt = type(img[0]) isFlat = True img2d = _img.reshape((dx, dy)) # Check if kernel is a square matrix with an odd number of elements per row # and column krn = np.array(kernel) dk, dk2 = krn.shape() if dk != dk2 or dk % 2 == 0 or dk <= 1: raise TypeError( "`kernel` is not a square 2D matrix or does not have an " "odd number of row / column elements") # Make a padded copy of the image; pad with mean of image to reduce edge # effects padd = dk // 2 img2dp = np.ones((dx + padd * 2, dy + padd * 2)) * np.mean(img2d) img2dp[padd:dx + padd, padd:dy + padd] = img2d imgRes = np.zeros(img2dp.shape()) # Convolve padded image with kernel for x in range(0, dx): for y in range(0, dy): imgRes[x + padd, y + padd] = np.sum(img2dp[x:x + dk, y:y + dk] * krn[:, :]) # Remove padding, flatten and restore value type if needed _img = imgRes[padd:dx + padd, padd:dy + padd] if isFlat: _img = _img.flatten() if isInt: _img = list(np.array(_img, dtype=np.int16)) return _img
def least_median_square_estimate(loc_range_diff_info: list, depth: float, max_iters: int) -> list: num_segments = len(loc_range_diff_info) lead_anchor_locations_list = [] asst_anchor_locations_list = [] range_diffs_list = [] zone_info_list = [] lut = [] for i in range(num_segments): loc_range_diff = loc_range_diff_info[i] num_anchors = len(loc_range_diff) lut += [i] * (num_anchors - 1) for j in range(num_anchors): if j == 0: zone, zone_letter = loc_range_diff[j][0][0], loc_range_diff[j][ 0][1] zone_info_list.append([zone, zone_letter]) lead_anchor_locations_list.append(loc_range_diff[j][1]) else: asst_anchor_locations_list.append(loc_range_diff[j][0:3]) range_diffs_list.append(loc_range_diff[j][3]) # Check if all anchors belong to same zone bflag_zone = True zone, zone_letter = zone_info_list[0][0], zone_info_list[0][1] for zone_info in zone_info_list: if zone_info != [zone, zone_letter]: bflag_zone = False break if not bflag_zone: raise ValueError("All the anchors are not in same zone!") num_asst_anchors = len(lut) # Generate subsets of assistant anchors with each subset containing 3 elements subsets = list(random_combinations(range(num_asst_anchors), 3, max_iters)) # For each subset, calculate the unknown location using multilateration # and then calculate median of square residues = (range_diff - (SA - SB))**2 , # where A and B represent the lead and the assistant anchor, respectively, and # S represent estimated unknown location min_median = np.inf robust_location = None iter_count = 0 for subset in subsets: # Pack together the assistant and the corresponding lead anchor's location anchor_locations = [] range_diffs = [] for index in subset: lead_anchor = lead_anchor_locations_list[lut[index]] asst_anchor = asst_anchor_locations_list[index] anchors = np.array([lead_anchor, asst_anchor]) range_diff = np.array([range_diffs_list[index]]) anchor_locations.append(anchors) range_diffs.append(range_diff) # Calculate initial soln as mean of Anchor Positions mean_anchor_location = np.zeros(3, dtype=np.float) num_anchors = 0 for anchor in anchor_locations: mean_anchor_location = mean_anchor_location + np.sum(anchor, axis=0) if use_ulab: num_anchors += anchor.shape()[0] else: num_anchors += anchor.shape[0] init_soln = mean_anchor_location / num_anchors init_soln[2] = depth # Estimate location from selected subset of anchors using Gauss-Newton method sensor_location = tdoa_multilateration_from_multiple_segments_gauss_newton( anchor_locations, range_diffs, init_soln) # For each estimated location calculate the square of residues, i.e., (observed_range_diff - calculated_range_diff)**2 sq_residues = [] for index in subset: lead_anchor = np.array(lead_anchor_locations_list[lut[index]]) asst_anchor = np.array(asst_anchor_locations_list[index]) range_diff = np.array([range_diffs_list[index]]) lead_to_sensor = np.linalg.norm((lead_anchor - sensor_location)) asst_to_sensor = np.linalg.norm((asst_anchor - sensor_location)) estimated_range_diff = lead_to_sensor - asst_to_sensor sq_residues.append((range_diff - estimated_range_diff)**2) # Calculate Median of Square-Residues current_median = np.median(np.array(sq_residues)) # Select the subset which results into lowest median of errors if current_median <= min_median: min_median = current_median robust_location = sensor_location if iter_count >= max_iters: break iter_count += 1 # Convert the location into Lat/Lon system try: lat, lon = to_latlon(robust_location[0], robust_location[1], zone, zone_letter) estimated_location = [lat, lon, robust_location[2]] except Exception as e: print("Not a valid estimate: " + str(e)) estimated_location = [] return estimated_location
def tdoa_multilateration_from_all_segments_gauss_newton( loc_range_diff_info: list, depth: float, max_iters: int = 10, eta: float = 1E-3, abs_tol: float = 1E-6): num_segment = len(loc_range_diff_info) anchor_location_list = [] zone_list = [] anchor_rangediff_list = [] for i in range(num_segment): loc_range_diff = loc_range_diff_info[i] num_anchors = len(loc_range_diff) # create numpy array to hold anchor locations and range-differences anchor_locations = np.zeros((num_anchors, 3), dtype=np.float) range_diffs = np.zeros(num_anchors - 1, dtype=np.float) # extract locations and range-differences from list and store them into respective numpy array for j in range(num_anchors): if j == 0: zone, zone_letter = loc_range_diff[j][0][0], loc_range_diff[j][ 0][1] zone_list.append([zone, zone_letter]) anchor_locations[j] = np.array(loc_range_diff[j][1]) else: anchor_locations[j] = np.array(loc_range_diff[j][0:3]) range_diffs[j - 1] = loc_range_diff[j][3] # Collect anchor locations and range-differences arrays anchor_location_list.append(anchor_locations) anchor_rangediff_list.append(range_diffs) # Check that all zone and zone letters are same bflag_zone = True zone, zone_letter = zone_list[0][0], zone_list[0][1] for zone_info in zone_list: if zone_info != [zone, zone_letter]: bflag_zone = False break if not bflag_zone: raise ValueError("All the anchors are not in same zone!") # =========== Gauss Newton Iterations for Location Estimation ========== # # Initial Solution is taken as Mean location of all the Anchors mean_anchor_location = np.zeros(3, dtype=np.float) num_anchors = 0 for anchor_location in anchor_location_list: if use_ulab: mean_anchor_location = mean_anchor_location + np.sum( anchor_location, axis=0) num_anchors += anchor_location.shape()[0] else: mean_anchor_location = mean_anchor_location + np.sum( anchor_location, axis=0) num_anchors += anchor_location.shape[0] mean_anchor_location = mean_anchor_location / num_anchors soln = np.array(mean_anchor_location) soln[2] = depth # replace 3rd coordinate with sensor depth # Main iterations for j in range(max_iters): # ==== Calculate the LHS and RHS of Normal Equation: J'*J*d = -J'*res, # where J is Jacobian, d is descent direction, res is residual ====== # jTj = np.zeros((2, 2)) jTres = np.zeros((2, 1)) for anchor_locations, range_diffs in zip(anchor_location_list, anchor_rangediff_list): if use_ulab: m = anchor_locations.shape()[0] else: m = anchor_locations.shape[0] denom_1 = np.linalg.norm(soln - anchor_locations[0]) for i in range(1, m): denom_2 = np.linalg.norm(soln - anchor_locations[i]) res = (range_diffs[i - 1] - (denom_1 - denom_2)) del_res = np.array( [[((soln[0] - anchor_locations[i][0]) / denom_2 - (soln[0] - anchor_locations[0][0]) / denom_1)], [((soln[1] - anchor_locations[i][1]) / denom_2 - (soln[1] - anchor_locations[0][1]) / denom_1)]]) if use_ulab: jTj = jTj + np.linalg.dot(del_res, del_res.transpose()) else: jTj = jTj + np.dot(del_res, del_res.transpose()) jTres = jTres + (del_res * res) # ===================== calculation ENDs here ======================================================= # # ============= Take the next step: ====================== # # Modification according to Levenberg-Marquardt suggestion jTj = jTj + np.diag(np.diag(jTj) * eta) eta = eta / 2.0 # Invert 2x2 matrix denom = jTj[0][0] * jTj[1][1] - jTj[1][0] * jTj[0][1] numer = np.array([[jTj[1][1], -jTj[0][1]], [-jTj[1][0], jTj[0][0]]]) if denom >= 1E-9: inv_jTj = numer / denom if use_ulab: temp = np.linalg.dot(inv_jTj, jTres) else: temp = np.dot(inv_jTj, jTres) del_soln = np.array([temp[0][0], temp[1][0], 0.0]) soln = soln - del_soln if np.linalg.norm(del_soln) <= abs_tol: break else: print("Can't invert the matrix!") break # Convert to Lat/Lon try: lat, lon = to_latlon(soln[0], soln[1], zone, zone_letter) estimated_location = [lat, lon, soln[2]] except Exception as e: print("Not a valid estimate: " + str(e)) estimated_location = [] return estimated_location
def get_dis(new_data,master_data): dist = np.sum((new_data-master_data)*(new_data-master_data)) return dist
dtype=np.uint8) b = np.array( [range(2**56 - 3, 2**56), range(2**16 - 3, 2**16), range(2**8 - 3, 2**8)], dtype=np.float) print(np.sort(a, axis=None)) print(np.sort(b, axis=None)) print(np.sort(a, axis=0)) print(np.sort(b, axis=0)) print(np.sort(a, axis=1)) print(np.sort(b, axis=1)) print("Testing np.sum:") a = np.array([253, 254, 255], dtype=np.uint8) print(np.sum(a)) print(np.sum(a, axis=0)) a = np.array([range(255 - 3, 255), range(240 - 3, 240), range(250 - 3, 250)], dtype=np.float) print(np.sum(a)) print(np.sum(a, axis=0)) print(np.sum(a, axis=1)) print("Testing np.mean:") a = np.array([253, 254, 255], dtype=np.uint8) print(np.mean(a)) print(np.mean(a, axis=0)) a = np.array([range(255 - 3, 255), range(240 - 3, 240),
def find_blobs(img, dxy, nsd=1.0): """ Detect continues area(s) ("blobs") with pixels above a certain threshold in an image. `img` contains the flattened image (1D), `dxy` image width and height, and `nsd` a factor to calculate the blob threshold from image mean and s.d. (thres = avg +sd *nsd). """ # Initialize blobs = [] for i in range(MAX_BLOBS): blobs.append(blob_struct()) nBlobs = 0 posList = [] # Extract the parameters dx, dy = dxy n = dx * dy # Copy image data into a float array pImg = np.array(img) # Calculate mean and sd across (filtered) image to determine threshold avg = np.mean(pImg) sd = np.std(pImg) thres = avg + sd * nsd # Find blob(s) ... # # Mark all pixels above a threshold pPrb = (pImg - avg) / sd pMsk = (pImg >= thres) * 255 nThres = int(np.sum(pMsk) / 255) # Check if these above-threshold pixels represent continuous blobs nLeft = nThres iBlob = 0 while nLeft > 0 and iBlob < MAX_BLOBS: # As long as unassigned mask pixels are left, find the next one using # `np.argmax()`, which returns the index of the (first) # hightest value, which should be 255 iPix = np.argmax(pMsk) x = iPix % dx y = iPix // dx # Unassigned pixel found ... posList.append((x, y)) pMsk[x + y * dx] = iBlob nFound = 1 bx = float(x) by = float(y) bp = pPrb[x + y * dx] # Find all unassigned pixels in the neighborhood of this seed pixel while len(posList) > 0: x0, y0 = posList.pop() for k in range(4): x1 = x0 + xoffs[k] y1 = y0 + yoffs[k] if ((x1 >= 0) and (x1 < dx) and (y1 >= 0) and (y1 < dy) and (pMsk[int(x1 + y1 * dx)] == 255)): # Add new position from which to explore posList.append((x1, y1)) pMsk[int(x1 + y1 * dx)] = iBlob nFound += 1 bx += float(x1) by += float(y1) bp += pPrb[int(x1 + y1 * dx)] # Update number of unassigned pixels nLeft -= nFound # Store blob properties (area, center of gravity, etc.); make sure that # the blob list remaines sorted by area k = 0 if iBlob > 0: while (k < iBlob) and (blobs[k].area > nFound): k += 1 if k < iBlob: blobs.insert(k, blob_struct()) blobs[k].ID = iBlob blobs[k].area = nFound blobs[k].x = by / nFound blobs[k].y = bx / nFound blobs[k].prob = bp / nFound iBlob += 1 nBlobs = iBlob # Copy blobs into list as function result tempL = [] for i in range(nBlobs): if blobs[i].area > 0: tempL.append(blobs[i].as_list) # Return list of blobs, otherwise empty list return tempL
start = time.ticks_us() for i in range(100): c[i] = a[i] - b[i] print("Python: " + str(time.ticks_diff(time.ticks_us(), start)) + " us") print("c=", c) #NumPy Calculate Diff na = np.array(a) nb = np.array(b) start = time.ticks_us() nc = na - nb print("Numpy: " + str(time.ticks_diff(time.ticks_us(), start)) + " us") print("nc=", nc) #Normal Calculate Sum start = time.ticks_us() sum = 0 for i in range(100): sum += a[i] print("Python: " + str(time.ticks_diff(time.ticks_us(), start)) + " us") print() print("sum=", sum) #NumPy Calculate Sum start = time.ticks_us() np_sum = np.sum(a) print("Numpy: " + str(time.ticks_diff(time.ticks_us(), start)) + " us") print("np_sum=", np_sum)