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 perform_robust_multilateration(loc_range_diff_info, depth):
num_segment = len(loc_range_diff_info)
estimated_locations = []
# estimate location from each segment
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
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
zone = None
zone_letter = None
for j in range(num_anchors):
if j == 0:
zone, zone_letter = loc_range_diff[j][0][0], loc_range_diff[j][
0][1]
locations[j] = np.array(loc_range_diff[j][1])
else:
locations[j] = np.array(loc_range_diff[j][0:3])
range_diffs[j - 1] = loc_range_diff[j][3]
# Call Gauss Newton Method for location estimation
initial_soln = np.mean(locations, axis=0)
# replace 3 coordinate with sensor depth
initial_soln[2] = depth
estimated_location = tdoa_multilateration_from_single_segement_gauss_newton(
locations, range_diffs, initial_soln)
# estimated_location = TDOALocalization.perform_tdoa_multilateration_closed_form(locations, range_diffs, depth)
if use_ulab:
bflag_estimated = estimated_location.size() > 0
else:
bflag_estimated = estimated_location.size > 0
# Convert to Lat/Lon
if bflag_estimated > 0:
lat, lon = to_latlon(estimated_location[0], estimated_location[1],
zone, zone_letter)
estimated_locations.append([lat, lon, estimated_location[2]])
return estimated_locations
# Ulab is a numpy-like module for micropython, meant to simplify and speed up common
# mathematical operations on arrays. This basic example shows mean/std on an image.
#
# NOTE: ndarrays cause the heap to be fragmented easily. If you run out of memory,
# there's not much that can be done about it, lowering the resolution might help.
import sensor, image, time, ulab as np
sensor.reset() # Reset and initialize the sensor.
sensor.set_pixformat(
sensor.GRAYSCALE) # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QQVGA) # Set frame size to QVGA (320x240)
clock = time.clock() # Create a clock object to track the FPS.
while (True):
img = sensor.snapshot() # Take a picture and return the image.
a = np.array(img, dtype=np.uint8)
print("mean: %d std:%d" % (np.mean(a), np.std(a)))
def draw_demo():
app.loop = (app.loop + 1) % 200
value = math.cos(math.pi * app.loop / 16) * 10
ui.canvas.draw_string(10, 5, "Seeed Grove",
color=(40 + int(value) * 2, 240 + int(value) * 2, 40 + int(value) * 2), scale=3, mono_space=0)
try:
CubeAudio.event()
if app.isconnected == False:
#if app.loop % 10 == 0:
# tmp = fm.fpioa.get_Pin_num(fm.fpioa.I2C0_SDA)
# fm.register(tmp, fm.fpioa.GPIOHS14)
# sda = GPIO(GPIO.GPIOHS14, GPIO.OUT)
# sda.value(1)
# fm.register(tmp, fm.fpioa.I2C0_SDA, force=True)
# print(app.loop)
#print(app.i2c0.scan())
#tmp = fm.fpioa.get_Pin_num(fm.fpioa.I2C0_SDA)
#fm.register(tmp, fm.fpioa.GPIOHS14)
#sda = GPIO(GPIO.GPIOHS14, GPIO.OUT)
#sda.value(1)
#fm.register(tmp, fm.fpioa.I2C0_SDA, force=True)
CubeAudio.event()
if app.loop % 5 == 1:
result = app.i2c0.scan()
ui.canvas.draw_string(290, 80, "Scan Dev: " + str(result),
color=(140 + int(value) * 5, 240 + int(value) * 5, 140 + int(value) * 5), scale=2, mono_space=0)
if SHT3x_ADDR in result:
app.sht3x = SHT3x(app.i2c0, SHT3x_ADDR)
app.isconnected = True
CubeAudio.event()
if SHT31_ADDR in result:
app.sht3x = SHT3x(app.i2c0, SHT31_ADDR)
app.isconnected = True
CubeAudio.event()
ui.canvas.draw_string(280, 25, "Wait Grove Sensor \n sht31/35 <<< << <-",
color=(140 + int(value) * 5, 240 + int(value) * 5, 140 + int(value) * 5), scale=2, mono_space=0)
if CubeAudio.event() == False:
value = math.cos(math.pi * app.loop / 100) * 50
tmp = int(value)
#print(value)
ui.canvas.draw_circle(0, 0, 100 + tmp, fill=False, color=(0, (150 + tmp) + 10, 0))
ui.canvas.draw_circle(0, 0, 100 + tmp * 2, fill=False, color=(0, (150 + tmp) + 20, 0))
ui.canvas.draw_circle(0, 0, 100 + tmp * 3, fill=False, color=(0, (150 + tmp) + 30, 0))
ui.canvas.draw_circle(0, 0, 100 + tmp * 4, fill=False, color=(0, (150 + tmp) + 40, 0))
else:
data = app.sht3x.read_temp_humd()
# print(data)
if app.sidu == None:
app.sidu = image.Image(os.getcwd() + "/res/images/sidu.jpg")
ui.canvas.draw_circle(350, 160, 100, fill=True, color=(255, 255, 255))
ui.canvas.draw_image(app.sidu, 270, 60, alpha=235 + int(value) * 2)
ui.canvas.draw_string(330, 190, "%.2d" % data[1], scale=4, color=(80, 80, 80))
ui.canvas.draw_rectangle(60, 60, 180, 200, thickness=4, color=(155, 155, 155))
if len(app.points) > 18:
app.points.pop(0)
elif data[0] > 1 and app.temp != int(data[0] * 10):
app.temp = int(data[0] * 10)
app.points.append(app.temp)
for p in range(len(app.points)):
#print(app.points)
if p < 1:
b = (60 + int(10 * (p))), 450 - app.points[p]
ui.canvas.draw_circle(b[0], b[1], 3, fill=True, color=(255, 155, 150))
else:
a, b = ((60 + int(10 * (p-1))), 450 - app.points[p-1]), ((60 + int(10 * (p))), 450 - app.points[p])
ui.canvas.draw_circle(b[0], b[1], 3, fill=False, color=(155, 155, 155))
ui.canvas.draw_line(a[0], a[1], b[0], b[1], thickness=4, color=(255,255,255))
ui.canvas.draw_string(60, 280, "Average temperature: %s" % str(ulab.mean(app.points) / 10.0),
color=(240 + int(value) * 5, 240 + int(value) * 5, 240 + int(value) * 5), scale=2, mono_space=0)
CubeAudio.event()
ui.display()
except Exception as e:
app.layer = 1
app.isconnected = False
app.points=[]
raise e
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),
range(250 - 3, 250)],
dtype=np.float)
#print(np.mean(a))
print(math.isclose(np.mean(a), 246.3333333333333, rel_tol=1e-06,
abs_tol=1e-06))
#print(np.mean(a, axis=0))
result = np.mean(a, axis=0)
ref_result = [245.33333333, 246.33333333, 247.33333333]
for p, q in zip(list(result), ref_result):
print(math.isclose(p, q, rel_tol=1e-06, abs_tol=1e-06))
#print(np.mean(a, axis=1))
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