mirror of
https://github.com/Astatin3/3Dscan.git
synced 2026-06-08 23:58:00 -06:00
IDK
This commit is contained in:
Astatin3
@@ -1,4 +1,6 @@
|
||||
from freenect2 import Device, FrameType
|
||||
import psutil
|
||||
import cv2
|
||||
|
||||
import open3d as o3d
|
||||
import numpy as np
|
||||
@@ -16,143 +18,84 @@ reconstruction.points = o3d.utility.Vector3dVector(np.random.random((5, 3)))
|
||||
vis.add_geometry(reconstruction)
|
||||
|
||||
|
||||
def preprocess_point_cloud(pcd, voxel_size):
|
||||
print(":: Downsample with a voxel size %.3f." % voxel_size)
|
||||
pcd_down = pcd.voxel_down_sample(voxel_size)
|
||||
threads = []
|
||||
|
||||
radius_normal = voxel_size * 5
|
||||
print(":: Estimate normal with search radius %.3f." % radius_normal)
|
||||
pcd_down.estimate_normals(
|
||||
o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
|
||||
|
||||
radius_feature = voxel_size * 10
|
||||
print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
|
||||
pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
|
||||
pcd_down,
|
||||
o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
|
||||
return pcd_down, pcd_fpfh
|
||||
|
||||
|
||||
def execute_global_registration(source_down, target_down, source_fpfh,
|
||||
target_fpfh, voxel_size):
|
||||
distance_threshold = voxel_size
|
||||
print(":: RANSAC registration on downsampled point clouds.")
|
||||
print(" Since the downsampling voxel size is %.3f," % voxel_size)
|
||||
print(" we use a liberal distance threshold %.3f." % distance_threshold)
|
||||
result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
|
||||
source_down, target_down, source_fpfh, target_fpfh, True,
|
||||
distance_threshold,
|
||||
o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
|
||||
3, [
|
||||
o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(
|
||||
0.9),
|
||||
o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
|
||||
distance_threshold)
|
||||
], o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))
|
||||
return result
|
||||
|
||||
|
||||
|
||||
|
||||
def update_points(color_frame, depth_frame):
|
||||
def parse_frame(depth_frame, color_frame):
|
||||
|
||||
global reconstruction
|
||||
|
||||
undistorted, registered, big_depth = device.registration.apply(frames[FrameType.Color], frames[FrameType.Depth],
|
||||
|
||||
undistorted, registered, big_depth = device.registration.apply(depth_frame,
|
||||
color_frame,
|
||||
with_big_depth=True)
|
||||
|
||||
points = device.registration.get_points_xyz_array(undistorted)
|
||||
|
||||
points = points[~np.isnan(points).any(axis=2)]
|
||||
|
||||
points = points.reshape(-1, 3) / 5
|
||||
|
||||
|
||||
cv2.imshow("1", np.array(registered.to_image()))
|
||||
# points = device.registration.get_points_xyz_array(undistorted)
|
||||
# points = points[~np.isnan(points).any(axis=2)]
|
||||
# points = points.reshape(-1, 3) / 5
|
||||
|
||||
# print(points.shape)
|
||||
|
||||
|
||||
# if np.isnan(points).any():
|
||||
# print("nan")
|
||||
# return
|
||||
#
|
||||
# num_points_to_select = int(0.1 * points.shape[0])
|
||||
# indices = np.random.choice(points.shape[0], num_points_to_select, replace=False)
|
||||
# reconstruction.points = o3d.utility.Vector3dVector(points)
|
||||
# vis.update_geometry(reconstruction)
|
||||
|
||||
|
||||
# points = points[indices]
|
||||
|
||||
pcd = o3d.geometry.PointCloud()
|
||||
|
||||
|
||||
if np.isnan(points).any():
|
||||
print("nan")
|
||||
|
||||
pcd.points = o3d.utility.Vector3dVector(points)
|
||||
# pcd = pcd.uniform_down_sample(100)
|
||||
pcd = pcd.voxel_down_sample(voxel_size)
|
||||
|
||||
# pcd.voxel_down_sample(voxel_size=0.05)
|
||||
|
||||
# reconstruction.points = pcd.points
|
||||
# reconstruction.points = points
|
||||
|
||||
if len(reconstruction.points) <= 5:
|
||||
reconstruction.points = pcd.points
|
||||
# visualise_sparse(reconstruction.points)
|
||||
return
|
||||
|
||||
print(len(reconstruction.points))
|
||||
|
||||
|
||||
|
||||
|
||||
print(":: Sample mesh to point cloud")
|
||||
# draw_registration_result(prev, curr, np.identity(4))
|
||||
|
||||
# source_down, source_fpfh = preprocess_point_cloud(reconstruction, voxel_size)
|
||||
# target_down, target_fpfh = preprocess_point_cloud(pcd, voxel_size)
|
||||
# result_ransac = execute_global_registration(source_down, target_down,
|
||||
# source_fpfh, target_fpfh,
|
||||
# voxel_size)
|
||||
#
|
||||
# pcd.transform(result_ransac.transformation)
|
||||
|
||||
# reconstruction.points = pcd.points
|
||||
|
||||
reconstruction.points.extend(pcd.points)
|
||||
print("123")
|
||||
# global threads
|
||||
# threads.pop(thread_index)
|
||||
|
||||
running = True
|
||||
|
||||
def compute():
|
||||
while running:
|
||||
if FrameType.Color in frames \
|
||||
and FrameType.Depth in frames \
|
||||
and FrameType.Color in frames:
|
||||
|
||||
print("Computing")
|
||||
|
||||
color_frame = frames[FrameType.Color]
|
||||
depth_frame = frames[FrameType.Depth]
|
||||
#
|
||||
# frames[FrameType.Color] = None
|
||||
# frames[FrameType.Depth] = None
|
||||
|
||||
update_points(color_frame, depth_frame)
|
||||
|
||||
vis.update_geometry(reconstruction)
|
||||
|
||||
|
||||
device = Device()
|
||||
frames = {}
|
||||
threads = []
|
||||
|
||||
for i in range(10):
|
||||
threads.append(Thread(target=compute))
|
||||
print(i)
|
||||
threads[i].start()
|
||||
depth_frame = None
|
||||
color_frame = None
|
||||
|
||||
n_cpus = psutil.cpu_count()
|
||||
|
||||
with device.running():
|
||||
|
||||
for type_, frame in device:
|
||||
|
||||
# print("updated")
|
||||
frames[type_] = frame
|
||||
if type_ == FrameType.Depth:
|
||||
depth_frame = frame
|
||||
elif type_ == FrameType.Color:
|
||||
color_frame = frame
|
||||
|
||||
if depth_frame is not None and \
|
||||
color_frame is not None:
|
||||
#
|
||||
if len(threads) > 100:
|
||||
depth_frame = None
|
||||
color_frame = None
|
||||
continue
|
||||
|
||||
undistorted, registered, big_depth = device.registration.apply(depth_frame,
|
||||
color_frame,
|
||||
with_big_depth=True)
|
||||
|
||||
cv2.imshow('Grayscale', np.array(registered.to_image()))
|
||||
|
||||
# cv2.imshow("1", np.array(registered.to_image()))
|
||||
# thread = Thread(target=parse_frame, args=(depth_frame, color_frame))
|
||||
# threads.append(thread)
|
||||
# thread.start()
|
||||
#
|
||||
# print(len(threads))
|
||||
#
|
||||
# depth_frame = None
|
||||
# color_frame = None
|
||||
|
||||
if not vis.poll_events():
|
||||
break
|
||||
# if not device.running():
|
||||
# break
|
||||
|
||||
vis.update_renderer()
|
||||
|
||||
@@ -160,9 +103,7 @@ vis.close()
|
||||
|
||||
running = False
|
||||
|
||||
print("joining threads...")
|
||||
|
||||
for thread in threads:
|
||||
thread.join()
|
||||
|
||||
|
||||
o3d.visualization.draw_geometries([reconstruction])
|
||||
thread.join()
|
||||
Reference in New Issue
Block a user