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)
