def compute_extrinsics(self, fg_list, p_inliers):
"""
computes extrinsic parameters i.e. orientation,width,height.
returns major axis and look-up table for width,height of each pixel.
:param raw_frame_list:
:param history:
:param n_mixture:
:param p_background:
:param p_inliers:
:return:
"""
assert fg_list, "extrincs, compute_extrinsics : no frames "
timestamp = time.time()
assert fg_list, "extrincs, compute_extrinsics : no fg mask found"
list_ellipse, tree_ellipse = self.get_list_fitting_polygon(fg_list)
ransac = RANSAC()
self.vvpoint, model_components = ransac.do_ellipse_ransac(list_ellipse=list_ellipse, thres=1, p_inliers=0.5)
size_dic = self.prepare_size_ransac(list_ellipse)
self.test_size(list_ellipse)
self.size_model = ransac.do_size_ransac(size_dic, 1, 0.3)
print "took ", time.time() - timestamp, "s"
print "vertical vanishing point is ", self.vvpoint, " size models are ", self.size_model
self.display_size_model(size_dic, self.size_model)
return tree_ellipse
def main(image1, image2):
p1 = load_image_to_tuple(image1)
p2 = load_image_to_tuple(image2)
npp1 = np.asarray(p1[2:], dtype=float)
npp2 = np.asarray(p2[2:], dtype=float)
knna, knnb, atoa, btob, distances = k_neirest_neighbours(npp1, npp2, k=200)
to_paint = points_cohesion(knna, knnb, atoa, btob, distances)
selected_pairsa = npp1[to_paint]
selected_knna = knna[to_paint]
paint(selected_pairsa, npp2, selected_knna, image1, image2)
# RANSAC
transformation_tuple_a = RANSAC(selected_pairsa, npp2, selected_knna)
paint(selected_pairsa, npp2, selected_knna, image1, image2,
transformation_tuple_a, TransformationType.AFFINE)
transformation_tuple_b = RANSAC(
selected_pairsa,
npp2,
selected_knna,
transformation=TransformationType.PERSPECTIVE)
paint(selected_pairsa, npp2, selected_knna, image1, image2,
transformation_tuple_b, TransformationType.PERSPECTIVE)
def get_train_neighbourhood_feature_and_label(self, c_D,
segment_out_ground,
threshold_in,
threshold_normals):
"""
Returns feature and label for neighbourhood classifier
"""
assert self.has_label(
), 'to train a neighborhood classifier you need to provide ground thruth labels'
neighbours_features = []
neighbours_labels = []
indices = np.array(range(len(self)))
# Segment out the ground
if segment_out_ground:
NB_RANDOM_DRAWS = 1000
best_ref_pt, best_normal = RANSAC(
self.features['geometric_center'], self.features['normal'],
NB_RANDOM_DRAWS, threshold_in, threshold_normals)
ground_mask = in_plane(self.features['geometric_center'],
self.features['normal'], best_ref_pt,
best_normal, threshold_in,
threshold_normals)
indices = indices[~ground_mask]
# Compute features and labels
potential = self.find_spatial_neighbours(indices, c_D)
for idx in range(len(indices)):
unique_mask = (potential[idx] > indices[idx])
cond_D_mask = self.get_cond_D_mask(indices[idx], potential[idx],
c_D)
mask = (unique_mask & cond_D_mask)
idx_features, idx_labels = self.compute_neighbourhood_feature_and_label(
indices[idx], potential[idx][mask])
if len(idx_features) > 0:
neighbours_features.append(idx_features)
neighbours_labels.append(idx_labels)
# Balance positive and negative samples
neighbours_features = np.vstack(neighbours_features)
neighbours_labels = np.hstack(neighbours_labels)
pos_mask = (neighbours_labels == 1)
pos_idxes = np.array(range(len(neighbours_features)))[pos_mask]
neg_idxes = np.array(range(len(neighbours_features)))[~pos_mask]
if len(pos_idxes) > len(neg_idxes):
pos_idxes = list(pos_idxes)
random.shuffle(pos_idxes)
neg_idxes = list(neg_idxes)
pos_idxes = pos_idxes[:len(neg_idxes)]
all_idxes = pos_idxes + neg_idxes
random.shuffle(all_idxes)
all_idxes = np.array(all_idxes)
neighbours_features = neighbours_features[all_idxes]
neighbours_labels = neighbours_labels[all_idxes]
return neighbours_features, neighbours_labels
output.append(scan)
return output
# Lines to handle python class
if __name__=="__main__":
output = readFromAWS('hallway_scan_3-8-2020')
scan = 'arc_test1_2'
curr_scan = readFromAWS(scan)
for data in curr_scan:
data['euler'] = kalman.Gravity([[float(data['Ax'])], [float(data['Ay'])], [float(data['Az'])]]).__repr__()
curr_coordinates = ransac.ConvertToCartesianEulerAngles(curr_scan)
actual_coordinates = []
for key, value in curr_coordinates.items():
hold = []
for coordinate in value:
hold.append(coordinate[0])
actual_coordinates.append(hold)
# Helper function to find distance between points
def distPoints(p0, p1):
return (np.sqrt((p0[0] - p1[0])**2 + (p0[1] - p1[1])**2 + (p0[2] - p1[2])**2))
# Actual RANSAC stuff begins below
SIFT_desc_2 = SIFT(corner_list_2, img_2_gray)
# match features
matches = SIFT_match(corner_list_1, corner_list_2, SIFT_desc_1, SIFT_desc_2)
# visualize matches
img_matched = display_matches(corner_list_1, corner_list_2, matches, img_1, img_2)
plt.axis('off')
plt.imshow(img_matched)
plt.show()
# compute fundamental matrix using RANSAC
num_samples = 8
num_iters = 2000
error_tolerance = 10
fundamental_matrix = RANSAC(matches, corner_list_1, corner_list_2, num_samples, num_iters, error_tolerance)
print fundamental_matrix
# draw 8 feature points and epipolar lines
SbS = np.concatenate((img_1, img_2), axis=1)
m,n = SbS.shape[:2]
shift_offset = n/2
# take 8 random feature points
all_inds = [i for i in range(len(matches))]
random.shuffle(all_inds)
inds = all_inds[:8]
# Draw feature points and epipolar lines on SbS image
plt.figure()
plt.imshow(SbS)
def compute_connected_components(self, c_D, segment_out_ground,
threshold_in, threshold_normals,
min_component_length):
"""
Builds a list of connected components of voxels from a voxelcloud object,
by performing a depth first search by using the neighbourhood condition
of voxels
The list of connected components is then returned
Each item of self.component is a list of indices, which are the indices of
the underlying VoxelCloud object
eg. components[i] = [1, 2, 3]
means that this component #i is made up of voxels 1, 2 and 3
"""
n_voxels = len(self)
voxel_neighbours = self.find_neighbours(list(range(n_voxels)), c_D)
# Initialize connected components
components = []
too_small_components = []
indices = np.array(list(range(n_voxels)))
indices_mask = np.ones(n_voxels, dtype=bool)
# Segment out the ground
if segment_out_ground:
NB_RANDOM_DRAWS = 1000
best_ref_pt, best_normal = RANSAC(
self.features['geometric_center'], self.features['normal'],
NB_RANDOM_DRAWS, threshold_in, threshold_normals)
ground_mask = in_plane(self.features['geometric_center'],
self.features['normal'], best_ref_pt,
best_normal, threshold_in,
threshold_normals)
ground_idxes = indices[ground_mask]
indices_mask[ground_idxes] = False
indices = np.array(list(range(n_voxels)))[indices_mask]
components.append(list(ground_idxes))
# Explore connected components
while len(indices) > 0:
stack = [indices[0]]
current_component = []
# Run a depth first search to find all connected voxels
while len(stack) > 0:
idx = stack.pop()
if ~indices_mask[idx]:
continue
current_component.append(idx)
indices_mask[idx] = False
next_idxs = voxel_neighbours[idx]
stack.extend(list(next_idxs[indices_mask[next_idxs]]))
if len(current_component) >= min_component_length:
components.append(current_component)
else:
too_small_components.append(current_component)
# Updating indices
indices = np.array(list(range(n_voxels)))[indices_mask]
return components, too_small_components
LSRP_list = []
dynamodb = boto3.resource(
'dynamodb',
region_name='us-east-2',
endpoint_url='http://dynamodb.us-east-2.amazonaws.com')
table = dynamodb.Table('SENTINEL')
scan_kalman = readFromAWS('hallway_scan_3-8-2020')
for scan in scan_kalman:
scan['euler'] = kalman.Gravity([[float(scan['Ax'])], [float(scan['Ay'])],
[float(scan['Az'])]]).__repr__()
test_coordinates = ransac.ConvertToCartesianEulerAngles(scan_kalman)
xs = []
ys = []
zs = []
for point in test_coordinates.values():
xs.append(point[0])
ys.append(point[1])
zs.append(point[2])
(Landmarks_New, LSRP_list,
Unassociated_Points) = ransac.RANSAC(test_coordinates, X, C, N, S, S_LIM)
print(LSRP_list)
print("Plotting Points and Landmarks...")
fig = plt.figure()
bf = cv.BFMatcher(cv.NORM_HAMMING, crossCheck=True)
# Match descriptors.
matches = bf.match(des1, des2)
# Sort matches and only use matches with fixed distance threshold
matches = sorted(matches, key=lambda x: x.distance)
# matches = filter(lambda x:x.distance < 30, matches)
points1 = np.zeros((len(matches), 2))
points2 = np.zeros((len(matches), 2))
for i, match in enumerate(matches):
points1[i] = kp1[match.queryIdx].pt
points2[i] = kp2[match.trainIdx].pt
#filter out outliers
points1, points2 = RANSAC(points1, points2, d=25)
so = str(overlay_num)
transform = None
while len(points1) > 20:
transform = cv.estimateRigidTransform(points1, points2, False)
if transform is not None:
ft = transform.flatten()
a, b, tx, c, d, ty = ft
sx = np.sign(a) * pow(a**2 + b**2, 0.5)
sy = np.sign(d) * pow(c**2 + d**2, 0.5)
#if the scale is ~1 then the transform is likely to be correct
if 0.9 < sx < 1.1 and 0.9 < sy < 1.1:
break
C = 500 #Consensus for RANSAC, number of points that must pass the tolerance check of the LSRP
N = 0 #Max number of trials in RANSAC before ending
S = 10 #Number of points to sample for RANSAC
S_LIM = 0.05 #unitless, half the length of a side of the cube to draw around the randomly sampled point in RANSAC
LSRP_list = []
##
##dynamodb = boto3.resource('dynamodb', region_name='us-east-2', endpoint_url='http://dynamodb.us-east-2.amazonaws.com')
##table = dynamodb.Table('SENTINEL')
scan_kalman = quick.readFromAWS('room_contbutnot0.5_1')
##for scan in scan_kalman:
## scan['euler'] = kalman.Gravity([[float(scan['Ax'])], [float(scan['Ay'])], [float(scan['Az'])]]).__repr__()
test_coordinates = ransac.ConvertToCartesianEulerAngles(scan_kalman)
(scaled_coordinates, factor) = ransac.NormalizeCartesian(test_coordinates)
(Landmarks_New, LSRP_list, Unassociated_Points,
Associated_Points) = ransac.RANSAC(scaled_coordinates, X, C, N, S, S_LIM)
print("RANSAC found " + str(len(LSRP_list)) + " LSRP's!")
print(LSRP_list)
## Assemble lists of the unassociated point coordinates for visualizations
xun = []
yun = []
zun = []
for point in Unassociated_Points.values():
xun.append(point[0])
yun.append(point[1])
zun.append(point[2])
del frame[-1]
frames.append(frame)
frame = [res[i]]
if i == len(res) - 1:
frames.append(frame)
print("The scan has been cleaned, now updating odometry...")
##for frame in frames:
## print("NEW FRAME, LENGTH = "+str(len(frame)))
## for scan in frame:
## print(scan['Motor encoder'])
for index in range(0, len(frames), 1):
frame = frames[index]
if index > 0: break
frame = frames[1]
(x, dx_sum) = EKF.UpdatePosition(x, dx_sum, dt1, dt2)
scan = RANSAC.ConvertToCartesian(frame, x)
lenscan = len(scan)
print(
"All " + str(lenscan) +
" points have been moved to a new dictionary, now running RANSAC on frame "
+ str(index))
#for plotting the points later
xs = []
ys = []
zs = []
for point in scan.values():
xs.append(point[0])
ys.append(point[1])
zs.append(point[2])
## xfs = []
## yfs = []