def create_mesh(path):
gsf_pings = utils.parse_or_load_gsf(path)
mbes_pings = gsf_data.convert_pings(gsf_pings)
V, F, bounds = mesh_map.mesh_from_pings(mbes_pings, 0.5)
return V, F, bounds
#!/usr/bin/python from auvlib.data_tools import std_data, gsf_data, utils from auvlib.bathy_maps import mesh_map import sys import numpy as np gsf_pings = utils.parse_or_load_gsf(sys.argv[1]) mbes_pings = gsf_data.convert_pings(gsf_pings) V, F, bounds = mesh_map.mesh_from_pings(mbes_pings, 0.5) height_map, bounds = mesh_map.height_map_from_pings(mbes_pings, 0.2) #mesh_map.show_height_map(height_map) R, G, B = mesh_map.height_map_to_texture(height_map) mesh_map.show_textured_mesh(V, F, R, G, B, bounds) #mesh_map.show_mesh(V, F)
beam_idx_list = np.where(diff > 0.35)[0]
if len(beam_idx_list) > 0:
# discard bad beams
choosen_pings[idx].beams = np.delete(ping.beams, beam_idx_list, axis = 0)
# newPing is used to plot data
# new_ping = copy.copy(beams)
# new_ping[beam_idx_list] = 0 # fill bad beam with empty value(0)
# else:
# new_ping = beams
''' Show beams before and after '''
# plt.figure(1)
# plt.clf()
# plt.subplot(121)
# plt.title('var: %f' % np.var(beams))
# plt.axis([0, 400, -95, -75])
# plt.grid()
# plt.scatter(np.arange(len(beams)), beams, s=2, c='b', marker='.')
# plt.subplot(122)
# plt.title(idx)
# plt.scatter(np.arange(len(beams)), new_ping, s=2, c='b', marker='.')
# plt.axis([0, 400, -95, -75])
# plt.grid()
# plt.pause(0.001)
std_data.write_data(choosen_pings, 'Data/EM2040/mid/pings_outlier_discard.cereal')
''' Show mesh (praying Orz...) '''
V_f, F_f, bounds_f = mesh_map.mesh_from_pings(choosen_pings, 0.5)
mesh_map.show_mesh(V_f, F_f)
#!/usr/bin/env python
from auvlib.data_tools import std_data
from auvlib.bathy_maps import mesh_map
import numpy as np
''' mix std pings from mid run and low run and make a mesh fro draping '''
low_pings = std_data.mbes_ping.read_data(
"Data/EM2040/low/pings_centered.cereal")
mid_pings = std_data.mbes_ping.read_data(
"Data/EM2040/mid/pings_centered.cereal")
choosen_pings = low_pings + mid_pings
V, F, bounds = mesh_map.mesh_from_pings(choosen_pings, 0.5)
mesh_map.show_mesh(V, F)
np.savez("Data/EM2040/mesh.npz", V=V, F=F, bounds=bounds)
print("???")
# ax = plt.subplot(projection='3d')
# ping1 = choosen_pings[13500]
# ping2 = choosen_pings[16000]
# x1 = np.array(ping1.beams)[:,0]
# y1 = np.array(ping1.beams)[:,1]
# z1 = np.array(ping1.beams)[:,2]
# x2 = np.array(ping2.beams)[:,0]
# y2 = np.array(ping2.beams)[:,1]
# z2 = np.array(ping2.beams)[:,2]
# ax.scatter(x1, y1, z1, c='r')
# ax.scatter(x2, y2, z2, c='b')
# ax.set_zlabel('Z')
# ax.set_ylabel('Y')
# ax.set_xlabel('X')
# plt.show()
return new_pings
# choosen_pings = std_pings[200:]
# for idx, ping in enumerate(choosen_pings):
# beams = np.array(ping.beams)[:,2]
# if len(beams) == 400:
# print_ping(beams,idx)
''' Run sound speed correction '''
new_pings = correct_overlapping(std_pings, 297) # 1277 for low, 2300 for mid
''' Save corrected pings as cereal file '''
std_data.write_data(std_pings, 'Data/EM2040/mid/pings_corrected.cereal')
''' Show mesh (praying Orz...) '''
V_f, F_f, bounds_f = mesh_map.mesh_from_pings(std_pings, 0.5)
mesh_map.show_mesh(V_f, F_f)
''' Find bound position '''
outputfile_mesh_map = "mesh_map" + ".npz"
#Path to saving location for mesh_map.
#If path is "" the file will be saved in the scripts location.
save_path_mesh = "../datasets/20190618_6/processed data/"
#Specify mesh_map resolution
resolution = .5
#Visualize the result?
visualization = True
#------------------------------------------------------------------------------
#LOAD MULTIBEAM DATA AND SAVE MESH_MAP FROM SELECTED REGION
#Load multibeam data
mbes_data = std_data.mbes_ping.read_data(mbes_file)
mbes_data = mbes_data[60000:70000]
#Create mesh_map. V = vertices, F = faces, bounds = bounds
V, F, bounds = mesh_map.mesh_from_pings(mbes_data, resolution)
#Save mesh_map
np.savez(save_path_mesh + outputfile_mesh_map, V=V, F=F, bounds=bounds)
#------------------------------------------------------------------------------
#VISUALIZE MESH_MAP (OPTIONAL)
if visualization:
mesh_map.show_mesh(V, F)
# for ping in std_pings:
# X.append(ping.pos_[0])
# Y.append(ping.pos_[1])
# Z.append(ping.pos_[2])
# range_l = 58000
# range_h = 71000
# ax.scatter(X[range_l:range_h], Y[range_l:range_h], Z[range_l:range_h])
# plt.show()
# # first run 47k~57k
# # second run 58k~71k(1st line 58K~59.7K, 2nd line 59.8k~61.3k, 3rd line 61.3k~62.9k, 4th line 63.2k~64.6k)
# Mesh map
V_f, F_f, bounds_f = mesh_map.mesh_from_pings(
std_pings[61500:64600],
0.5) # .5 is the resolution of the constructed mesh
mesh_map.show_mesh(V_f, F_f)
# 2 Sidescan data
# xtf_pings = xtf_data.xtf_sss_ping.read_data("xtf_pings_centered.cereal")
# xtf_ping0 = xtf_pings[0] # let's just look at the first ping!
# print "Total number of sidescan pings:", len(xtf_pings)
# print "Position of sensor:", xtf_ping0.pos_
# print "Number of port sidescan intensities:", len(xtf_ping0.port.pings)
# print "Number of starboard sidescan intensities:", len(xtf_ping0.stbd.pings)
# print "Time of data collection:", xtf_ping0.time_string_
# print "Intensity of first port hit (time 0s):", xtf_ping0.port.pings[0]
# print "Time of arrival of last port intensity (s):", xtf_ping0.port.time_duration
# print "Intensity of last port hit:", xtf_ping0.port.pings[-1]
yaws = []
while line:
a = line.split()
_timestamp = a[0:1][0]
_yaw = float(a[4:5][0])
_roll = float(a[5:6][0])
_pitch = float(a[6:7][0])
timestamps.append(_timestamp)
yaws.append(_yaw)
rolls.append(_roll)
pitchs.append(_pitch)
line = f.readline()
head = nav(timestamps, yaws, rolls, pitchs)
f.close()
# second run 58k~71k(1st line 58K~59.7K, 2nd line 59.8k~61.3k, 3rd line 61.5k~62.9k, 4th line 63.2k~64.6k)
one_line = []+std_pings[58000:59500]
two_line = []+std_pings[59900:61150]
thr_line = []+std_pings[62000:62800]#[]+std_pings[61550:62800]
fou_line = []+std_pings[63250:64500]
one_line_new = calibrate(head, one_line, 0.06)
two_line_new = calibrate(head, two_line, 0.06)
thr_line_new = calibrate(head, thr_line, -0.03)
new_pings = one_line_new + two_line_new + thr_line_new# + fou_line
# Show mech (praying)
V_f, F_f, bounds_f = mesh_map.mesh_from_pings(new_pings, 0.5) # .5 is the resolution of the constructed mesh
mesh_map.show_mesh(V_f, F_f)
params = []
for beam in beams:
params.append(np.polyfit(np.arange(np.size(beam)), beam, 2))
predicted_beams = []
polynomials = []
for param_set in params:
polynomials.append(np.poly1d(param_set))
for i in range(len(size_list)):
print(i)
polynomial = polynomials[i]
size = size_list[i]
beam = beams[i]
difference = np.abs(polynomial(np.arange(size)) - beam)
idx_list = np.where(difference > 0.3)[0]
if len(idx_list) != 0:
new_beam = []
for idx, elem in enumerate(std_pings[i].beams):
if idx not in idx_list:
new_beam.append(elem)
std_pings[i].beams = new_beam
V, F, bounds = mesh_map.mesh_from_pings(std_pings, 0.5)
mesh_map.show_mesh(V, F)
np.savez("beautiful_mesh.npz", V=V, F=F, bounds=bounds)
# convert .all file to std data
std_pings = all_data.convert_matched_entries(all_ping, nav)
choosen_pings = std_pings
''' check the incontinous in one ping '''
new_pings = []
beams_buff = np.array(choosen_pings[0].beams)[:, 2]
for idx, ping in enumerate(choosen_pings):
beams = np.array(ping.beams)[:, 2]
beams_left = copy.copy(beams)
beams_left[0:-1] = beams[1:] # move beams one index to left direction
delta = abs(beams_left - beams)
# check the inconsistincy in one ping
beam_idx_list = np.where(delta > 1)[0]
if len(beam_idx_list) > 0: # "make up" new ping from previous ping
vechile_move = ping.pos_ - new_pings[-1].pos_
new_beams = np.array(new_pings[-1].beams) + vechile_move
ping.beams = new_beams
new_pings.append(ping)
else:
new_pings.append(ping)
beams_buff = beams
std_data.write_data(new_pings,
'/home/chs/Desktop/KTH1/pings_outlier_fixed.cereal')
''' Show mesh (praying Orz...) '''
V_f, F_f, bounds_f = mesh_map.mesh_from_pings(new_pings, 2.0)
mesh_map.show_mesh(V_f, F_f)
direction = np.dot(auv_vec, np.cross(beam_vec, normal_vec)) # decide +-
phi = phi * direction / abs(direction)
# cal index in 4D
deltaz_index = round(beam_vec[2] / deltaz_reso) * deltaz_reso
dist_index = round(beam_dist / dist_reso) * dist_reso
beam_angle_index = round(angle / beamangle_reso) * beamangle_reso
incident_index = round(phi / incident_reso) * incident_reso
''' Import multibeam data '''
std_file = "/home/chs/Desktop/Sonar/Data/EM2040/mid/pings_centered.cereal"
std_pings = std_data.mbes_ping.read_data(std_file)
''' Get mesh and norm '''
mesh_reso = 0.5 # get mesh from std pings
V, F, bounds = mesh_map.mesh_from_pings(std_pings, mesh_reso)
N = mesh_map.compute_normals(
V, F
) # compute normals for the entire mesh(unit vector of reflection surface)
''' Make 4D matrix '''
# Set para bounds and resolution
deltaz_reso = 1
dist_reso = 1
beamangle_reso = 0.01
incident_reso = 0.01
meas_list = [1] #, 2, 3, 4, 5, 6, 7, 8] # measure image index list
# Init multiprocess list and pool
deltaz_ls = Manager().list([])