aligned with matlab

pmd.py

@@ -221,7 +221,16 @@ def main(config_path, img_folder):
     for i in range(n_cam):
         print('cam_id = ', i)
         contours_point = get_meshgrid_contour(binary_masks[i], save_path, debug=False)
-        world_points, world_points_boundary, world_points_boundary_3 = get_world_points(contours_point, cam_params[i], i, grid_spacing, cfg['d'], erosion_pixels=2, debug=0)
+        #world_points, world_points_boundary, world_points_boundary_3 = get_world_points(contours_point, cam_params[i], i, grid_spacing, cfg['d'], erosion_pixels=2, debug=0)
+        #world_points_x, world_points_y = np.meshgrid(np.linspace(31.8020, 77.9135, 1), np.linspace(33.5894,79.9621,1))
+        min_x, max_x, min_y, max_y = 31.8020, 77.9135-1, 33.5894, 79.9621-1
+        meshwidth = 1
+        world_points_x, world_points_y = np.meshgrid(np.arange(min_x, max_x + meshwidth, meshwidth), np.arange(min_y, max_y + meshwidth, meshwidth))
+        world_points_z = np.zeros_like(world_points_x)-cfg['d']
+        #print()
+        print('world_points_z = ', world_points_x.reshape(-1, 1).shape, world_points_y.reshape(-1, 1).shape, world_points_z.reshape(-1, 1).shape)
+
+        world_points = np.hstack((world_points_x.reshape(-1, 1), world_points_y.reshape(-1, 1),  world_points_z.reshape(-1, 1)))
         camera_points, u_p, v_p = get_camera_points(world_points, cam_params[i], save_path, i, debug=0)
         point_data = {'x_w': world_points[:, 0], 'y_w': world_points[:, 1], 'z_w': world_points[:, 2], 
                     'x_c': camera_points[:, 0], 'y_c': camera_points[:, 1], 'z_c': camera_points[:, 2],  
@@ -231,6 +240,8 @@ def main(config_path, img_folder):
         z_raw,  gradient_xy = reconstruction_cumsum(world_points, camera_points, screen_points, debug=0, smooth=smooth, align=align, denoise=denoise)
         
         z_raw_xy = np.round(z_raw[:, :2]).astype(int)
+
+        print('world_points =', world_points)
         
         # # 创建布尔掩码，初始为 True
         # mask = np.ones(len(z_raw_xy), dtype=bool)
@@ -248,9 +259,9 @@ def main(config_path, img_folder):
         mask = np.ones(len(z_raw_xy), dtype=bool)
 
         # 遍历每个边界点，标记它们在 aligned 中的位置
-        for boundary_point in world_points_boundary_3:
-            # 标记与当前边界点相同 xy 坐标的行
-            mask &= ~np.all(z_raw_xy == boundary_point[:2], axis=1)
+        # for boundary_point in world_points_boundary_3:
+        #     # 标记与当前边界点相同 xy 坐标的行
+        #     mask &= ~np.all(z_raw_xy == boundary_point[:2], axis=1)
         
         # 使用掩码过滤出非边界点
         non_boundary_points = z_raw[mask] 
@@ -312,13 +323,14 @@ def main(config_path, img_folder):
     total_cloud_point[:,0] = np.round(total_cloud_point[:,0])
     total_cloud_point[:,1] = np.round(total_cloud_point[:,1])
     #fitted_points = post_process(total_cloud_point, debug=0)
-    fitted_points = post_process_with_grad(img_folder, n_cam, 1)
+    test = post_process_with_grad(img_folder, n_cam, 1)
+    fitted_points = total_cloud_point
     #fitted_points = total_cloud_point
     #align_fitted, _ = align2ref(fitted_points)
     align_fitted = fitted_points
     
     write_point_cloud(os.path.join(img_folder, 'cloudpoint.txt'), np.round(align_fitted[:, 0]-np.mean(align_fitted[:, 0])), np.round(align_fitted[:, 1]-np.mean(align_fitted[:, 1])), align_fitted[:,2]-np.min(align_fitted[:,2]))
-    if 1:
+    if 0:
         fig = plt.figure()
         ax = fig.add_subplot(111, projection='3d')
 

src/recons.py

@@ -336,7 +336,7 @@ def reconstruction_cumsum(world_points, camera_points, screen_points, debug=Fals
  
     normal_vector = np.nan_to_num(normal_vector, nan=0.0)
 
-    #print('normal_vector = ', normal_vector[0])
+    #print('normal_vector = ', normal_vector)
 
     # 计算法向量在X方向的梯度
     gradient_x = normal_vector[0]

src/utils.py

@@ -433,6 +433,7 @@ def get_camera_points(world_points, world_mat_contents, save_path, cam_id, debug
         v_p_array.append(v_p)
     
     x_c_array, y_c_array, z_c_array, u_p_array, v_p_array = np.array(x_c_array), np.array(y_c_array), np.array(z_c_array), np.array(u_p_array), np.array(v_p_array)
+    #print('camera_points =', np.mean(x_c_array), np.mean(y_c_array), np.mean(z_c_array))
     camera_points = np.column_stack((x_c_array, y_c_array, z_c_array))
 
     if debug:
@@ -470,15 +471,24 @@ def get_screen_points(point_data, x_phase_unwrapped, y_phase_unwrapped, screen_p
     Rs2w = screen_to_world['screen_to_world_rotation']
     Ts2w = screen_to_world['screen_to_world_translation']
 
+    #checked
+    #print('Rs2w = ', Rs2w)
+    #print('Ts2w = ', Ts2w)
+
     # 计算 x_phase_unwrapped 中所有非 NaN 值的平均值
     average_x_phase_unwrapped = np.nanmean(x_phase_unwrapped)
     average_y_phase_unwrapped = np.nanmean(y_phase_unwrapped)
 
+    #checked
+    # print('average_x_phase_unwrapped = ', average_x_phase_unwrapped)
+    # print('average_y_phase_unwrapped = ', average_y_phase_unwrapped)
+
     # import pdb; pdb.set_trace()
     # 计算 value_map 并找到最小值
     value_map = np.abs(x_phase_unwrapped - average_x_phase_unwrapped) + np.abs(y_phase_unwrapped - average_y_phase_unwrapped)
     min_index = np.unravel_index(np.nanargmin(value_map), value_map.shape)
     
+    
     # import pdb; pdb.set_trace()
     # 选定一个中心相位值作为绝对相位值
     # abs_phasepoint_picture = np.array(min_index)
@@ -496,7 +506,8 @@ def get_screen_points(point_data, x_phase_unwrapped, y_phase_unwrapped, screen_p
                             Axis_conversion[1] * cfg['pixelSize_mm'] * (abs_phasepoint_screen[1] - screenorigin_point[1]),
                             0])
     abs_phasepoint_world = np.dot(Rs2w, arrow_abs_mm) + Ts2w.T
-    
+
+    #checked
     print('abs_phasepoint 绝对相位值:')
     print(abs_phasepoint)
     print('abs_phasepoint_world 绝对相位值对应的世界坐标系的位置:')
@@ -517,6 +528,9 @@ def get_screen_points(point_data, x_phase_unwrapped, y_phase_unwrapped, screen_p
         phase_x_now = x_phase_unwrapped[v_now, u_now]  # 注意这里的索引要按照Python的行列顺序，与MATLAB相反 raw
         phase_y_now = y_phase_unwrapped[v_now, u_now]
 
+        #print('phase_x_now = ', phase_x_now)
+        #print('phase_y_now = ', phase_y_now)
+
         #phase_x_now = x_phase_unwrapped[u_now, v_now]  
         #phase_y_now = y_phase_unwrapped[u_now, v_now]
 
@@ -539,6 +553,8 @@ def get_screen_points(point_data, x_phase_unwrapped, y_phase_unwrapped, screen_p
     x_data = np.array(x_data)
     y_data = np.array(y_data)
     z_data = np.array(z_data)   
+    print('u_p size = ', x_data.shape)
+    print('screen_points = ', x_data, np.nanmean(x_data),np.nanmean(y_data),np.nanmean(z_data))
     screen_points = np.column_stack((x_data, y_data, z_data))
 
     if debug:
@@ -1287,15 +1303,23 @@ def post_process_with_grad(img_folder, n_cam, debug=False):
     #print(x_grad_expand.shape, y_grad_expand.shape, x_expand.shape, y_expand.shape)
     # 调用 Southwell 重建函数
     #Z_reconstructed = southwell_integrate_smooth(x_grad_expand, y_grad_expand, x_expand, y_expand, (0, 0))
+    print('x_gradient shape = ', x_gradient.shape)
+    x_gradient = x_gradient.reshape(47, -1)
+    y_gradient = y_gradient.reshape(47, -1)
+    Z_reconstructed = np.cumsum(x_gradient, 1)*1 + np.cumsum(y_gradient, 0)*1
 
-    Z_reconstructed = np.cumsum(x_grad_expand, 1)*1 + np.cumsum(y_grad_expand, 0)*1
-    print('min_z =', np.min(Z_reconstructed))
+   
+    #print('y_grad_expand =', np.cumsum(y_grad_expand, 0))
+    #print('max_z =', np.max(Z_reconstructed))
 
-    #Z_reconstructed = poisson_reconstruct(x_grad_expand, y_grad_expand)
+    #Z_reconstructed = poisson_reconstruct(x_gradient, y_gradient)
+    print('min_z =', np.min(Z_reconstructed))
+    print('max_z =', np.max(Z_reconstructed))
+    print('x_grad_expand =', np.cumsum(x_grad_expand, 1))
     
-    z1_lookup = {(x1, y1): z1 for x1, y1, z1 in np.column_stack((x_expand.reshape(-1,1), y_expand.reshape(-1,1), Z_reconstructed.reshape(-1,1)))}
+    #z1_lookup = {(x1, y1): z1 for x1, y1, z1 in np.column_stack((x_expand.reshape(-1,1), y_expand.reshape(-1,1), Z_reconstructed.reshape(-1,1)))}
 
-    z1_values = np.array([z1_lookup.get((x, y), np.nan) for x, y in np.column_stack((x, y))])
+    #z1_values = np.array([z1_lookup.get((x, y), np.nan) for x, y in np.column_stack((x, y))])
     #fitted_points = post_process(np.column_stack((x,y,z1_values)), debug=0)
 
 
@@ -1308,9 +1332,9 @@ def post_process_with_grad(img_folder, n_cam, debug=False):
         # x_vals = fitted_points[:,0]
         # y_vals = fitted_points[:,1]
         # z_vals = fitted_points[:,2]
-        x_vals = x_expand
-        y_vals = y_expand
-        z_vals = x_grad_expand
+        x_vals = x.reshape(47, -1)
+        y_vals = y.reshape(47, -1)
+        z_vals = Z_reconstructed
         # 绘制3D点云
         ax.scatter(x_vals, y_vals, z_vals, c=z_vals, cmap='viridis', marker='o')
 
@@ -1322,7 +1346,8 @@ def post_process_with_grad(img_folder, n_cam, debug=False):
         plt.show()
     
     #return fitted_points
-    return np.column_stack((x,y,z1_values))
+    #return np.column_stack((x,y,Z_reconstructed))
+    return 0