hofee/new_nbv_rec
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

upd

Browse Source
This commit is contained in:
hofee
2025-06-12 14:31:41 +08:00
parent c0c4f9b610
commit 6cb1368ffa
14 changed files with 9353 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

79
runners/evaluate_pbnbv.py
View File

@@ -24,18 +24,20 @@ class VoxelStruct:
self.empty_voxels = []
self.unknown_voxels = []
self.frontier_voxels = []
self.curr_camera_pose = None
self.bbx_min = None
self.bbx_max = None
self.voxel_types: Dict[Tuple[float, float, float], VoxelType] = {}
def update_voxel_map(self, points: np.ndarray,
camera_pose: np.ndarray) -> Tuple[List[np.ndarray], List[np.ndarray]]:
points = self.transform_points(points, camera_pose)
#points = self.transform_points(points, camera_pose)
new_occupied = self.voxelize_points(points)
self.occupied_voxels.extend(new_occupied)
self.update_bounding_box()
self.ray_tracing(camera_pose[:3, 3], camera_pose[:3, :3])
self.update_frontier_voxels()
self.curr_camera_pose = camera_pose
return self.frontier_voxels, self.occupied_voxels
def ray_tracing(self, camera_position: np.ndarray, camera_rotation: np.ndarray):
@@ -91,14 +93,10 @@ class VoxelStruct:
hemisphere_radius = self.camera_working_distance + bbx_diagonal / 2
else:
hemisphere_radius = self.camera_working_distance
theta_step = np.pi / (12 * self.num_parallels)
phi_step = np.pi / (6 * self.viewpoints_per_parallel)
# 使用更密集的采样
theta_step = np.pi / (6 * self.num_parallels) # 减小theta的步长
phi_step = np.pi / (6 * self.viewpoints_per_parallel) # 减小phi的步长
# 从顶部到底部采样
for theta in np.arange(0, np.pi/6 + theta_step, theta_step):
# 在每个纬度上采样
for phi in np.arange(0, 2*np.pi, phi_step):
x = hemisphere_radius * np.sin(theta) * np.cos(phi)
y = hemisphere_radius * np.sin(theta) * np.sin(phi)
@@ -273,7 +271,6 @@ class PBNBV:
def capture(self, point_cloud: np.ndarray, camera_pose: np.ndarray):
frontier_voxels, occupied_voxels = self.voxel_struct.update_voxel_map(point_cloud, camera_pose)
# self.voxel_struct.visualize_voxel_struct(camera_pose)
self.fit_ellipsoids(frontier_voxels, occupied_voxels)
def reset(self):
@@ -382,8 +379,8 @@ class PBNBV:
return []
center = (self.voxel_struct.bbx_min + self.voxel_struct.bbx_max) / 2
radius = np.linalg.norm(self.voxel_struct.bbx_max - self.voxel_struct.bbx_min) / 2 + self.focal_length
#import ipdb; ipdb.set_trace()
radius = np.linalg.norm(self.voxel_struct.bbx_max - self.voxel_struct.bbx_min) / 2 + 0.3
candidate_views = []
latitudes = np.linspace(np.deg2rad(40), np.deg2rad(90), longitude_num)
@@ -432,15 +429,15 @@ class PBNBV:
scores = [self.evaluate_viewpoint(view) for view in candidate_views]
best_idx = np.argmax(scores)
return candidate_views[best_idx]
return candidate_views[best_idx],candidate_views
def execute(self) -> Tuple[np.ndarray, bool]:
best_view = self.select_best_view()
best_view,candidate_views = self.select_best_view()
has_frontier = any(e["type"] == "frontier" for e in self.ellipsoids)
done = not has_frontier
return best_view, done
return best_view, candidate_views, done
import os
import json
@@ -478,8 +475,8 @@ class EvaluatePBNBV(Runner):
CM = 0.01
self.min_new_pts_num = self.min_new_area * (CM / self.voxel_size) ** 2
self.overlap_limit = ConfigManager.get(namespace.Stereotype.RUNNER, "overlap_limit")
self.pbnbv = PBNBV()
self.pbnbv = PBNBV(self.voxel_size)
''' Experiment '''
self.load_experiment("nbv_evaluator")
self.stat_result_path = os.path.join(self.output_dir, "stat.json")
@@ -541,11 +538,49 @@ class EvaluatePBNBV(Runner):
status_manager.set_progress("inference", "inferencer", f"dataset", len(self.test_set_list), len(self.test_set_list))
def get_output_data(self):
pose_matrix, done = self.pbnbv.execute()
def visualize(self, best_view, candidate_views):
import plotly.graph_objects as go
fig = go.Figure()
colors = ['aggrnyl', 'agsunset', 'algae', 'amp', 'armyrose', 'balance',
'blackbody', 'bluered', 'blues', 'blugrn', 'bluyl', 'brbg']
color = colors[0]
candidate_pose = best_view
origin_candidate = candidate_pose[:3, 3]
z_axis_candidate = candidate_pose[:3, 2]
fig.add_trace(go.Cone(
x=[origin_candidate[0]], y=[origin_candidate[1]], z=[origin_candidate[2]],
u=[z_axis_candidate[0]], v=[z_axis_candidate[1]], w=[z_axis_candidate[2]],
colorscale=color,
sizemode="absolute", sizeref=0.1, anchor="tail", showscale=False
))
for candidate_pose in candidate_views:
origin_candidate = candidate_pose[:3, 3]
z_axis_candidate = candidate_pose[:3, 2]
color = colors[1]
fig.add_trace(go.Cone(
x=[origin_candidate[0]], y=[origin_candidate[1]], z=[origin_candidate[2]],
u=[z_axis_candidate[0]], v=[z_axis_candidate[1]], w=[z_axis_candidate[2]],
colorscale=color,
sizemode="absolute", sizeref=0.05, anchor="tail", showscale=False
))
offset = np.asarray([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
pose_matrix = pose_matrix @ offset
fig.update_layout(
title="Clustered Poses and Input Points",
scene=dict(
xaxis_title='X',
yaxis_title='Y',
zaxis_title='Z'
),
margin=dict(l=0, r=0, b=0, t=40),
scene_camera=dict(eye=dict(x=1.25, y=1.25, z=1.25))
)
fig.show()
def get_output_data(self):
pose_matrix,candidate_views, done = self.pbnbv.execute()
rot = pose_matrix[:3,:3]
pose_6d = PoseUtil.matrix_to_rotation_6d_numpy(rot)
@@ -554,7 +589,9 @@ class EvaluatePBNBV(Runner):
pose_9d = np.concatenate([pose_6d, translation], axis=0).reshape(1,9)
pose_9d = pose_9d.repeat(50, axis=0)
#import ipdb; ipdb.set_trace()
return {"pred_pose_9d": pose_9d}
#self.visualize(pose_matrix,candidate_views)
return {"pred_pose_9d": pose_9d, "candidate_views": candidate_views}
def predict_sequence(self, data, cr_increase_threshold=0, overlap_area_threshold=25, scan_points_threshold=10, max_iter=50, max_retry = 10, max_success=3):
scene_name = data["scene_name"]
@@ -611,8 +648,8 @@ class EvaluatePBNBV(Runner):
predict_result = PredictResult(pred_pose_9d, input_pts=input_data["combined_scanned_pts"], cluster_params=dict(eps=0.25, min_samples=3))
# -----------------------
import ipdb; ipdb.set_trace()
predict_result.visualize()
#import ipdb; ipdb.set_trace()
#predict_result.visualize()
# -----------------------
pred_pose_9d_candidates = predict_result.candidate_9d_poses
#import ipdb; ipdb.set_trace()