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
|
from freenect2 import Device, FrameType
|
||||||
|
import psutil
|
||||||
|
import cv2
|
||||||
|
|
||||||
import open3d as o3d
|
import open3d as o3d
|
||||||
import numpy as np
|
import numpy as np
|
||||||
@@ -16,143 +18,84 @@ reconstruction.points = o3d.utility.Vector3dVector(np.random.random((5, 3)))
|
|||||||
vis.add_geometry(reconstruction)
|
vis.add_geometry(reconstruction)
|
||||||
|
|
||||||
|
|
||||||
def preprocess_point_cloud(pcd, voxel_size):
|
threads = []
|
||||||
print(":: Downsample with a voxel size %.3f." % voxel_size)
|
|
||||||
pcd_down = pcd.voxel_down_sample(voxel_size)
|
|
||||||
|
|
||||||
radius_normal = voxel_size * 5
|
def parse_frame(depth_frame, color_frame):
|
||||||
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):
|
|
||||||
|
|
||||||
global reconstruction
|
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)
|
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])
|
# reconstruction.points = o3d.utility.Vector3dVector(points)
|
||||||
# indices = np.random.choice(points.shape[0], num_points_to_select, replace=False)
|
# vis.update_geometry(reconstruction)
|
||||||
|
|
||||||
|
|
||||||
# points = points[indices]
|
print("123")
|
||||||
|
# global threads
|
||||||
pcd = o3d.geometry.PointCloud()
|
# threads.pop(thread_index)
|
||||||
|
|
||||||
|
|
||||||
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)
|
|
||||||
|
|
||||||
running = True
|
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()
|
device = Device()
|
||||||
frames = {}
|
|
||||||
threads = []
|
|
||||||
|
|
||||||
for i in range(10):
|
depth_frame = None
|
||||||
threads.append(Thread(target=compute))
|
color_frame = None
|
||||||
print(i)
|
|
||||||
threads[i].start()
|
n_cpus = psutil.cpu_count()
|
||||||
|
|
||||||
with device.running():
|
with device.running():
|
||||||
|
|
||||||
for type_, frame in device:
|
for type_, frame in device:
|
||||||
|
|
||||||
# print("updated")
|
if type_ == FrameType.Depth:
|
||||||
frames[type_] = frame
|
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():
|
if not vis.poll_events():
|
||||||
break
|
break
|
||||||
# if not device.running():
|
|
||||||
# break
|
|
||||||
|
|
||||||
vis.update_renderer()
|
vis.update_renderer()
|
||||||
|
|
||||||
@@ -160,9 +103,7 @@ vis.close()
|
|||||||
|
|
||||||
running = False
|
running = False
|
||||||
|
|
||||||
|
print("joining threads...")
|
||||||
|
|
||||||
for thread in threads:
|
for thread in threads:
|
||||||
thread.join()
|
thread.join()
|
||||||
|
|
||||||
|
|
||||||
o3d.visualization.draw_geometries([reconstruction])
|
|
||||||
+4
-3
@@ -1,3 +1,4 @@
|
|||||||
open3d
|
open3d~=0.18.0
|
||||||
numpy
|
numpy~=1.25.2
|
||||||
freenect2
|
freenect2~=0.2.3
|
||||||
|
psutil~=5.9.4
|
||||||
Reference in New Issue
Block a user