def clip_pcd_by_distance_plane(pcd, vec1, vec2, pt1, threshold):
plane = Plane3D.create_plane_from_vectors_and_point(vec1, vec2, pt1)
distance = plane.distance_to_plane(pcd.data.T)
data_close = pcd.data[:,distance<threshold]
data_far = pcd.data[:,distance>=threshold]
pcd_close = PointCloud(data_close)
pcd_far = PointCloud(data_far)
return pcd_close, pcd_far
def filter(self, point_cloud):
result = PointCloud(point_cloud.lidar_origin, point_cloud.lidar_orientation)
for point in point_cloud.map_frame_list:
element = self.index_to_key_(self.get_cell_index_(point))
if not(element in self.voxel_set_):
self.voxel_set_.add(element)
result.add_point([point])
return result
def apply(self, point_cloud):
result = PointCloud(point_cloud.lidar_origin,
point_cloud.lidar_orientation)
copy_list = copy.deepcopy(point_cloud.map_frame_list)
shuffle(copy_list)
list_of_points = copy_list[0:self.wished_size]
for point in list_of_points:
result.add_point([point])
return result
def apply(self, point_cloud):
bin_size = 2 * np.pi / self.__number_of_bins
histogram = [list() for _ in range(self.__number_of_bins)]
result = PointCloud(point_cloud.lidar_origin,
point_cloud.lidar_orientation)
list_of_points = point_cloud.map_frame_list
# generate histogram
for index in range(len(list_of_points)):
angle = point_cloud.get_descriptor("normals_direction", index)
bin_index = int(np.floor(angle / bin_size))
histogram[bin_index].append(index)
# remove points of the largest remaining bin to maximize entropy of resulting normal angle distribution
for _ in range(point_cloud.count - self.wished_size):
lengths = list(
map(lambda histogram_bin: len(histogram_bin), histogram))
index = lengths.index(max(lengths))
# remove random point from bin
point_index = random.randint(0, len(histogram[index]) - 1)
histogram[index].pop(point_index)
# create result point cloud
for histogram_bin in histogram:
for index in histogram_bin:
result.add_point([list_of_points[index]])
result.add_descriptor(
"normals", point_cloud.get_descriptor("normals", index))
result.add_descriptor(
"normals_direction",
point_cloud.get_descriptor("normals_direction", index))
return result
def get_point_cloud(self, pair):
"""Get 3D point cloud from image pair."""
disparity = self.block_matcher.compute_disparity(pair)
points = self.block_matcher.compute_3d(
disparity, self.calibration.disp_to_depth_mat)
colors = cv2.cvtColor(pair[0], cv2.COLOR_BGR2RGB)
return PointCloud(points, colors)
def images_from_cloud(self, src_path, dst_path, testing=False):
src_suffix = ".h5"
dst_suffix = ".jpg"
files = [
f for f in glob.glob(src_path + "*" + src_suffix, recursive=True)
]
if not os.path.exists(dst_path):
os.mkdir(dst_path)
for f in tqdm(files):
sample_name = self.extract_name(f, src_path, src_suffix)
sub_directory = dst_path + "/" + sample_name
if not os.path.exists(
sub_directory): # Create a directory for each video file
os.mkdir(sub_directory)
hf = h5py.File(f, 'r')
cloud = hf['cloud2'][:]
hf.close()
frames = PointCloud().pc2voxel_reshaped_depth(cloud, depth=20)
for i in range(frames.shape[0]):
frame = frames[i, :, :]
output_filename = sub_directory + "/" + str(i) + dst_suffix
cv2.imwrite(output_filename, frame)
if testing == True:
break
def read_points_data(filename):
planes = []
point_clouds = []
with open(filename, "r") as fp:
lines = fp.readlines()
if lines[0][0] == 'T':
for line in lines[1:4]:
plane_param = list(map(float, line.split(",")))
planes.append(Plane3D.create_plane_from_list(plane_param))
last_num = 0
start_line = 6
end_line = start_line
for line in lines[6::]:
if line[0] != '\n' and end_line != len(lines) - 1 and int(
line[0]) == last_num:
end_line += 1
else:
end_line += 1
data = np.zeros((3, end_line - start_line))
if line[0] == '\n':
break
for idx, line in enumerate(lines[start_line:end_line]):
points = list(map(float, line.split(',')))
data[:, idx] = points[1::]
point_clouds.append(PointCloud(data))
start_line = end_line
last_num = int(line[0])
return planes, point_clouds
def main():
parser = OptionParser()
parser.add_option("-t", "--track", dest="track_file", help="GPS track file name", metavar="TRACK_FILE", type="string")
parser.add_option("-o", "--output", dest="output_filename", help="Output point cloud file name, e.g., output_point_cloud.dat", type="string")
parser.add_option("--test_case", dest="test_case", type="int", help="Test cases: 0: region-0; 1: region-1; 2: SF-region.", default=0)
(options, args) = parser.parse_args()
if not options.track_file:
parser.error("Track file not given.")
if not options.output_filename:
parser.error("Output pointcloud filename not given.")
R = const.R
if options.test_case == 0:
LOC = const.Region_0_LOC
elif options.test_case == 1:
LOC = const.Region_1_LOC
elif options.test_case == 2:
LOC = const.SF_LOC
else:
parser.error("Test case indexed %d not supported!"%options.test_case)
tracks = gps_track.load_tracks(options.track_file)
point_cloud = PointCloud(np.array([]), np.array([]))
point_cloud.extract_point_cloud(tracks, LOC, R)
point_cloud.visualize_point_cloud(LOC, R)
point_cloud.save(options.output_filename)
def populate(self):
"""
Function that creates the dataset samples.
:return:
"""
s = time()
renderer = Renderer(mode=0)
lightmanager = LightManager()
cameramanager = CameraManager()
for i in range(self.size):
lightmanager.make()
building = self.factory.produce()
building.make()
if use_materials:
_monomaterial = np.random.random() < MATERIAL_PROB
mat = self.material_factory.produce()
for v in building.volumes:
if not _monomaterial:
mat = self.material_factory.produce()
v.apply(mat)
v.add_modules()
self.json.add(building, '{}.png'.format(i), '{}.obj'.format(i))
cameramanager.make_main()
renderer.render(filename='building_{}'.format(i))
if RENDER_VIEWS > 1:
for view in range(1, RENDER_VIEWS):
cameramanager.make()
lightmanager.make()
if RANDOMIZE_TEXTURES:
if use_materials:
_monomaterial = np.random.random() < MATERIAL_PROB
mat = self.material_factory.produce()
for v in building.volumes:
if not _monomaterial:
mat = self.material_factory.produce()
v.apply(mat)
renderer.render(filename='building_{}_{}'.format(i, view))
building.save(i)
building.save(i, ext='ply')
building.demolish()
cloud = PointCloud()
cloud.make(i)
print('Whole process took: {}'.format(time() - s))
def plot_image(pc: point_cloud.PointCloud, img: np.ndarray, depth_map: np.ndarray, scale=0.2, step=1):
"""
Renders pixels in 3D with depth as Z
:param pc: PointCloud object
:param img: colorized image to plot (shape (w, h, 3))
:param depth_map: depth map (shape (w, h))
:param scale: distance between points
:param step: step size of loop through pixels
:return: void
"""
for y in range(0, img.shape[1], step):
for x in range(0, img.shape[0], step):
if x >= img.shape[1] or y >= img.shape[0]:
break
pc.add_point((x * scale, y * scale, depth_map[x, y]), tuple(img[x, y]))
def test2(self):
filename = "../../dataset/micro/100109.mp4"
extractor = VideoProcessor(filename)
image_collection = extractor.process_video(roi_extraction=True, filter_enabled=True, average_frames=True)
thresh = int(np.percentile(image_collection.ravel(), 95))
cloud, labels =ImageProcessor3D().point_cloud_from_collecton(image_collection, threshold=thresh,
filter_outliers=True)
for i in range(9):
if i == 0:
projection = PointCloud().cloud_projection(cloud=cloud)
else:
cloud_rotated = PointCloud().rotate(cloud=cloud, pos=i)
projection = PointCloud().cloud_projection(cloud=cloud_rotated)
plt.subplot(3, 3, i+1)
plt.imshow(projection, cmap='gray')
plt.show()
def collectFeats(PC_set, n_3dpoints, ind_frame, fea_pos, fea_frame, Hw2c,cprams):
'''collect features in one frame'''
for label in range(n_3dpoints):
if ind_frame in fea_frame[label]:
ind = fea_frame[label].index(ind_frame)
if label in PC_set:
PC_cur = PC_set[label]
PC_cur.newCorres(fea_pos[label][ind], ind_frame, Hw2c,cprams)
else:
PC_set[label] = PointCloud(label, np.zeros(3), fea_pos[label][ind], ind_frame, Hw2c,cprams)
return PC_set
def projections_from_cloud(self, src_path, dst_path, testing=False):
src_suffix = ".h5"
dst_suffix = ".jpg"
files = [
f for f in glob.glob(src_path + "*" + src_suffix, recursive=True)
]
if not os.path.exists(dst_path):
os.mkdir(dst_path)
for f in tqdm(files):
sample_name = self.extract_name(f, src_path, src_suffix)
sub_directory = dst_path + "/" + sample_name
if not os.path.exists(
sub_directory): # Create a directory for each video file
os.mkdir(sub_directory)
hf = h5py.File(f, 'r')
cloud = hf['cloud2'][:]
hf.close()
for i in range(9):
if i == 0:
projection = PointCloud().cloud_projection(cloud=cloud)
else:
cloud_rotated = PointCloud().rotate(cloud=cloud, pos=i)
projection = PointCloud().cloud_projection(
cloud=cloud_rotated)
# projection = cv2.resize(projection, (224, 224), interpolation=cv2.INTER_AREA)
output_filename = sub_directory + "/" + str(i) + dst_suffix
cv2.imwrite(output_filename, projection)
if testing == True:
break
def populate(self):
for i in range(self.size):
building = self.factory.produce()
building.make()
if use_materials:
_monomaterial = np.random.random() < MATERIAL_PROB
mat = self.material_factory.produce()
print(mat.name)
for v in building.volumes:
if not _monomaterial:
mat = self.material_factory.produce()
v.apply(mat)
for module_name in MODULES:
for side in range(2):
mod = GridApplier(
ModuleFactory().mapping[module_name])
module = ModuleFactory().produce(module_name)
module.connect(v, side)
step = (np.random.randint(ceil(module.scale[0]),
6),
np.random.randint(ceil(module.scale[0]),
6))
mod.apply(module,
step=step,
offset=(2.0, 2.0, 2.0, 1.0))
self.json.add(building, '{}.png'.format(i), '{}.obj'.format(i))
# building.save(filename=str(i))
renderer = Renderer(mode=0)
renderer.render(filename='building_{}'.format(i))
building.save(i)
building.save(i, ext='ply')
building.demolish()
cloud = PointCloud()
cloud.make(i)
def main():
parser = OptionParser()
parser.add_option("-t",
"--track",
dest="track_file",
help="GPS track file name",
metavar="TRACK_FILE",
type="string")
parser.add_option(
"-o",
"--output",
dest="output_filename",
help="Output point cloud file name, e.g., output_point_cloud.dat",
type="string")
parser.add_option(
"--test_case",
dest="test_case",
type="int",
help="Test cases: 0: region-0; 1: region-1; 2: SF-region.",
default=0)
(options, args) = parser.parse_args()
if not options.track_file:
parser.error("Track file not given.")
if not options.output_filename:
parser.error("Output pointcloud filename not given.")
R = const.R
if options.test_case == 0:
LOC = const.Region_0_LOC
elif options.test_case == 1:
LOC = const.Region_1_LOC
elif options.test_case == 2:
LOC = const.SF_LOC
else:
parser.error("Test case indexed %d not supported!" % options.test_case)
tracks = gps_track.load_tracks(options.track_file)
point_cloud = PointCloud(np.array([]), np.array([]))
point_cloud.extract_point_cloud(tracks, LOC, R)
point_cloud.visualize_point_cloud(LOC, R)
point_cloud.save(options.output_filename)
def scan(self, environment, origin):
point_cloud = PointCloud(origin, 0)
for ray_angle in np.arange(self.min_angle, self.max_angle,
self.angular_resultion):
ray_angle_noise = np.random.normal(0, self.angular_variance, 1)
ray_angle += ray_angle_noise
dx = np.sin(ray_angle)
dy = np.cos(ray_angle)
ray = (origin[0] + self.max_range * dx,
origin[1] + self.max_range * dy)
ray_line = LineString([origin, ray])
# x, y = ray_line.xy
# ax.plot(x, y, color='black', linewidth=1)
intersect = ray_line.intersection(environment)
if not intersect.is_empty:
if (isinstance(intersect, shapely.geometry.point.Point)):
np_ray = np.array([
intersect.coords[0][0] - origin[0],
intersect.coords[0][1] - origin[1]
])
ray_length = np.linalg.norm(np_ray)
range_noise = np.random.normal(0, self.range_variance, 1)
point_cloud.add_point([
origin + np_ray *
((ray_length + range_noise) / ray_length)
])
elif (isinstance(intersect,
shapely.geometry.multipoint.MultiPoint)):
# get observation closest to the sensor
# print(intersect)
nearest_observation_distance = np.inf
nearest_observation = None
for p in intersect:
candidate_distance = np.linalg.norm(
np.array(origin) - np.array(p))
if candidate_distance < nearest_observation_distance:
nearest_observation = p
nearest_observation_distance = candidate_distance
np_ray = np.array([
nearest_observation.x - origin[0],
nearest_observation.y - origin[1]
])
ray_length = np.linalg.norm(np_ray)
range_noise = np.random.normal(0, self.range_variance, 1)
point_cloud.add_point([
origin + np_ray *
((ray_length + range_noise) / ray_length)
])
else:
print('Unhandled intersection type', type(intersect))
return point_cloud
def main():
"""
Convert TXT to PLY.
"""
parser = argparse.ArgumentParser(description='Convert TXT to PLY.')
parser.add_argument('input', type=str, help='TXT file.')
parser.add_argument('output', type=str, help='PLY file.')
args = parser.parse_args()
if not os.path.exists(args.input):
print('Input file does not exist.')
exit(1)
point_cloud = PointCloud.from_txt(args.input)
print('Read %s.' % args.input)
point_cloud.to_ply(args.output)
print('Wrote %s.' % args.output)
def read_points_raw(filename):
data = np.loadtxt(filename, delimiter=',')
pcd = PointCloud(data[:, 1:4].T)
return pcd
import numpy as np
import argparse
from point_cloud import PointCloud
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Write an example point cloud as TXT.')
parser.add_argument('output', type=str, help='TXT file for example point cloud.')
args = parser.parse_args()
point_cloud = PointCloud(np.random.random((100, 3)))
point_cloud.to_txt(args.output)
print('Wrote %s.' % args.output)
# The Euclidean distance matrix. Unit is squared meters
EDM = np.array(
[ [ 0, 0.79, 1.63, 2.42, 2.82, 3.55, 2.44, 2.87, 2.22, 1.46 ],
[ 0.79, 0, 1.45, 2.32, 2.49, 3.67, 2.32, 2.54, 1.92, 1.35 ],
[ 1.63, 1.45, 0, 1.92, 2.09, 4.02, 3.48, 3.66, 2.50, 1.68 ],
[ 2.42, 2.32, 1.92, 0, 0.86, 2.43, 2.92, 3.14, 1.56, 1.14 ],
[ 2.82, 2.49, 2.09, 0.86, 0, 3.15, 3.10, 3.07, 1.58, 1.56 ],
[ 3.55, 3.76, 4.02, 2.43, 3.15, 0, 2.44, 2.88, 2.11, 2.45 ],
[ 2.44, 2.32, 3.48, 2.92, 3.10, 2.44, 0, 0.85, 1.52, 2.00 ],
[ 2.87, 2.54, 3.66, 3.14, 3.07, 2.88, 0.85, 0, 1.60, 2.31 ],
[ 2.22, 1.92, 2.50, 1.56, 1.58, 2.11, 1.52, 1.60, 0, 0.97 ],
[ 1.46, 1.35, 1.68, 1.14, 1.56, 2.45, 2.00, 2.31, 0.97, 0 ] ]
)**2
# Create the marker objects
markers = PointCloud(EDM=EDM, labels=labels)
# We know that these markers should be roughly on a plane
markers.flatten(['7','5','3','4'])
# Let the FPGA ref point be the center
markers.center('FPGA')
# And align x-axis onto speaker 7
markers.align('7','z')
# Now there a few correction vectors to apply between the measurement points
# and the center of the baffles
corr_twitter = {
'7' : np.array([+0.01, 0, -0.05]),
'3' : np.array([0., -0.01, -0.05]),
def pcd_callback(self, msg):
rospy.logwarn("Getting pcd at: %d.%09ds, (%d,%d)",
msg.header.stamp.secs, msg.header.stamp.nsecs,
msg.height, msg.width)
pcd_original = pointcloud2_to_xyz_array(msg)
pcd = PointCloud(pcd_original.T)
self.plane, pcd_inlier, pcd_outlier = self.estimate_plane(pcd)
transform_matrix, trans, rot, euler = get_transformation(
frame_from='/velodyne',
frame_to='/world',
time_from=msg.header.stamp,
time_to=msg.header.stamp,
static_frame='/world',
tf_listener=self.tf_listener,
tf_ros=self.tf_ros)
if not transform_matrix is None:
plane_world_param = np.matmul(
np.linalg.inv(transform_matrix).T,
np.array(
[[self.plane.a, self.plane.b, self.plane.c,
self.plane.d]]).T)
plane_world_param = plane_world_param / np.linalg.norm(
plane_world_param[0:3])
if self.plane_tracker is None:
self.plane_tracker = Tracker(msg.header.stamp,
plane_world_param)
else:
self.plane_tracker.predict(msg.header.stamp)
self.plane_tracker.update(plane_world_param)
print("plane_world:", plane_world_param.T)
print("plane_traker:", self.plane_tracker.filter.x_post.T)
# self.plane_world = Plane3D(plane_world_param[0,0], plane_world_param[1,0], plane_world_param[2,0], plane_world_param[3,0])
self.plane_world = Plane3D(self.plane_tracker.filter.x_post[0, 0],
self.plane_tracker.filter.x_post[1, 0],
self.plane_tracker.filter.x_post[2, 0],
self.plane_tracker.filter.x_post[3, 0])
center_pos = np.matmul(
transform_matrix,
np.array([[
10, 0, (-self.plane.a * 10 - self.plane.d) / self.plane.c,
1
]]).T)
center_pos = center_pos[0:3].flatten()
# normal = np.matmul( transform_matrix, np.array([[0., 0., 1., 0.]]).T)
# normal = normal[0:3]
normal = None
marker_array = self.create_and_publish_plane_markers(
self.plane_world,
frame_id='world',
center=center_pos,
normal=normal)
self.pub_plane_markers.publish(marker_array)
plane_msg = Plane()
plane_msg.coef[0], plane_msg.coef[1], plane_msg.coef[
2], plane_msg.coef[
3] = self.plane.a, self.plane.b, self.plane.c, self.plane.d
self.pub_plane.publish(plane_msg)
# pcd_msg_inlier = create_point_cloud(pcd_inlier.T, frame_id='velodyne')
# pcd_msg_outlier = create_point_cloud(pcd_outlier.T, frame_id='velodyne')
pcd_msg_inlier = xyz_array_to_pointcloud2(pcd_inlier.T,
stamp=msg.header.stamp,
frame_id='velodyne')
pcd_msg_outlier = xyz_array_to_pointcloud2(pcd_outlier.T,
stamp=msg.header.stamp,
frame_id='velodyne')
self.pub_pcd_inlier.publish(pcd_msg_inlier)
self.pub_pcd_outlier.publish(pcd_msg_outlier)
rospy.logwarn("Finished plane estimation")
for j, v in enumerate(collection.collection):
mod = GridApplier(Window)
w = Window()
w.connect(v, 1)
step = (np.random.randint(1, 6), np.random.randint(1, 6))
if j == 0:
mod.apply(w, step=step, offset=(2.0, 2.0, 2.0, 1.0))
else:
mod.apply(w, step=step)
w = Window()
w.connect(v, 0, 0)
step = (np.random.randint(1, 6), np.random.randint(1, 6))
if j == 0:
mod.apply(w, step=step, offset=(2.0, 2.0, 2.0, 1.0))
else:
mod.apply(w, step=step)
renderer = Renderer(mode=0)
renderer.render(filename='building_{}'.format(image))
building.save(image)
building.save(image, ext='ply')
building.demolish()
cloud = PointCloud()
cloud.make(image)
# cloud = PyntCloud.from_file("Models/{}.obj".format(image))
# cloud.to_file("{}.ply".format(image))
# cloud.to_file("{}.npz".format(image))
def filter_point_cloud_using_grid(point_cloud, grid_size, loc, R, threshold=1):
""" Filter GPS point_cloud using a grid.
Args:
- point_cloud: GPS point cloud
- grid_size: grid size in meters
- loc: center of the region
- R: radius of the region
Return:
- sample_point_cloud: an object of PointCloud.
"""
min_easting = loc[0] - R
max_easting = loc[0] + R
min_northing = loc[1] - R
max_northing = loc[1] + R
n_grid_x = int((max_easting - min_easting) / grid_size + 0.5)
n_grid_y = int((max_northing - min_northing) / grid_size + 0.5)
if n_grid_x > 1E4 or n_grid_y > 1E4:
print "ERROR in filter_point_cloud_using_grid! The sampling grid is too small!"
sys.exit(1)
geo_hash = {}
dir_hash = {}
geo_hash_count = {}
geo_hash_direction = {}
# Traversing through all GPS points
for pt_idx in range(0, len(point_cloud.locations)):
pt = point_cloud.locations[pt_idx]
pt_dir = point_cloud.directions[pt_idx]
pt_dir_norm = np.linalg.norm(pt_dir)
px = int((pt[0] - min_easting) / grid_size)
py = int((pt[1] - min_northing) / grid_size)
if px < 0 or px > n_grid_x or py < 0 or py > n_grid_y:
print "ERROR! Point outside the grid!"
sys.exit(1)
if geo_hash.has_key((px, py)):
geo_hash_count[(px, py)] += 1
geo_hash[(px, py)] += pt
if pt_dir_norm > 0.1:
geo_hash_direction[(px, py)].append(np.copy(pt_dir))
else:
geo_hash_count[(px, py)] = 1.0
geo_hash[(px, py)] = np.copy(pt)
if pt_dir_norm > 0.1:
geo_hash_direction[(px, py)] = [pt_dir]
else:
geo_hash_direction[(px, py)] = []
# Deal with each non-empty box
sample_point_locations = []
sample_point_directions = []
for key in geo_hash.keys():
if geo_hash_count[key] < threshold:
continue
pt = geo_hash[key] / geo_hash_count[key]
directions = geo_hash_direction[key]
if len(directions) == 0:
sample_point_locations.append(pt)
sample_point_directions.append(np.array([0.0, 0.0]))
continue
# Clustering the directions
n_angle_bin = 16
delta_angle = 2 * np.pi / n_angle_bin
angle_directions = []
tmp_directions = list(directions)
while True:
angle_bin_count = np.zeros(n_angle_bin)
for direction in tmp_directions:
angle = np.arccos(np.dot(direction, np.array([1.0, 0.0])))
if direction[1] < 0:
angle = 2 * np.pi - angle
angle_bin_idx = int(angle / delta_angle)
angle_bin_count[angle_bin_idx] += 1
# Find max
max_idx = np.argmax(angle_bin_count)
if angle_bin_count[max_idx] < 1:
break
max_angle_dir = (max_idx + 0.5) * delta_angle
max_angle_vec = np.array(
[np.cos(max_angle_dir),
np.sin(max_angle_dir)])
new_directions = []
result_direction = np.array([0.0, 0.0])
for direction in tmp_directions:
if np.dot(direction, max_angle_vec) > np.cos(np.pi / 12.0):
result_direction += direction
else:
new_directions.append(np.copy(direction))
tmp_directions = new_directions
result_direction /= np.linalg.norm(result_direction)
sample_point_locations.append(np.copy(pt))
sample_point_directions.append(np.copy(result_direction))
sample_point_cloud = PointCloud(np.array(sample_point_locations),
np.array(sample_point_directions))
return sample_point_cloud
print('[Data] read ' +
common.filename(config, 'part_space_file', '_f.h5', dataset))
#statistics = statistics.reshape(1, 1, statistics.shape[0], statistics.shape[1], statistics.shape[2])
#statistics = np.repeat(statistics, space.shape[0], axis=0)
#print(space.shape, statistics.shape)
#invalid_space = space*statistics
points = []
point_dir = common.filename(config, 'bounding_box_txt_directory', '',
dataset) + '/'
for file in os.listdir(point_dir):
point_file = point_dir + file
point_cloud = PointCloud.from_txt(point_file)
#print('[Data] read ' + point_file)
points.append(point_cloud.points.shape[0])
frames = [1] * inputs.shape[0]
frame_dir = common.filename(config, 'velodyne_individual_gt_txt_directory',
'', dataset) + '/'
for i in range(inputs.shape[0]):
for k in range(-config['gt_range'], config['gt_range'] + 1,
config['gt_skip']):
if k == 0:
continue
txt_file = frame_dir + '%d_%d_%d.txt' % (i, k, frames[i])
def initial_PC():
# import calibration data for rgb camera
rgb_Calib = pkl.load(open("data/RGBcamera_Calib_result.pkl","rb"))
focal_length = rgb_Calib['fc'] # focal length
dist_k = rgb_Calib['kc'] # distortion factor
princip_center = rgb_Calib['cc'] # principle points
# import feature result
feature_pos = np.load('data/feature_pos.npy')
feature_frame = np.load('data/feature_frame.npy')
# import prediction result
pred_pos_rob = np.load('data/pos_pred.npy')
pred_q_rob = np.load('data/q_pred.npy')
pred_T = np.load('data/T_pred.npy')
# import time line of lidar and camera
time_camera = np.load('data/time_camera.npy')
time_lidar = np.load('data/time_lidar.npy')
# number of detected physical points
n_points_3d = len(feature_pos)
# number of camera
n_camera = time_camera.shape[0]
# focal length and distortion factor
f, k1, k2 = np.sum(focal_length) / 2, dist_k[0], dist_k[1]
pu, pv = princip_center[0], princip_center[1]
# match time step between camera and lidar
lidar_ind = []
for i in range(time_camera.shape[0]):
lidar_ind.append(np.argmin(np.abs(time_camera[i] - time_lidar)))
# initialize Point cloud
PC_set = {}
# ind_frame is the current camera frame
for ind_frame in range(time_camera.shape[0]):
# extract corresponding lidar frame based on camera frame
ind_lidar = lidar_ind[ind_frame]
# current transformation of camera
cur_T = pred_T[4 * ind_lidar: 4 * ind_lidar + 4, :]
# construct camera parameters given by transformation
cur_camp = get_cparams(cur_T, f, k1, k2)
# i is the label of physical 3D point
for i in range(len(feature_frame)):
# if current physical point has less than 2 correspondences, filter it out.
if len(feature_frame[i]) < 2:
continue
# check whether this physical point is detected in current camera frame
if ind_frame in feature_frame[i]:
# if so, extract its 2d information as well as store camera frame
ind = feature_frame[i].index(ind_frame)
cur_2d = feature_pos[i][ind]
# if this physical point has been already added into PC set
# update exist object information
if i in PC_set:
PC_set[i].newCorres(p2d=cur_2d, cam_ind=feature_frame[i][ind], Hwc2=cur_T, cprams=cur_camp)
# if meet new physical point, create a new Point cloud structure and add into set
else:
PC_set[i] = PointCloud(label=i, ini_3D=None, ini_2D=cur_2d, ini_frame=feature_frame[i][ind], ini_Hw2c=cur_T, cprams=cur_camp)
return PC_set, pu, pv
def test_clean_PC_set():
PC_set = {}
Hw2c = np.zeros((4,4))
cprams = np.zeros((1,9))
PC1 = PointCloud(label=0, ini_3D=None, ini_2D=[0,0], ini_frame=1, ini_Hw2c=Hw2c,cprams=cprams,inliners=None)
PC2 = PointCloud(label=1, ini_3D=[0,0,0], ini_2D=[10,10], ini_frame=0, ini_Hw2c=Hw2c, cprams=cprams, inliners=1)
PC3 = PointCloud(label=2, ini_3D=[10, 10, 10], ini_2D=[20, 20], ini_frame=0, ini_Hw2c=Hw2c, cprams=cprams, inliners=1)
PC4 = PointCloud(label=3, ini_3D=[20, 20, 20], ini_2D=[30, 30], ini_frame=1, ini_Hw2c=Hw2c + 1, cprams=cprams + 0.1, inliners=1)
PC5 = PointCloud(label=4, ini_3D=[30, 30, 30], ini_2D=[40, 40], ini_frame=1, ini_Hw2c=Hw2c + 1, cprams=cprams + 0.1, inliners=0)
PC6 = PointCloud(label=5, ini_3D=None, ini_2D=None, ini_frame=None, ini_Hw2c=Hw2c, cprams=cprams, inliners=None)
PC7 = PointCloud(label=6, ini_3D=None, ini_2D=None, ini_frame=None, ini_Hw2c=Hw2c, cprams=cprams, inliners=None)
# add new correspondences
PC2.newCorres(p2d=[100,100], cam_ind=1, Hwc2=Hw2c + 1, cprams=cprams + 0.1, inliners=0)
# add new correspondences
PC2.newCorres(p2d=[1000, 1000], cam_ind=2, Hwc2=Hw2c + 2, cprams=cprams + 0.2, inliners=1)
PC_set[1] = PC1
PC_set[2] = PC2
PC_set[3] = PC3
PC_set[4] = PC4
PC_set[5] = PC5
PC_set[6] = PC6
PC_set[7] = PC7
# do cleaning
PC_set_after_cleaning = clean_PC_set(PC_set)
points_3d, points_2d, points_indices, camera_indices, camera_params = convertPC(PC_set_after_cleaning)
print('------- final result: -------------')
print(points_2d)
print('------------------------------------')
print(camera_indices)
print('------------------------------------')
print(points_indices)
print('------------------------------------')
def test_linearT(Hw2c_set, points_2d, points_3d, points_indices,
camera_indices, camera_params, gt_points_3d):
n_points = points_3d.shape[0]
n_cameras = camera_params.shape[0]
# Obtain first frame
print('\n---------------Testing linearT()------------------\n')
# collect point clouds
pkl_folder = 'pkl'
if not os.path.exists(pkl_folder):
os.makedirs(pkl_folder)
if not os.path.exists(os.path.join(pkl_folder, 'PC_set.pkl')):
PC_set = {}
gt_3D_set = {}
for p in range(points_3d.shape[0]):
p3d = points_3d[p, :]
gt_p3d = gt_points_3d[p]
gt_3D_set[p] = gt_p3d
print('- Point {0}/ {1}'.format(p, points_3d.shape[0]))
label = points_indices[p]
PC = PointCloud(label=label, ini_3D=p3d)
# Find 2d (u,v)
add2 = 0
for i in range(points_indices.shape[0]):
if points_indices[i] == label:
PC.newCorres(points_2d[i], camera_indices[i], Hw2c_set[i],
camera_params[camera_indices[i], :])
# add2 += 1
PC_set[p] = PC
PC_data = {}
PC_data['pred'] = PC_set
PC_data['gt'] = gt_3D_set
with open(os.path.join(pkl_folder, 'PC_set.pkl'), 'wb') as f:
pkl.dump(PC_data, f, pkl.HIGHEST_PROTOCOL)
else:
with open(os.path.join(pkl_folder, 'PC_set.pkl'), 'rb') as f:
PC_data = pkl.load(f)
PC_set = PC_data['pred']
gt_3D_set = PC_data['gt']
# Test linear
Errors_orig = []
Errors_pred = []
total_ignore = 0
for i in PC_set.keys():
cur_PC = PC_set[i]
gt_p3d = gt_3D_set[i]
if len(cur_PC.Pos2D) > 1:
orgerror = compare_with_groundtruth(PC_set[i].Pos3D, gt_p3d)
# Errors_orig.append(orgerror)
# print('\n * Point',cur_PC.Pos3D)
# print('* Has {0} frames'.format(len(cur_PC.Pos2D)))
cur_PC, opt_inliers = RansacLinearT(cur_PC, pu=0, pv=0)
if cur_PC == None:
print('-------- Ignore! ------------')
total_ignore += 1
PC_set.pop(i)
continue
PC_set[i] = cur_PC
# perror = compare_with_groundtruth(cur_PC.Pos3D, gt_p3d)
perror = linearT_errEst(cur_PC=cur_PC,
pu=0,
pv=0,
inliners=opt_inliers)
print('>> ', i, '/', len(PC_set.keys()),
' Error:',
orgerror, ' vs ', perror)
Errors_pred.append(perror)
# time.sleep(0.5)
print('Total errors:')
print('Original:', np.sum(np.array(Errors_orig)) / len(Errors_pred))
print('Pred:', np.sum(np.array(Errors_pred)) / len(Errors_pred))
print('Total ignore PC:', total_ignore)
# save the obtained PC_set to the file
points_3d, points_2d, points_indices, camera_indices, camera_params = util.convertPC(
PC_set, n_cameras=n_cameras, n_3dpoints=n_points)
DATA = {}
DATA['params'] = camera_params
DATA['cam_indices'] = camera_indices
DATA['points_indices'] = points_indices
DATA['2d'] = points_2d
DATA['3d'] = points_3d
with open('linearTed_online_data.pkl', 'wb') as f:
pkl.dump(DATA, f, pkl.HIGHEST_PROTOCOL)
plt.figure()
plt.scatter(range(len(Errors_pred)), Errors_pred)
plt.show()
return Errors_orig, Errors_pred
def correct_direction_with_lines(sample_point_cloud,
lines,
search_radius=15,
angle_threshold=np.pi / 6.0):
""" Correct sample_point_cloud using the extracted lines
Args:
- sample_point_cloud: an object of PointCloud
- lines: an array of [(p_start_e, p_start_n), (p_end_e, p_end_n)]
- search_radius: distance to search, in meters. Default is 10m.
Return:
- new_sample_point_cloud: an object of PointCloud
"""
new_sample_locations = []
new_sample_directions = []
line_starts = lines[:, 0, :]
line_ends = lines[:, 1, :]
line_vecs = line_ends - line_starts
sample_point_hash = {}
for pt_idx in range(0, sample_point_cloud.locations.shape[0]):
if np.linalg.norm(sample_point_cloud.locations[pt_idx]) < 0.1:
new_sample_locations.append(sample_point_cloud.locations[pt_idx])
new_sample_locations.append(sample_point_cloud.directions[pt_idx])
pt = sample_point_cloud.locations[pt_idx]
pt_key = (int(pt[0]), int(pt[1]))
# Find nearby line segments
nearby_line_idxs = []
vec1s = pt - line_starts
vec2s = pt - line_ends
for line_idx in range(0, len(vec1s)):
if np.dot(vec1s[line_idx], vec2s[line_idx]) > 0:
continue
line_dir = line_vecs[line_idx] / np.linalg.norm(
line_vecs[line_idx])
line_norm = np.array([-1 * line_dir[1], line_dir[0]])
dist = abs(np.dot(vec1s[line_idx], line_norm))
if dist <= search_radius:
nearby_line_idxs.append(line_idx)
if len(nearby_line_idxs) == 0:
continue
# Check if direction is consistent
potential_direction = np.array([0.0, 0.0])
direction_acceptable = False
for line_idx in nearby_line_idxs:
line_dir = line_vecs[line_idx] / np.linalg.norm(
line_vecs[line_idx])
dot_value = np.dot(line_dir, sample_point_cloud.directions[pt_idx])
if abs(dot_value) > np.cos(angle_threshold):
direction_acceptable = True
if dot_value > 0:
potential_direction += line_dir
else:
potential_direction += -1 * line_dir
if direction_acceptable:
potential_direction /= np.linalg.norm(potential_direction)
if not sample_point_hash.has_key(pt_key):
sample_point_hash[pt_key] = [potential_direction]
new_sample_locations.append(
sample_point_cloud.locations[pt_idx])
new_sample_directions.append(potential_direction)
else:
should_insert = True
for pt_direction in sample_point_hash[pt_key]:
if np.dot(pt_direction,
potential_direction) > np.cos(angle_threshold):
should_insert = False
if should_insert:
new_sample_locations.append(
sample_point_cloud.locations[pt_idx])
new_sample_directions.append(potential_direction)
sample_point_hash[pt_key].append(potential_direction)
return PointCloud(np.array(new_sample_locations),
np.array(new_sample_directions))
from point_cloud import PointCloud
if __name__ == '__main__':
if len(sys.argv) < 2:
print('[Data] Usage python 13_ply_observations.py config_folder')
exit(1)
config_folder = sys.argv[1] + '/'
assert os.path.exists(
config_folder), 'directory %s does not exist' % config_folder
config_files = [
config_file for config_file in os.listdir(config_folder)
if not (config_file.find('prior') > 0)
]
for config_file in config_files:
print('[Data] reading ' + config_folder + config_file)
config = utils.read_json(config_folder + config_file)
for k in range(config['n_observations']):
txt_directory = common.dirname(config, 'txt_gt_dir') + str(k) + '/'
assert os.path.exists(txt_directory)
ply_directory = common.dirname(config, 'ply_gt_dir') + str(k) + '/'
if not os.path.exists(ply_directory):
os.makedirs(ply_directory)
for filename in os.listdir(txt_directory):
point_cloud = PointCloud.from_txt(txt_directory + filename)
point_cloud.to_ply(ply_directory + filename[:-4] + '.ply')
print('[Data] wrote ' + ply_directory + filename[:-4] + '.ply')
def filter_point_cloud_using_grid(point_cloud,
point_directions,
sample_grid_size,
loc,
R):
""" Sample the input point cloud using a uniform grid. If there are points in the cell,
we will use the average.
"""
min_easting = loc[0]-R
max_easting = loc[0]+R
min_northing = loc[1]-R
max_northing = loc[1]+R
n_grid_x = int((max_easting - min_easting)/sample_grid_size + 0.5)
n_grid_y = int((max_northing - min_northing)/sample_grid_size + 0.5)
if n_grid_x > 1E4 or n_grid_y > 1E4:
print "ERROR! The sampling grid is too small!"
sys.exit(1)
sample_points = []
#sample_directions = []
sample_canonical_directions = []
geo_hash = {}
dir_hash = {}
geo_hash_count = {}
geo_hash_direction = {}
for pt_idx in range(0, len(point_cloud.locations)):
pt = np.copy(point_cloud.locations[pt_idx])
#pt_dir = point_cloud.directions[pt_idx]
px = int((pt[0] - min_easting) / sample_grid_size)
py = int((pt[1] - min_northing) / sample_grid_size)
if px<0 or px>n_grid_x or py<0 or py>n_grid_y:
print "ERROR! Point outside the grid!"
sys.exit(1)
if geo_hash.has_key((px, py)):
geo_hash_count[(px, py)] += 1
geo_hash[(px, py)] += pt
#dir_hash[(px, py)] += pt_dir
for direction in point_directions[pt_idx]:
geo_hash_direction[(px, py)].append(np.copy(direction))
else:
geo_hash_count[(px, py)] = 1.0
geo_hash_direction[(px, py)] = []
for direction in point_directions[pt_idx]:
geo_hash_direction[(px, py)].append(np.copy(direction))
geo_hash[(px, py)] = pt
#dir_hash[(px, py)] = pt_dir
for key in geo_hash.keys():
pt = geo_hash[key] / geo_hash_count[key]
#dir_hash[key] /= geo_hash_count[key]
sample_points.append(pt)
#sample_directions.append(dir_hash[key])
directions = []
for direction in geo_hash_direction[key]:
if len(directions) == 0:
directions.append(direction)
else:
found_match = False
for idx in range(0, len(directions)):
dot_value = np.dot(direction, directions[idx])
if abs(dot_value) > 0.7:
found_match = True
break
if not found_match:
directions.append(np.copy(direction))
sample_canonical_directions.append(list(directions))
sample_point_cloud = PointCloud(sample_points, [-1]*len(sample_points), [-1]*len(sample_points))
return sample_point_cloud, sample_canonical_directions
ROOT = os.path.dirname(os.path.abspath(__file__))
HOME = str(Path.home())
device = torch.device('cuda') if torch.cuda.is_available() else torch.device(
'cpu')
model = torch.load(os.path.join(ROOT, args.model))
model.to(device)
data_dir = os.path.join(HOME, args.data_dir)
frame_dir = os.path.join(data_dir, 'frame')
front_color_dir = os.path.join(data_dir, 'front_color')
side_color_dir = os.path.join(data_dir, 'side_color')
plane_estimator = PlaneEstimator(args, model)
tracker = MOT_Tracker(args, model)
side_pcd = PointCloud(vis=False)
history = defaultdict(list)
################ MAIN LOOP, READ FRAMES ONE BY ONE ###############
num_frames = len(os.listdir(frame_dir))
print("Found %d frames, start loading now....")
i_start = 1
num_frames = 500
for i in range(i_start, i_start + num_frames):
try:
frame = pickle.load(
open(os.path.join(frame_dir, 'frame_%07d.pkl' % (i)), 'rb'))
print('Loaded frame number %d' % i)
