import numpy as np
import cv2
from scipy.spatial import KDTree
from scipy.optimize import least_squares
from sklearn.decomposition import PCA
from sklearn.cluster import KMeans
from sklearn.neighbors import NearestNeighbors
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import os
import time
class GranularBallSFM:
def __init__(self, params=None):
# 算法参数(基于论文中的理论推导和实验验证)
self.params = params or {
# 特征预处理参数
'sift_contrast_threshold': 0.04,
'superpoint_threshold': 0.005,
'entropy_window_size': 15,
# 粒度球生成参数
'init_radius': 100,
'min_radius': 15,
'max_radius': 50,
'eta': 0.8, # 梯度-半径转换系数
'kappa': 20, # 熵项缩放系数
'tau': 5, # 熵项温度参数
'topology_tau': 5, # 拓扑容差参数
# 粗粒度重建参数
'coarse_threshold': 50,
'subblock_size': 8,
# 细粒度重建参数
'fine_threshold': 15,
'huber_delta': 2.0,
# 低纹理补足参数
'textureless_entropy': 2.0,
'textureless_radius': 100,
'icp_lambda': 0.1,
# 可视化参数
'visualize': True
}
# 存储中间结果
self.images = []
self.keypoints = []
self.descriptors = []
self.entropy_maps = []
self.gradient_maps = []
self.granular_balls = []
self.topology_graph = None
self.camera_poses = []
self.point_cloud = []
self.reconstructed_mesh = None
def load_images(self, image_paths):
"""加载图像序列"""
print(f"加载 {len(image_paths)} 张图像...")
self.images = [cv2.imread(path) for path in image_paths]
self.images = [cv2.cvtColor(img, cv2.COLOR_BGR2RGB) for img in self.images]
self.images = [cv2.cvtColor(img, cv2.COLOR_RGB2GRAY) for img in self.images]
return self.images
def feature_preprocessing(self):
"""特征预处理(4.1节)"""
print("执行特征预处理...")
sift = cv2.SIFT_create(contrastThreshold=self.params['sift_contrast_threshold'])
self.keypoints = []
self.descriptors = []
self.entropy_maps = []
self.gradient_maps = []
for img in self.images:
# 1. 计算SIFT特征
kp, des = sift.detectAndCompute(img, None)
# 2. 计算局部熵图
entropy_map = self._compute_entropy_map(img)
# 3. 计算梯度幅值图
gradient_map = self._compute_gradient_map(img)
self.keypoints.append(kp)
self.descriptors.append(des)
self.entropy_maps.append(entropy_map)
self.gradient_maps.append(gradient_map)
return self.keypoints, self.descriptors, self.entropy_maps, self.gradient_maps
def _compute_entropy_map(self, img):
"""计算局部熵图(公式4)"""
h, w = img.shape
entropy_map = np.zeros((h, w))
win_size = self.params['entropy_window_size']
half_win = win_size // 2
for y in range(half_win, h - half_win):
for x in range(half_win, w - half_win):
window = img[y - half_win:y + half_win + 1, x - half_win:x + half_win + 1]
hist = np.histogram(window, bins=256, range=(0, 255))[0]
hist = hist / hist.sum() + 1e-10 # 避免除以零
entropy = -np.sum(hist * np.log2(hist))
entropy_map[y, x] = entropy
# 归一化
entropy_map = (entropy_map - entropy_map.min()) / (entropy_map.max() - entropy_map.min())
return entropy_map
def _compute_gradient_map(self, img):
"""计算梯度幅值图(公式3)"""
sobel_x = cv2.Sobel(img, cv2.CV_64F, 1, 0, ksize=3)
sobel_y = cv2.Sobel(img, cv2.CV_64F, 0, 1, ksize=3)
gradient_magnitude = np.sqrt(sobel_x ** 2 + sobel_y ** 2)
# 归一化
gradient_magnitude = (gradient_magnitude - gradient_magnitude.min()) / \
(gradient_magnitude.max() - gradient_magnitude.min())
return gradient_magnitude
def generate_granular_balls(self, img_idx=0):
"""粒度球生成算法(4.2节)"""
print(f"为图像 {img_idx} 生成粒度球...")
img = self.images[img_idx]
entropy_map = self.entropy_maps[img_idx]
gradient_map = self.gradient_maps[img_idx]
kps = self.keypoints[img_idx]
h, w = img.shape
init_radius = self.params['init_radius']
min_radius = self.params['min_radius']
max_radius = self.params['max_radius']
eta = self.params['eta']
kappa = self.params['kappa']
tau = self.params['tau']
# 初始化粗粒度粒球
balls = []
queue = []
# 创建初始网格覆盖
for y in range(init_radius // 2, h, init_radius):
for x in range(init_radius // 2, w, init_radius):
center = (x, y)
radius = init_radius
projection = self._get_projection_area(img, center, radius)
ball = {'center': center, 'radius': radius, 'projection': projection}
queue.append(ball)
# 自适应粒度分裂
balls_fine = []
while queue:
ball = queue.pop(0)
center = ball['center']
radius = ball['radius']
projection = ball['projection']
# 计算平均梯度(公式3)
G_avg = np.mean([gradient_map[p[1], p[0]] for p in projection])
# 计算局部熵(公式4)
hist = np.histogram([img[p[1], p[0]] for p in projection], bins=256, range=(0, 255))[0]
hist = hist / hist.sum() + 1e-10
E_local = -np.sum(hist * np.log2(hist))
# 计算自适应半径(公式2)
r_k = eta * G_avg + kappa * np.exp(-E_local / tau)
# 粒度决策
if r_k < min_radius:
# 细粒度区域 - 分裂
sub_balls = self._split_ball(ball, r_k, img)
for sub_ball in sub_balls:
if sub_ball['radius'] > max_radius:
queue.append(sub_ball)
else:
balls_fine.append(sub_ball)
elif r_k > max_radius:
# 需要重新分裂
ball['radius'] = r_k
queue.append(ball)
else:
# 最优粒度 - 保留
ball['radius'] = r_k
balls.append(ball)
balls += balls_fine
self.granular_balls = balls
# 构建拓扑图(公式8)
self.topology_graph = self._build_topology_graph(balls)
return balls, self.topology_graph
def _get_projection_area(self, img, center, radius):
"""获取粒球投影区域"""
h, w = img.shape
xc, yc = center
projection = []
for y in range(max(0, int(yc - radius)), min(h, int(yc + radius + 1))):
for x in range(max(0, int(xc - radius)), min(w, int(xc + radius + 1))):
if (x - xc) ** 2 + (y - yc) ** 2 <= radius ** 2:
projection.append((x, y))
return projection
def _split_ball(self, ball, target_radius, img):
"""分裂粒球"""
center = ball['center']
radius = ball['radius']
projection = ball['projection']
# 提取粒球内特征点
points = []
for p in projection:
points.append(p)
if len(points) < 4: # 最少需要4个点进行聚类
return [ball]
# K-means聚类 - 修复了括号问题
k = max(2, int(np.sqrt(len(points))) // 2
kmeans = KMeans(n_clusters=k, random_state=0).fit(points)
labels = kmeans.labels_
centers = kmeans.cluster_centers_
# 创建子粒球
sub_balls = []
for i in range(k):
cluster_points = [points[j] for j in range(len(points)) if labels[j] == i]
if not cluster_points:
continue
# 计算新球心和新半径
center_new = (int(centers[i][0]), int(centers[i][1]))
distances = [np.linalg.norm(np.array(p) - np.array(center_new)) for p in cluster_points]
radius_new = min(target_radius, np.max(distances) if distances else target_radius)
# 确保半径不小于最小值
radius_new = max(radius_new, self.params['min_radius'])
projection_new = self._get_projection_area(img, center_new, radius_new)
sub_ball = {'center': center_new, 'radius': radius_new, 'projection': projection_new}
sub_balls.append(sub_ball)
return sub_balls
def _build_topology_graph(self, balls):
"""构建粒球拓扑图(公式8)"""
print("构建拓扑图...")
centers = [ball['center'] for ball in balls]
radii = [ball['radius'] for ball in balls]
tau = self.params['topology_tau']
# 使用KDTree加速邻域搜索
kdtree = KDTree(centers)
graph = {i: [] for i in range(len(balls))}
for i, ball in enumerate(balls):
# 查找邻近粒球
neighbors = kdtree.query_ball_point(ball['center'], ball['radius'] + max(radii) + tau)
for j in neighbors:
if i == j:
continue
# 计算中心距离
dist = np.linalg.norm(np.array(ball['center']) - np.array(balls[j]['center']))
# 检查是否相交(公式8)
if dist <= ball['radius'] + balls[j]['radius'] + tau:
graph[i].append(j)
return graph
def hierarchical_reconstruction(self):
"""分层三维重建(4.3节)"""
print("开始分层三维重建...")
# 阶段1: 粗粒度全局骨架构建
self.coarse_global_reconstruction()
# 阶段2: 细粒度局部BA优化
self.fine_grained_ba()
# 阶段3: 低纹理区域补足
self.textureless_completion()
print("三维重建完成!")
return self.point_cloud, self.camera_poses, self.reconstructed_mesh
def coarse_global_reconstruction(self):
"""粗粒度全局骨架构建(4.3.1节)"""
print("粗粒度全局骨架构建...")
coarse_balls = [ball for ball in self.granular_balls if ball['radius'] > self.params['coarse_threshold']]
# 1. 计算EWGD描述子(公式14-16)
for ball in coarse_balls:
# 获取粒球内的特征点
features = self._get_features_in_ball(ball)
if not features:
continue
# 计算SIFT聚合特征(公式14)
sift_features = [f[2] for f in features if f[2] is not None]
if sift_features:
sift_agg = np.mean(sift_features, axis=0)[:64] # 64维均值向量
else:
sift_agg = np.zeros(64)
# 计算熵权子块特征(公式15)
subblock_feature = self._compute_entropy_weighted_subblocks(ball)
# 拼接EWGD描述子(公式16)
ewgd = np.concatenate((sift_agg, subblock_feature))
ball['ewgd'] = ewgd
# 估计法向量(PCA)
points = [f[:2] for f in features]
if len(points) >= 3:
pca = PCA(n_components=3)
pca.fit(points)
ball['normal'] = pca.components_[2] # 最小特征值对应的特征向量
else:
ball['normal'] = np.array([0, 0, 1])
# 2. 全局旋转优化(公式17)
self._global_rotation_optimization(coarse_balls)
# 初始化相机位姿
self.camera_poses = [np.eye(4) for _ in range(len(self.images))]
# 初始化点云
self.point_cloud = self._initialize_point_cloud(coarse_balls)
def _get_features_in_ball(self, ball):
"""获取粒球内的特征点"""
features = []
center = ball['center']
radius = ball['radius']
for i, kp in enumerate(self.keypoints[0]): # 假设使用第一张图像的特征点
x, y = kp.pt
if (x - center[0]) ** 2 + (y - center[1]) ** 2 <= radius ** 2:
desc = self.descriptors[0][i] if self.descriptors[0] is not None else None
features.append((x, y, desc))
return features
def _compute_entropy_weighted_subblocks(self, ball):
"""计算熵权子块特征(公式15)"""
center = ball['center']
radius = ball['radius']
img = self.images[0]
entropy_map = self.entropy_maps[0]
subblock_size = self.params['subblock_size']
# 划分粒球为子块
x_min = max(0, int(center[0] - radius))
x_max = min(img.shape[1], int(center[0] + radius))
y_min = max(0, int(center[1] - radius))
y_max = min(img.shape[0], int(center[1] + radius))
subblock_width = (x_max - x_min) // subblock_size
subblock_height = (y_max - y_min) // subblock_size
subblock_feature = np.zeros(128) # 简化版本
# 简化实现:实际应使用更复杂的特征提取
for i in range(subblock_size):
for j in range(subblock_size):
# 计算子块位置
x_start = x_min + i * subblock_width
x_end = x_min + (i + 1) * subblock_width
y_start = y_min + j * subblock_height
y_end = y_min + (j + 1) * subblock_height
if x_start >= x_end or y_start >= y_end:
continue
# 计算子块熵值
sub_entropy = np.mean(entropy_map[y_start:y_end, x_start:x_end])
# 计算权重(熵越高权重越大)
weight = np.exp(sub_entropy) # 简化权重计算
# 提取子块特征(简化实现)
sub_img = img[y_start:y_end, x_start:x_end]
sub_feature = np.histogram(sub_img.flatten(), bins=128, range=(0, 255))[0]
sub_feature = sub_feature / sub_feature.sum() if sub_feature.sum() > 0 else sub_feature
# 加权累加
subblock_feature += weight * sub_feature
return subblock_feature
def _global_rotation_optimization(self, coarse_balls):
"""全局旋转优化(公式17)"""
# 简化实现:实际应使用李代数优化
print("执行全局旋转优化...")
# 构建法向量图
normals = [ball.get('normal', np.array([0, 0, 1])) for ball in coarse_balls]
# 优化目标:最小化相邻粒球法向量差异
# 实际实现应使用更复杂的优化算法
for i, ball in enumerate(coarse_balls):
if i not in self.topology_graph:
continue
neighbors = self.topology_graph[i]
if not neighbors:
continue
# 计算平均法向量
neighbor_normals = [coarse_balls[j].get('normal', np.array([0, 0, 1])) for j in neighbors]
avg_normal = np.mean(neighbor_normals, axis=0)
avg_normal = avg_normal / np.linalg.norm(avg_normal)
# 更新法向量
ball['normal'] = 0.7 * ball['normal'] + 0.3 * avg_normal
ball['normal'] = ball['normal'] / np.linalg.norm(ball['normal'])
def _initialize_point_cloud(self, coarse_balls):
"""初始化点云"""
point_cloud = []
for ball in coarse_balls:
# 简化实现:使用球心作为3D点
# 实际实现应使用三角测量
x, y = ball['center']
z = 0 # 简化假设
normal = ball.get('normal', np.array([0, 0, 1]))
point_cloud.append((x, y, z, normal))
return point_cloud
def fine_grained_ba(self):
"""细粒度BA优化(4.3.2节)"""
print("细粒度BA优化...")
fine_balls = [ball for ball in self.granular_balls if ball['radius'] < self.params['fine_threshold']]
# 简化实现:实际应使用LM优化
for ball in fine_balls:
# 获取粒球内的3D点
points_in_ball = self._get_points_in_ball(ball)
if not points_in_ball:
continue
# 获取观测到这些点的相机
cameras = self._get_cameras_observing_points(points_in_ball)
# 优化残差(公式18)
residuals = []
for point_idx in points_in_ball:
for cam_idx in cameras:
# 计算重投影误差
error = self._compute_reprojection_error(point_idx, cam_idx)
# 应用Huber损失(公式19)
huber_error = self._huber_loss(error, self.params['huber_delta'])
residuals.append(huber_error)
# 平均残差(简化)
avg_residual = np.mean(residuals) if residuals else 0
# 更新点位置(简化实现)
# 实际实现应使用优化算法更新点和相机参数
for point_idx in points_in_ball:
self._update_point_position(point_idx, avg_residual)
# 更新亏格(公式12-13)
self._update_genus()
def _get_points_in_ball(self, ball):
"""获取粒球内的3D点"""
points_in_ball = []
center = ball['center']
radius = ball['radius']
for i, point in enumerate(self.point_cloud):
x, y, z, _ = point
if (x - center[0]) ** 2 + (y - center[1]) ** 2 <= radius ** 2:
points_in_ball.append(i)
return points_in_ball
def _get_cameras_observing_points(self, point_indices):
"""获取观测到指定点的相机"""
# 简化实现:返回所有相机
return list(range(len(self.camera_poses)))
def _compute_reprojection_error(self, point_idx, cam_idx):
"""计算重投影误差"""
# 简化实现
return np.random.rand() * 2 # 随机误差
def _huber_loss(self, r, delta):
"""Huber损失函数(公式19)"""
if abs(r) <= delta:
return 0.5 * r ** 2
else:
return delta * (abs(r) - 0.5 * delta)
def _update_point_position(self, point_idx, residual):
"""更新点位置(简化)"""
# 实际实现应使用优化算法
x, y, z, normal = self.point_cloud[point_idx]
# 沿法线方向微调
self.point_cloud[point_idx] = (x, y, z + residual * 0.01, normal)
def _update_genus(self):
"""更新亏格(公式12-13)"""
# 简化实现
pass
def textureless_completion(self):
"""低纹理区域补足(4.3.3节)"""
print("低纹理区域补足...")
textureless_balls = []
# 识别低纹理区域(公式21)
for ball in self.granular_balls:
# 计算局部熵
features = self._get_features_in_ball(ball)
if not features:
continue
intensities = [self.images[0][int(p[1]), int(p[0])] for p in features]
hist = np.histogram(intensities, bins=256, range=(0, 255))[0]
hist = hist / hist.sum() + 1e-10
E_local = -np.sum(hist * np.log2(hist))
# 检查触发条件
if E_local < self.params['textureless_entropy'] and \
ball['radius'] > self.params['textureless_radius']:
textureless_balls.append(ball)
# 执行点面ICP(公式22)
for ball in textureless_balls:
self._point_to_plane_icp(ball)
# 孔洞填充
self._fill_holes()
def _point_to_plane_icp(self, ball):
"""点面ICP优化(公式22)"""
# 简化实现
center = np.array(ball['center'])
normal = ball.get('normal', np.array([0, 0, 1]))
# 查找邻近点
points = np.array([p[:3] for p in self.point_cloud])
if len(points) == 0:
return
nn = NearestNeighbors(n_neighbors=10).fit(points)
distances, indices = nn.kneighbors([center])
# 计算参考平面
neighbor_points = points[indices[0]]
pca = PCA(n_components=3)
pca.fit(neighbor_points)
ref_normal = pca.components_[2]
ref_point = np.mean(neighbor_points, axis=0)
# 更新法向量
ball['normal'] = 0.5 * normal + 0.5 * ref_normal
ball['normal'] = ball['normal'] / np.linalg.norm(ball['normal'])
def _fill_holes(self):
"""孔洞填充"""
# 简化实现
print("执行孔洞填充...")
# 实际实现应使用Poisson表面重建等算法
self.reconstructed_mesh = self.point_cloud
def visualize_results(self):
"""可视化重建结果"""
if not self.params['visualize']:
return
fig = plt.figure(figsize=(15, 10))
# 1. 可视化粒球
ax1 = fig.add_subplot(221)
img_idx = 0
img = self.images[img_idx]
ax1.imshow(img, cmap='gray')
ax1.set_title('Granular Balls')
for ball in self.granular_balls:
center = ball['center']
radius = ball['radius']
circle = plt.Circle(center, radius, color='r', fill=False, alpha=0.5)
ax1.add_patch(circle)
# 标记粗粒度球
if ball['radius'] > self.params['coarse_threshold']:
ax1.plot(center[0], center[1], 'bo', markersize=3)
# 标记细粒度球
elif ball['radius'] < self.params['fine_threshold']:
ax1.plot(center[0], center[1], 'go', markersize=3)
# 2. 可视化拓扑连接
ax2 = fig.add_subplot(222)
ax2.imshow(img, cmap='gray')
ax2.set_title('Topology Graph')
for i, ball in enumerate(self.granular_balls):
if i in self.topology_graph:
neighbors = self.topology_graph[i]
for j in neighbors:
if j > i: # 避免重复绘制
start = ball['center']
end = self.granular_balls[j]['center']
ax2.plot([start[0], end[0]], [start[1], end[1]], 'b-', alpha=0.3)
# 3. 可视化点云
if self.point_cloud:
ax3 = fig.add_subplot(223, projection='3d')
ax3.set_title('3D Point Cloud')
points = np.array([p[:3] for p in self.point_cloud])
normals = np.array([p[3] for p in self.point_cloud])
ax3.scatter(points[:, 0], points[:, 1], points[:, 2], c='b', s=1)
# 可视化法向量
for i in range(0, len(points), 10): # 每隔10个点显示法向量
start = points[i]
end = start + 10 * normals[i]
ax3.plot([start[0], end[0]], [start[1], end[1]], [start[2], end[2]], 'r-')
# 4. 可视化重建网格(简化)
ax4 = fig.add_subplot(224, projection='3d')
ax4.set_title('Reconstructed Surface')
if self.point_cloud:
points = np.array([p[:3] for p in self.point_cloud])
ax4.plot_trisurf(points[:, 0], points[:, 1], points[:, 2], cmap='viridis', alpha=0.8)
plt.tight_layout()
plt.savefig('granular_ball_sfm_results.png')
plt.show()
def run_full_pipeline(self, image_paths):
"""运行完整的三维重建流程"""
start_time = time.time()
# 1. 加载图像
self.load_images(image_paths)
# 2. 特征预处理
self.feature_preprocessing()
# 3. 粒度球生成(使用第一张图像)
self.generate_granular_balls(img_idx=0)
# 4. 分层三维重建
self.hierarchical_reconstruction()
# 5. 可视化结果
self.visualize_results()
end_time = time.time()
print(f"完整流程耗时: {end_time - start_time:.2f}秒")
return self.point_cloud, self.camera_poses, self.reconstructed_mesh
# 使用示例
if __name__ == "__main__":
# 图像路径(替换为您的图像序列)
image_dir = "path/to/your/images"
image_paths = [os.path.join(image_dir, f) for f in sorted(os.listdir(image_dir))
if f.endswith(('.jpg', '.png', '.jpeg'))]
# 创建并运行算法
sfm = GranularBallSFM()
point_cloud, camera_poses, reconstructed_mesh = sfm.run_full_pipeline(image_paths[:3]) # 使用前3张图像测试
# 保存结果(根据实际需要扩展)
np.save("point_cloud.npy", np.array(point_cloud))
np.save("camera_poses.npy", np.array(camera_poses))
print("三维重建结果已保存!")"D:\Python Charm\Python Pro\pythonProject1\.venv\Scripts\python.exe" C:\Users\英杰\Desktop\777\代码\最终算法\pythonProject1\算法大框架.py
File "C:\Users\英杰\Desktop\777\代码\最终算法\pythonProject1\算法大框架.py", line 228
k = max(2, int(np.sqrt(len(points))) // 2
^
SyntaxError: '(' was never closed
进程已结束,退出代码为 1