add elegant algorithm

.gitignore

@@ -0,0 +1 @@
+.*/

app.py

@@ -1,13 +1,89 @@
 import gradio as gr
+import cv2
+import matplotlib
 import numpy as np
+import os
+import time
+from PIL import Image
+import torch
+import torch.nn.functional as F
+import open3d as o3d
+import trimesh
+import tempfile
+import shutil
+from pathlib import Path
+from concurrent.futures import ThreadPoolExecutor
+from gradio_imageslider import ImageSlider
+from huggingface_hub import hf_hub_download
 import plotly.graph_objs as go
+from plotly.subplots import make_subplots
+from collections import deque
 from scipy.ndimage import convolve
-import os
+
+
+# from ppd.utils.set_seed import set_seed
+# from ppd.utils.align_depth_func import recover_metric_depth_ransac
+# from ppd.utils.depth2pcd import depth2pcd
+# from moge.model.v2 import MoGeModel 
+# from ppd.models.ppd import PixelPerfectDepth
+
+# try:
+#     import spaces
+#     HUGGINFACE_SPACES_INSTALLED = True
+# except ImportError:
+#     HUGGINFACE_SPACES_INSTALLED = False
+
+# css = """
+# #img-display-container {
+#     max-height: 100vh;
+# }
+# #img-display-input {
+#     max-height: 100vh;
+# }
+# #img-display-output {
+#     max-height: 100vh;
+# }
+# #download {
+#     height: 62px;
+# }
+
+# #img-display-output .image-slider-image {
+#     object-fit: contain !important;
+#     width: 100% !important;
+#     height: 100% !important;
+# }
+# # """
+
+# set_seed(666)
+
+# DEVICE = torch.device("cuda" if torch.cuda.is_available() else "cpu")
+
+# default_steps = 20
+# model = PixelPerfectDepth(sampling_steps=default_steps)
+# ckpt_path = hf_hub_download(
+#     repo_id="gangweix/Pixel-Perfect-Depth",
+#     filename="ppd.pth",
+#     repo_type="model"
+# )
+# state_dict = torch.load(ckpt_path, map_location="cpu")
+# model.load_state_dict(state_dict, strict=False)
+# model = model.eval()
+# model = model.to(DEVICE)
+
+# moge_model = MoGeModel.from_pretrained("Ruicheng/moge-2-vitl-normal").eval()
+# moge_model = moge_model.to(DEVICE)
+
+
+# 用来保存当前显示的图（全局缓存一份）
+current_img = None
+normalize_hist = None
+g_est_nosie = None
+
 
 def readRAW(path):
 
     filesize = os.path.getsize(path)
-    print(filesize)
+    print('filesize: ',filesize)
     if filesize == 31*40*64*2:
         output = np.fromfile(path, dtype=np.int16)
     else:
@@ -32,142 +108,575 @@ def readRAW(path):
         p3 = (B3 << 2) | ((B4 >> 6) & 0x03)
 
         output = np.stack([p0, p1, p2, p3], axis=1).flatten()
-    # output = np.fromfile(path, dtype=np.int16).reshape(31,40,64*2)
-    # output = np.fromfile(path, dtype=np.int16).reshape(30,40,64)
-
     return output.reshape(31,40,64)
 
+def on_image_click(evt: gr.SelectData):
+        global current_img
+        if current_img is None:
+            return None
 
-def load_bin(file):
+        x, y = evt.index  # 注意：是 (x, y)
+        img = current_img.copy()
 
-    
-    
+        # 画一个红点
+        cv2.circle(img, (x, y), 5, (255, 0, 0), -1)
 
-    # raw_hist = readRAW(file.name)[1:,...].astype(np.float32)
-    raw_hist = readRAW(file.name).astype(np.float32)
+        # current_img = img
+        return img
+    
+def update_image(img):
+    global current_img
+    current_img = img
 
-    print("raw_hist shape:", raw_hist[0,0,:])
-    # raw_hist = raw_hist[::-1, ::-1, :]
+def update_hist(hist):
+    global normalize_hist
+    normalize_hist = hist
 
-    print("raw_hist shape:", raw_hist[0,0,:])
+def load_bin(file):
+    raw_hist = readRAW(file.name).astype(np.float32)    
 
-    # raw_hist = readRAW(file.name)
-    # 默认显示一张 sum 图像
+    # multishot = raw_hist[..., 62] * 1024 + raw_hist[..., 63]
+    # raw_hist = image[1:,...].copy()
 
+    
     multishot =  (raw_hist[...,62]*1024 + raw_hist[...,63])
-    # multishot[multishot==0] = 20e3
-    normalize_data =  1 / multishot * 20e3
-
-    nor_hist = (raw_hist) * normalize_data[...,np.newaxis]
+    normalize_data =  1 / multishot * 1/1024
+    nor_hist = (raw_hist[...,:-2]) * normalize_data[...,np.newaxis]
+    
 
-    # nor_hist = (raw_hist) 
 
-    img = np.sum(nor_hist[1:, :, :-2], axis=2)
-    img = np.log(img +1)
-    norm_img = (img - img.min()) / (img.max())
-    img_uint8 = (norm_img * 255).astype(np.uint8)
+    # nor_hist = raw_hist / (multishot[..., None] + 1e-6)   # 防止除0
 
-    img_tc_zoomed = np.repeat(np.repeat(img_uint8, 16, axis=0), 16, axis=1) 
+    img = np.log1p(np.sum(nor_hist[1:, :, :-2], axis=2))
 
+    norm_img = (img - img.min()) / (img.max() - img.min() + 1e-6)
 
-    img = np.argmax(nor_hist[1:, :, 5:-2], axis=2)
-    norm_img = (img - img.min()) / (img.max() + 1e-8)
-    img_uint8 = (norm_img * 255).astype(np.uint8)
-    img_tof_zoomed = np.repeat(np.repeat(img_uint8, 16, axis=0), 16, axis=1) 
-    
-    return img_tc_zoomed,img_tof_zoomed, raw_hist, nor_hist
+    img_tc_zoomed = np.kron((norm_img * 255).astype(np.uint8),
+                            np.ones((16, 16), dtype=np.uint8))
 
+    update_image(img_tc_zoomed)
+    update_hist(nor_hist)
 
-def plot_pixel_histogram(evt: gr.SelectData, raw_hist, nor_hist):
-    # print("evt:", evt)
-    x, y = evt.index  # Gradio SelectData 对象
-    x = x // 16
-    y = y // 16
-    raw_values = raw_hist[y+1, x, :]
-    
-    tof = np.argmax(nor_hist[y+1, x, :-2])
-    range = 5
-    sim_values = nor_hist[y+1, x, tof-range:tof+range+1]
-    histogram_sim = raw_hist[1:, :, tof-range:tof+range+1]
-    print(sim_values.shape, histogram_sim.shape)
+    return raw_hist,img_tc_zoomed
 
-    img = np.tensordot(sim_values,histogram_sim, axes=(0, 2))
+cmap = matplotlib.colormaps.get_cmap('viridis')
+def gray_to_color_zoom(img_1ch):
+    # img_1ch: H×W, uint8 or float
+    img_norm = (img_1ch - img_1ch.min()) / (img_1ch.max() - img_1ch.min() + 1e-6)
 
-    # img = np.log(img +1)
-    norm_img = (img - img.min()) / (img.max() + 1e-8)
-    img_uint8 = (norm_img * 255).astype(np.uint8)
-    img_tof_zoomed = np.repeat(np.repeat(img_uint8, 16, axis=0), 16, axis=1) 
+    color_img = cmap(img_norm)[..., :3]   # 取 RGB，去掉 alpha
+    color_img = (color_img * 255).astype(np.uint8)
+    color_img = np.repeat(np.repeat(color_img, 16, axis=0), 16, axis=1) 
+    return color_img
 
-    vctEmbd = raw_hist[:1,:,:].flatten().astype(np.int32) >> 2
-    fRX_Temp = (vctEmbd[15] << 3) + vctEmbd[14]
+def main(share=True):
+    print("Initializing Demo...")
+    title = "# VisionICs 3D DEMO"
+    description = """ 上传 `.bin/.raw` 文件，点击图像像素查看该像素的直方图 """
 
-    LDVCC = (((((vctEmbd[65] << 8) + vctEmbd[64])) - 1024) / 1024 * 1.7 * 0.9638 + 1.42) * 6
-    fTx_Temp = (((vctEmbd[67] << 8) + vctEmbd[66] - 1024) / 5.34 + 30)
-    BVD = vctEmbd[23]
+    # @(spaces.GPU if HUGGINFACE_SPACES_INSTALLED else (lambda x: x))
+    # def predict_depth(image, denoise_steps):
+    #     depth, resize_image = model.infer_image(image, sampling_steps=denoise_steps)
+    #     return depth, resize_image
 
-    # fTx_Temp = float(vctEmbd[61]+((vctEmbd[63] & 0xc0) << 2)) * 0.178 - 38.18
-    # LDVCC = ((((vctEmbd[63]&0x30)<<4) + vctEmbd[60] - 110) * 13.7 + 5000) / 1000
-    y_min = np.min(raw_values[:-2]) - 10
-    y_max = np.max(raw_values[:-2]) + 10
-    fig = go.Figure()
-    fig.add_trace(go.Scatter(y=raw_values, mode="lines+markers"))
-    fig.update_layout(
-        title=f"Pixel ({x}, {y}) 在所有 {raw_values.shape[0]} 帧的强度变化  {f'RX: {fRX_Temp}  °C'} {f'TX: {fTx_Temp:.2f} °C'} {f'LDVCC: {LDVCC:.2f} V'} {f'BVD: {BVD} V'}",
-        xaxis_title="帧索引 (T)",
-        yaxis_title="强度值",
-        yaxis=dict(
-        range=[y_min, y_max])  # Set the min and max for y-axis
-    )
+    # @(spaces.GPU if HUGGINFACE_SPACES_INSTALLED else (lambda x: x))
+    # def predict_moge_depth(image):
+    #     image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+    #     image = torch.tensor(image / 255, dtype=torch.float32, device=DEVICE).permute(2, 0, 1)
+    #     metric_depth, mask, intrinsics = moge_model.infer(image)
+    #     metric_depth[~mask] = metric_depth[mask].max()
+    #     return metric_depth, mask, intrinsics
+    def estimate_noise(hist,noise_filter_steps):
+        noise_hist = np.sort(hist, axis=2)[..., ::-1][...,32:]
+        lower_bound = np.median(noise_hist, axis=2)
+        est_nosie = (lower_bound  +  noise_filter_steps * np.std(noise_hist,axis=2))
+        return est_nosie
+    
+    
+    def mean_pool_same_axis2(arr, k=3):
+        pad = k // 2
+        # reflect padding，最像真实数据
+        arr_pad = np.pad(arr, ((0,0),(0,0),(pad,pad)), mode='median')
 
+        H, W, C = arr.shape
+        out = np.zeros_like(arr)
 
-    return fig, img_tof_zoomed
+        for i in range(C):
+            window = arr_pad[:, :, i : i + k]
+            # out[:, :, i] = np.median(window, axis=2)
+            out[:, :, i] = np.mean(window, axis=2)
 
-# def plot_depth(nor_hist):
+        return out
+    
+    def median_pool_same_axis2(arr, k=12):
+        pad = k // 2
+        # reflect padding，最像真实数据
+        arr_pad = np.pad(arr, ((0,0),(0,0),(pad,pad)), mode='median')
+
+        H, W, C = arr.shape
+        out = np.zeros_like(arr)
+
+        for i in range(C):
+            window = arr_pad[:, :, i : i + k]
+            out[:, :, i] = np.median(window, axis=2)
+
+        return out
+    def min_pool_same_axis2(arr, k=12):
+        pad = k // 2
+        # reflect padding，最像真实数据
+        arr_pad = np.pad(arr, ((0,0),(0,0),(pad,pad)), mode='median')
+
+        H, W, C = arr.shape
+        out = np.zeros_like(arr)
+
+        for i in range(C):
+            window = arr_pad[:, :, i : i + k]
+            out[:, :, i] = np.min(window, axis=2)
+
+        return out   
+    def plot_pixel_histogram(evt: gr.SelectData, raw_hist, show_filter_hist):
+        CUSTOM_COLORS = [
+        "#1f77b4",  # 蓝
+        "#ff7f0e",  # 橙
+        "#2ca02c",  # 绿
+        "#d62728",  # 红
+        "#9467bd",  # 紫
+        ]
+            
+        # print("evt:", evt)
+        x, y = evt.index  # Gradio SelectData 对象
+        x = x // 16
+        y = y // 16
+        # multishot =  (raw_hist[...,62]*1024 + raw_hist[...,63])
+        # normalize_data =  1 / multishot *25e4 * 1/1024
+        # nor_hist = (raw_hist[...,:-2]) * normalize_data[...,np.newaxis]
+
+        ego_tof_hist = raw_hist[y+1, x, :]
+        # fig = go.Figure()
+        fig = make_subplots(specs=[[{"secondary_y": True}]])
+      
+      
+        fig.add_trace(go.Scatter(y=ego_tof_hist, mode="lines+markers",name="Raw"),secondary_y=False)
+    
+        if normalize_hist is not None and show_filter_hist:
+            global g_est_nosie
+            ego_normalize_hist = normalize_hist[y, x, :]
+            ego_tof = np.argmax(ego_normalize_hist)
+            fig.add_trace(go.Scatter(y=ego_normalize_hist, mode="lines+markers",name="Filtered"),secondary_y=True)
+            fig.add_vline(
+                x=ego_tof,
+                line_dash="dash",
+                line_width=2
+            )
+            # fig.add_hline(
+            #     y = g_est_nosie[y, x],
+            #     line_dash="dash",
+            #     line_width=2,secondary_y=True
+            # )
+            # print('est nosie ',  g_est_nosie[y, x])
+            
+            
+            fig.update_layout(
+                        title=f"Pixel ({x}, {y}) 在所有 {ego_tof} ",
+                        xaxis_title="帧索引 (T)",
+                        yaxis_title="强度值",
+                        # yaxis=dict(
+                        # range=[y_min, y_max])  # Set the min and max for y-axis
+                    )
+            
+            
+        return fig
+
+
+
+
+    def on_submit(image,cycle_steps, neighbor_filter_steps, noise_filter_steps, apply_scatter_filter,apply_ref_filter,apply_noise_filter,tof_range_min_steps,tof_range_max_steps, request: gr.Request = None):
+        global g_est_nosie
+        raw_hist = image[1:,...].copy()
+
+   
+        low, high = [tof_range_min_steps,tof_range_max_steps]
+
+        t0 = time.perf_counter()
+        multishot =  (raw_hist[...,62]*1024 + raw_hist[...,63])
+        normalize_data =  1 / multishot *cycle_steps * 1/1024
+        nor_hist = (raw_hist[...,:-2]) * normalize_data[...,np.newaxis]
+
+        dcr_cps = 4000
+        bin_size_ns = 0.25
+        total_bin = 62
+        integ_time_s = total_bin * bin_size_ns * cycle_steps * 1e-9
+        
+        count_per_bin = dcr_cps * integ_time_s *normalize_data
+        nor_hist = nor_hist - (count_per_bin)[...,np.newaxis]
+        nor_hist[nor_hist<0] = 0
+
+        filter_hist = np.zeros_like(nor_hist)
+
+        nor_hist[...,:low] = 0
+        nor_hist[...,high:] = 0
+
+        if apply_scatter_filter:
+            est_nosie = median_pool_same_axis2(nor_hist,k=12)
+            # est_nosie = min_pool_same_axis2(nor_hist,k=12)
+            sqrt_nosie = np.sqrt(est_nosie)
+            est_nosie = est_nosie + noise_filter_steps * sqrt_nosie
+            
+            g_est_nosie = est_nosie
+            nor_hist = nor_hist  - est_nosie
+            nor_hist[nor_hist<0] = 0 
+            bin_range = 3
+            for i in range(0,62,bin_range):   
+                map = (nor_hist[...,i:i+bin_range])
+                ratio = 1/(np.max(map)-np.min(map))*255
+                data = (map-np.min(map)) * ratio
+                # _, otsu_thresh = cv2.threshold(data.flatten().astype(np.uint8), 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
+                _, otsu_thresh = cv2.threshold(data.flatten().astype(np.uint8), 0, 255, cv2.THRESH_BINARY + cv2.THRESH_TRIANGLE)
+                mask = map > ( _ ) / ratio
+                filter_map =  map * mask 
+                filter_hist[...,i:i+bin_range] = filter_map
+
+        
+
+        t1 = time.perf_counter()
+        print("elapsed:", (t1 - t0)*1e3, "miliseconds")
+
+        mask_peak_filter = 1
+
+        filter_hist = filter_hist * 25e3
+ 
+        kernel = np.ones((3, 3, 3), dtype=np.float32)
+        kernel[1, 1, 1] = 0
+
+        mask_filter = (filter_hist > 1).astype(np.uint8)
+        out = convolve(mask_filter, kernel, mode='nearest', cval=0.0)
+        mask = out >= neighbor_filter_steps
+        print(filter_hist,out,mask)
+
+        # filter_hist = mask * filter_hist
+        filter_hist = np.where(mask, filter_hist, 0)
+        edge_range = 3
+        filter_hist[...,:edge_range] = 0
+        nor_hist[...,:edge_range] = 0
+        filter_hist[...,-edge_range:] = 0
+        nor_hist[...,-edge_range:] = 0
+        if apply_scatter_filter:
+            tof = np.argmax(filter_hist,axis=2) 
+        else:
+            tof = np.argmax(nor_hist,axis=2)
+        
+        peak = np.take_along_axis(nor_hist, tof[..., None], axis=2)[..., 0]
+
+        update_hist(filter_hist)
+        
+        if apply_noise_filter:
+            # th = est_nosie
+            est_nosie = mean_pool_same_axis2(nor_hist,8)
+            sqrt_nosie = np.sqrt(est_nosie)
+            est_nosie = est_nosie + noise_filter_steps * sqrt_nosie
+            th = np.take_along_axis(est_nosie, tof[..., None], axis=2)[..., 0]
+
+        else:
+            th = 0
+
+        mask_nosie = peak > th
+        tof = tof * mask_nosie  * mask_peak_filter
+        img_tof_zoomed= gray_to_color_zoom(tof)
+        peak = np.log1p(peak)
+        img_peak_zoomed= gray_to_color_zoom(peak)
+
+        # tof = tof/np.max(tof+1e-7)*255
+        # norm_tof = (tof - tof.min()) / (tof.max() + 1e-8)
+        # norm_tof_uint8 = (norm_tof * 255).astype(np.uint8)
+        # img_tof_zoomed = np.repeat(np.repeat(norm_tof_uint8, 16, axis=0), 16, axis=1) 
+
+        # peak = peak/np.max(peak+1e-7)*255
+        # norm_peak = (peak - peak.min()) / (peak.max() + 1e-8)
+        #  = (norm_peak * 255).astype(np.uint8)
+        #  = np.repeat(np.repeat(norm_peak_uint8, 16, axis=0), 16, axis=1) 
+
+        # H, W = image.shape[:2]
+        # ppd_depth, resize_image = predict_depth(image[:, :, ::-1], denoise_steps)
+        # resize_H, resize_W = resize_image.shape[:2]
+
+        # # moge provide metric depth and intrinsics
+        # moge_depth, mask, intrinsics = predict_moge_depth(resize_image)
+
+        # # relative depth -> metric depth
+        # metric_depth = recover_metric_depth_ransac(ppd_depth, moge_depth, mask)
+        # intrinsics[0, 0] *= resize_W 
+        # intrinsics[1, 1] *= resize_H
+        # intrinsics[0, 2] *= resize_W
+        # intrinsics[1, 2] *= resize_H
+
+        # # metric depth -> point cloud
+        # pcd = depth2pcd(metric_depth, intrinsics, color=cv2.cvtColor(resize_image, cv2.COLOR_BGR2RGB), input_mask=mask, ret_pcd=True)
+        # if apply_filter:
+        #     cl, ind = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
+        #     pcd = pcd.select_by_index(ind)
+
+        # tempdir = Path(tempfile.gettempdir(), 'ppd')
+        # tempdir.mkdir(exist_ok=True)
+        # output_path = Path(tempdir, request.session_hash)
+        # shutil.rmtree(output_path, ignore_errors=True)
+        # output_path.mkdir(exist_ok=True, parents=True)
+        
+        # ply_path = os.path.join(output_path, 'pointcloud.ply')
+
+        # # save pcd to temporary .ply
+        # pcd.points = o3d.utility.Vector3dVector(
+        #     np.asarray(pcd.points) * np.array([1, -1, -1], dtype=np.float32)
+        # )
+        # o3d.io.write_point_cloud(ply_path, pcd)
+        # vertices = np.asarray(pcd.points)
+        # vertex_colors = (np.asarray(pcd.colors) * 255).astype(np.uint8)
+        # mesh = trimesh.PointCloud(vertices=vertices, colors=vertex_colors)
+        # glb_path = os.path.join(output_path, 'pointcloud.glb')
+        # mesh.export(glb_path)
+
+
+        # # save raw depth (npy)
+        # depth = cv2.resize(ppd_depth, (W, H), interpolation=cv2.INTER_LINEAR)
+        # raw_depth_path = os.path.join(output_path, 'raw_depth.npy')
+        # np.save(raw_depth_path, depth)
+
+        # depth_vis = (depth - depth.min()) / (depth.max() - depth.min() + 1e-5) * 255.0
+        # depth_vis = depth_vis.astype(np.uint8)
+        # colored_depth = (cmap(depth_vis)[:, :, :3] * 255).astype(np.uint8)
+
+        # split_region = np.ones((image.shape[0], 50, 3), dtype=np.uint8) * 255
+        # combined_result = cv2.hconcat([image[:, :, ::-1], split_region, colored_depth[:, :, ::-1]])
+
+        # vis_path = os.path.join(output_path, 'image_depth_vis.png')
+        # cv2.imwrite(vis_path, combined_result)
+    
+        # file_names = ["image_depth_vis.png", "raw_depth.npy", "pointcloud.ply"]
+            
+        # download_files = [
+        #     (output_path / name).as_posix()
+        #     for name in file_names
+        #     if (output_path / name).exists()
+        # ]
 
-#     kernel = np.array([[1,1,1],[1,1,1],[1,1,1]])
+        # return [(image, colored_depth), glb_path, download_files]
+        return [img_tof_zoomed,img_peak_zoomed]
 
-#     # Create an empty array to store the results
-#     output = np.zeros((96, 240, 254))
+    
+    def draw_slice(input_image,slice_steps):
+        
+        raw_hist = input_image[1:,...] #remove embd
 
-#         # Perform the convolution along the first two axes (height and width)
-#     for i in range(254):
-#         output[:, :, i] = convolve(nor_hist[:, :, i], kernel, mode='constant', cval=0)
+         
+        
 
-#     modulate1 =  np.arange(1,181,1)
-#     modulate = modulate1 * modulate1 /(180*180)
-#     arr  = output[...,:180]  * modulate
+        print(slice_steps)
+        multishot =  (raw_hist[...,62]*1024 + raw_hist[...,63])
+        normalize_data =  1 / multishot *25e4
+        nor_hist = (raw_hist) * normalize_data[...,np.newaxis]
 
-#     tc_bin = np.sum(arr,axis=(0,1))
-#     max_id = np.argmax(tc_bin[:-2])
+        est_nosie = median_pool_same_axis2(nor_hist,k=12)
+        sqrt_nosie = np.sqrt(est_nosie)
+        est_nosie = est_nosie + 0.4 * sqrt_nosie
+        
+        nor_hist = nor_hist  - est_nosie
+        nor_hist[nor_hist<0] = 0 
 
-#     # modulate = np.concatenate([a, b,c])
-#     pad_head = np.ones(max_id-4)
-#     expand_kernel = np.arange(1,13,1) * 0.01
-#     pad_tail = np.ones((180-len(pad_head)-len(expand_kernel)))
-#     expand_filter = np.concatenate([pad_head, expand_kernel,pad_tail])
+        slice_img = nor_hist[...,slice_steps]
 
+        map = (nor_hist[...,slice_steps])
 
-#     arr_expandfilter  =  arr *   expand_filter
-#     tof  = np.argmax(arr,axis=2)
-#     tof_filter  = np.argmax(arr_expandfilter,axis=2)
+        ratio = 1/(np.max(map)-np.min(map))*255
+        data = (map-np.min(map)) * ratio
+        nonzero = data[data > 0]   # 排除 0
+        # _, otsu_thresh = cv2.threshold(data.flatten().astype(np.uint8), 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
+        _, otsu_thresh = cv2.threshold(nonzero.flatten().astype(np.uint8), 0, 255, cv2.THRESH_BINARY + cv2.THRESH_TRIANGLE)
+        mask = map > ( _ ) / ratio
 
-#     return tof, tof_filter
 
-with gr.Blocks() as demo:
-    gr.Markdown("## 上传 31,40,64 int16 `.bin/.raw` 文件，点击图像像素查看该像素的 64 帧直方图")
+        rgb_img = gray_to_color_zoom(slice_img)
+        rgb_mask_img = gray_to_color_zoom(mask.astype(np.float32))
+        print(mask.shape)
+        return rgb_img,rgb_mask_img
     
-    file_input = gr.File(label="上传 .raw/.bin 文件", file_types=[".raw", ".bin"])
-    image_tc_display = gr.Image(interactive=True, label="tc")
-    image_tof_display = gr.Image(interactive=True, label="tof")
+    with gr.Blocks(theme=gr.themes.Soft()) as demo:
+        gr.Markdown(title)
+        gr.Markdown(description)
+        gr.Markdown("### Simple Elegant Algorithm")
+
+        file_input = gr.File(label="上传 .raw/.bin/.txt 文件", file_types=[".raw", ".bin", ".txt"])
+        input_image = gr.State()
+
+        with gr.Row():
+            # Left: input image + settings
+            with gr.Column():
+                total_count_image = gr.Image(label="Total Count Image", image_mode="RGB", type='numpy', elem_id='img-display-input')
+            with gr.Column():
+                histogram = gr.Plot(label="像素直方图")
+        with gr.Row():
+            # Right: 3D point cloud + depth
+            with gr.Column():
+                tof_image = gr.Image(label="ToF Image", image_mode="RGB", type='numpy', elem_id='img-display-input')
+            with gr.Column():
+                peak_image = gr.Image(label="Peak Image", image_mode="RGB", type='numpy', elem_id='img-display-input')
+
+        with gr.Row():
+            with gr.Column():
+                submit_btn = gr.Button(value="Predict")
+                with gr.Accordion(label="Settings", open=False):
+                    show_filter_hist = gr.Checkbox(label="Show Filter HIST", value=False)
+
+                    cycle_steps = gr.Slider(label="reflect filter Steps", minimum=1, maximum=262144, value=25e4, step=1)
+
+                    tof_range_min_steps = gr.Slider(label="ToF Range Max Steps", minimum=0, maximum=62, value=5, step=1)
+                    tof_range_max_steps = gr.Slider(label="ToF Range Min Steps", minimum=0, maximum=62, value=60, step=1)
+
+
+
+                    apply_scatter_filter = gr.Checkbox(label="Apply scatter filter points", value=True)
+
+                    apply_ref_filter = gr.Checkbox(label="Apply reflect filter points", value=False)
+                    neighbor_filter_steps = gr.Slider(label="reflect filter Steps", minimum=1 , maximum=26, value=12, step=1)
+
+                    apply_noise_filter = gr.Checkbox(label="Apply noise filter points", value=False)
+                    noise_filter_steps = gr.Slider(label="noise filter Steps (STD)", minimum=0, maximum=1, value=0.3, step=0.01)
+                
+                # with gr.Accordion(label="Settings", open=False):
+
+                    with gr.Row():
+
+                        with gr.Column():
+                            slice_steps = gr.Slider(label="Slice Steps", minimum=0, maximum=63, value=0, step=1)
+                            slice_image = gr.Image(label="Slice Image", image_mode="RGB", type='numpy')
+              
+                            
+                        with gr.Column():
+                            binary_th_steps = gr.Slider(label="Binary Steps", minimum=0, maximum=256, value=128, step=1)
+                            slice_histogram  = gr.Image(label="Slice Image", image_mode="RGB", type='numpy')
+                        mask_image = gr.Image(label="Mask Image", image_mode="RGB", type='numpy')
+
+            # with gr.Column():
+            #     noise_image = gr.Image(label="Nosie Image", image_mode="RGB", type='numpy', elem_id='img-display-input')
+            # with gr.Column():
+            #     multishot_image = gr.Image(label="Multishot Image", image_mode="RGB", type='numpy', elem_id='img-display-input')
+                                                           
+                # with gr.Tabs():
+                #     with gr.Tab("3D View"):
+                #         model_3d = gr.Model3D(display_mode="solid", label="3D Point Map", clear_color=[1,1,1,1], height="60vh")
+                #     with gr.Tab("Depth"):
+                #         depth_map = ImageSlider(label="Depth Map with Slider View", elem_id='img-display-output', position=0.5)
+                #     with gr.Tab("Download"):
+                #         download_files = gr.File(type='filepath', label="Download Files")
+        
+       
+        file_input.change(load_bin, inputs=file_input, outputs=[input_image,total_count_image])
+
+        # total_count_image.change(
+        #     fn=update_image,
+        #     inputs=total_count_image,
+        #     outputs=total_count_image
+        # )
+        
+
+        total_count_image.select(plot_pixel_histogram, inputs=[ input_image ,show_filter_hist], outputs=[histogram])
+
+        total_count_image.select(
+            fn=on_image_click,
+            outputs=total_count_image
+        )
+
+        submit_btn.click(
+            fn=lambda: [None, None, None, "", "", ""]
+            ,outputs=[tof_image,peak_image]
+        ).then(
+            fn=on_submit,
+            inputs=[input_image,cycle_steps, neighbor_filter_steps, noise_filter_steps, apply_scatter_filter,apply_ref_filter,apply_noise_filter,tof_range_min_steps,tof_range_max_steps]
+            ,outputs=[tof_image,peak_image]
+        )
+
+        slice_steps.change(
+                    draw_slice,
+                    inputs=[input_image,slice_steps],
+                    outputs=[slice_image,mask_image]
+                    )
+
+        # example_files = os.listdir('assets/examples')
+        # example_files.sort()
+        # example_files = [os.path.join('assets/examples', filename) for filename in example_files]
+        # examples = gr.Examples(
+        #     examples=example_files, 
+        #     inputs=input_image, 
+        #     outputs=[depth_map, model_3d, download_files], 
+        #     fn=on_submit,
+        #     cache_examples=False
+        # )
+        
+    demo.queue().launch(share=share) 
+
+if __name__ == '__main__':
+    main(share=False)
+
+
+
+def mean_pool_same_axis2(arr, k=3):
+    pad = k // 2
+    # reflect padding，最像真实数据
+    arr_pad = np.pad(arr, ((0,0),(0,0),(pad,pad)), mode='median')
+
+    H, W, C = arr.shape
+    out = np.zeros_like(arr)
+
+    for i in range(C):
+        window = arr_pad[:, :, i : i + k]
+        # out[:, :, i] = np.median(window, axis=2)
+        out[:, :, i] = np.mean(window, axis=2)
+
+    return out
+
+def median_pool_same_axis2(arr, k=12):
+    pad = k // 2
+    # reflect padding，最像真实数据
+    arr_pad = np.pad(arr, ((0,0),(0,0),(pad,pad)), mode='median')
+
+    H, W, C = arr.shape
+    out = np.zeros_like(arr)
+
+    for i in range(C):
+        window = arr_pad[:, :, i : i + k]
+        out[:, :, i] = np.median(window, axis=2)
+
+    return out
+
+# raw_hist (30,40,64)
+# bin_range = 3
+
+
+# multishot =  (raw_hist[...,62]*1024 + raw_hist[...,63])
+# normalize_data =  1 / multishot *cycle_steps * 1/1024
+# nor_hist = (raw_hist[...,:-2]) * normalize_data[...,np.newaxis]
+
+# filter_hist = np.zeros_like(nor_hist)
+# est_nosie = median_pool_same_axis2(nor_hist,k=12)
+# sqrt_nosie = np.sqrt(est_nosie)
+# est_nosie = est_nosie + noise_filter_steps * sqrt_nosie
+
+# g_est_nosie = est_nosie
+# nor_hist = nor_hist  - est_nosie
+# nor_hist[nor_hist<0] = 0 
+
+# for i in range(0,62,bin_range):   
+#     map = (nor_hist[...,i:i+bin_range])
+
+   
 
-    histogram = gr.Plot(label="像素强度曲线")
-    raw_hist = gr.State()
-    nor_hist = gr.State()
-    image_sim_display = gr.Image(interactive=True, label="sim")
+#     ratio = 1/(np.max(map)-np.min(map))*255
+#     data = (map-np.min(map)) * ratio
 
-    file_input.change(load_bin, inputs=file_input, outputs=[image_tc_display, image_tof_display, raw_hist, nor_hist])
+#     _, otsu_thresh = cv2.threshold(data.flatten().astype(np.uint8), 0, 255, cv2.THRESH_BINARY + cv2.THRESH_TRIANGLE)
 
-    image_tof_display.select(plot_pixel_histogram, inputs=[ raw_hist, nor_hist], outputs=[histogram,image_sim_display])
+#     mask = map > ( _ ) / ratio
+#     filter_map =  map * mask
+#     # filter_map = map * mask 
+#     filter_hist[...,i:i+bin_range] = filter_map
 
-# demo.launch(share=True)
-demo.launch(share=False)
+# tof = np.argmax(filter_hist,axis=2) 
+# peak = np.take_along_axis(nor_hist, tof[..., None], axis=2)[..., 0]

app_.py

@@ -0,0 +1,483 @@
+import gradio as gr
+import numpy as np
+import plotly.graph_objs as go
+from scipy.ndimage import convolve
+import os
+import matplotlib.pyplot as plt
+from scipy.signal import find_peaks
+from scipy.ndimage import gaussian_filter1d
+import cv2
+
+def readRAW(path):
+
+    filesize = os.path.getsize(path)
+    print(filesize)
+    if filesize == 31*40*64*2:
+        output = np.fromfile(path, dtype=np.int16)
+    else:
+        with open(path, "rb") as f:
+            raw_data = f.read()
+
+
+        raw10 = np.frombuffer(raw_data, dtype=np.uint8)
+        n_blocks = raw10.shape[0] // 5
+
+        raw10 = raw10[:n_blocks * 5].reshape(-1, 5)
+
+        B0 = raw10[:, 0].astype(np.uint16)
+        B1 = raw10[:, 1].astype(np.uint16)
+        B2 = raw10[:, 2].astype(np.uint16)
+        B3 = raw10[:, 3].astype(np.uint16)
+        B4 = raw10[:, 4]
+
+        p0 = (B0 << 2) | ((B4 >> 0) & 0x03)
+        p1 = (B1 << 2) | ((B4 >> 2) & 0x03)
+        p2 = (B2 << 2) | ((B4 >> 4) & 0x03)
+        p3 = (B3 << 2) | ((B4 >> 6) & 0x03)
+
+        output = np.stack([p0, p1, p2, p3], axis=1).flatten()
+    # output = np.fromfile(path, dtype=np.int16).reshape(31,40,64*2)
+    # output = np.fromfile(path, dtype=np.int16).reshape(30,40,64)
+
+    return output.reshape(31,40,64)
+
+
+def load_bin(file):
+
+    # raw_hist = readRAW(file.name)[1:,...].astype(np.float32)
+    raw_hist = readRAW(file.name).astype(np.float32)
+
+    print("raw_hist shape:", raw_hist[0,0,:])
+    # raw_hist = raw_hist[::-1, ::-1, :]
+
+    print("raw_hist shape:", raw_hist[0,0,:])
+
+    # raw_hist = readRAW(file.name)
+    # 默认显示一张 sum 图像
+
+    multishot =  (raw_hist[...,62]*1024 + raw_hist[...,63])
+    # multishot[multishot==0] = 20e3
+    # normalize_data =  1 / multishot * 20e3
+    normalize_data =  1 / multishot * 4e4 * 1/1023
+    
+    nor_hist = (raw_hist) * normalize_data[...,np.newaxis]
+
+    # nor_hist = (raw_hist) 
+
+    img = np.sum(nor_hist[1:, :, :-2], axis=2)
+    img = np.log(img +1)
+    norm_img = (img - img.min()) / (img.max())
+    img_uint8 = (norm_img * 255).astype(np.uint8)
+
+    img_tc_zoomed = np.repeat(np.repeat(img_uint8, 16, axis=0), 16, axis=1) 
+
+
+    img = np.argmax(nor_hist[1:, :, 15:-2], axis=2)+15
+
+    nosie_est = np.mean(nor_hist[1:, :, -6:-3],axis=2)
+    th = nosie_est + 3*np.sqrt(nosie_est)
+    
+    peak = np.max(nor_hist[1:, :, 5:-2], axis=2)
+
+    mask = peak > th
+    # img = img * mask
+    print('std of tof' , np.std(img.flatten()),'std of peak' , np.std(peak.flatten()))
+    norm_img = (img - img.min()) / (img.max() + 1e-8)
+    img_uint8 = (norm_img * 255).astype(np.uint8)
+    img_tof_zoomed = np.repeat(np.repeat(img_uint8, 16, axis=0), 16, axis=1) 
+    
+    return img_tc_zoomed,img_tof_zoomed, raw_hist, nor_hist
+
+
+def plot_pixel_histogram(evt: gr.SelectData, raw_hist, nor_hist):
+    # print("evt:", evt)
+    x, y = evt.index  # Gradio SelectData 对象
+    x = x // 16
+    y = y // 16
+
+    raw_hist = raw_hist - np.min(raw_hist[...,:-5],axis=2)[...,np.newaxis]
+    raw_hist[raw_hist<0] = 0
+ 
+    rm_scatter_hist = np.zeros_like(raw_hist)
+    # r=1
+    # for i in range(r,62):
+    #     range_hist = raw_hist[...,i] 
+    #     data = range_hist 
+    #     _, otsu_thresh = cv2.threshold(data.flatten().astype(np.uint8), 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
+    #     mask = range_hist > _ 
+    #     filter_map = range_hist * mask
+    #     raw_hist[...,i] = filter_map
+
+
+    raw_values = raw_hist[y+1, x, :]
+    raw_values1 = raw_hist[y+2, x, :]
+    raw_values2 = raw_hist[y, x, :]
+    raw_values3 = raw_hist[y+1, x+1, :]
+    raw_values4 = raw_hist[y+1, x-1, :]
+    
+    tof = np.argmax(nor_hist[y+1, x, 10:-5]) + 10
+
+    tof_map  = np.argmax(nor_hist[1:, :, 5:-5], axis=2)
+
+    kernel = np.array(([1,2,1]), dtype=np.int32)
+ 
+    result_conv = convolve(raw_values, kernel, mode='constant', cval=0)
+    # result_conv = data
+    I4 = tof
+
+    I3 = I4-1
+    I5 = I4+1
+    C3 = result_conv[I3]
+    C4 = result_conv[I4]
+    C5 = result_conv[I5]
+    shift_mat = (C5-C3)/(4.0 * C4 -2.0 * C3 - 2.0 * C5)
+
+    sr_tof = (tof + shift_mat ) * 500 * 0.15
+
+    noise = np.mean(nor_hist[1:,...,:3],axis=2)
+
+    range_hist = 3
+
+    nor_hist[nor_hist>3e3] = 3e3
+    epsilon=1e-10
+    array = (nor_hist[y+1, x, tof-range_hist:tof+range_hist+1])  - noise[y,x]
+    safe_array = np.where(array <= 0, epsilon, array) 
+    sim_values = (safe_array)
+    array =  (nor_hist[1:, :, tof-range_hist:tof+range_hist+1])  -  noise[...,np.newaxis]
+    safe_array = np.where(array <= 0, epsilon, array)
+    histogram_sim = (safe_array)
+    print(sim_values.shape, histogram_sim.shape,noise.shape)
+
+    img = np.tensordot(sim_values,histogram_sim, axes=(0, 2))
+    # img = np.log10(img)
+    print(np.max(img))
+    # img[img<0] = 0
+    
+    img = img/np.max(img+1e-7)*255
+
+    print('selected value: ',img[y,x],img.shape)
+
+#     img = np.zeros((30,40))
+#     for i in range(30):
+#         for j in range(40):
+#                 tof_ = np.argmax(nor_hist[i+1, j, :-2])
+                
+#                 # sim_values = nor_hist[i+1, j, tof_-range_hist:tof_+range_hist+1] 
+
+#                 array = (nor_hist[i+1, j, tof_-range_hist:tof_+range_hist+1])  - noise[i,j]
+#                 safe_array = np.where(array <= 0, epsilon, array) 
+#                 # print(safe_array.shape)
+#                 if safe_array.shape[0]==0:
+#                     continue
+#                 sim_values = (safe_array)
+                
+          
+#                 # histogram_sim = nor_hist[1:, :, tof_-range_hist:tof_+range_hist+1] 
+#                 array =  (nor_hist[1:, :, tof_-range_hist:tof_+range_hist+1])  -  noise[...,np.newaxis]
+#                 safe_array = np.where(array <= 0, epsilon, array)
+#                 if safe_array.shape[0]==0:
+#                     continue    
+#                 histogram_sim = (safe_array)
+
+# # 
+#                 img_ = np.tensordot(sim_values,histogram_sim, axes=(0, 2))
+#                 img_ = np.log(img_)
+#                 img_[img_<0] = 0
+#                 img_ = img_/np.max(img_+1e-7)*255
+#                 _, otsu_thresh = cv2.threshold(img_.astype(np.uint8), 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
+#                 if (img_[i,j]-_)>0:
+#                     # if np.std(img_.flatten())<50:
+#                     img[i,j] = 255
+#                     # print(i,j,img_[i,j],_)
+#                 else:
+#                     img[i,j] = 0
+
+#                     # print(i,j,img_[i,j],_,'remove')
+#                 if i==y and j==x:
+#                     print(i,j,'sim ',img_[i,j],' th ',_,'selected')
+                    
+                    
+#     # img = np.zeros((30,40))
+#     img = img * tof_map
+    norm_img = (img - img.min()) / (img.max() + 1e-8)
+    img_uint8 = (norm_img * 255).astype(np.uint8)
+    img_tof_zoomed = np.repeat(np.repeat(img_uint8, 16, axis=0), 16, axis=1) 
+    
+    vctEmbd = raw_hist[:1,:,:].flatten().astype(np.int32) >> 2
+    fRX_Temp = (vctEmbd[15] << 3) + vctEmbd[14]
+
+    LDVCC = (((((vctEmbd[65] << 8) + vctEmbd[64])) - 1024) / 1024 * 1.7 * 0.9638 + 1.42) * 6
+    fTx_Temp = (((vctEmbd[67] << 8) + vctEmbd[66] - 1024) / 5.34 + 30)
+    BVD = vctEmbd[23]
+
+    # fTx_Temp = float(vctEmbd[61]+((vctEmbd[63] & 0xc0) << 2)) * 0.178 - 38.18
+    # LDVCC = ((((vctEmbd[63]&0x30)<<4) + vctEmbd[60] - 110) * 13.7 + 5000) / 1000
+    y_min = np.min(raw_values[:-2]) - 10
+    y_max = np.max(raw_values[:-2]) + 10
+
+    CUSTOM_COLORS = [
+    "#1f77b4",  # 蓝
+    "#ff7f0e",  # 橙
+    "#2ca02c",  # 绿
+    "#d62728",  # 红
+    "#9467bd",  # 紫
+    ]
+    dash_styles = ["solid", "dash", "dot", "dashdot"]
+    fig = go.Figure()
+    # fig.add_trace(go.Scatter(y=raw_values, mode="lines+markers"))
+    # fig.add_trace(go.Scatter(y=raw_values1, mode="lines+markers"))
+    # fig.add_trace(go.Scatter(y=raw_values2, mode="lines+markers"))
+    # fig.add_trace(go.Scatter(y=raw_values3, mode="lines+markers"))
+    # fig.add_trace(go.Scatter(y=raw_values4, mode="lines+markers"))
+    # 取默认颜色序列
+    # colorway = fig.layout.colorway
+    # if colorway is None:
+    #     colorway = go.Figure().layout.colorway  # fallback
+    start = 5
+    range_num = 4
+   
+
+    hist_list = [raw_values1,raw_values2,raw_values3,raw_values4]
+    ego_tof = np.argmax(raw_values[start:-5 ]) +start
+
+    color = CUSTOM_COLORS[0]
+    fig.add_trace(go.Scatter(y=raw_values, mode="lines+markers",line_color=color))
+
+    fig.add_vline(
+        x=ego_tof,
+        line_color=color,
+        line_dash="solid",
+        line_width=2
+    )
+
+
+
+    ego_tof_hist = raw_values[ego_tof-range_num:ego_tof+range_num+1]
+    ego_tof_hist = ego_tof_hist - np.min(ego_tof_hist)
+    ego_tof_hist = ego_tof_hist/np.linalg.norm(ego_tof_hist)
+
+    ego_tof_neighbor_hist =[]
+    ego_tof_neighbor_proj = []
+    neighbor_tof_ego_proj = []
+    for i,v in enumerate(hist_list):
+
+        neighbor_tof = np.argmax(v[start:-5])+start
+
+        neighbor_hist = v[ego_tof-range_num:ego_tof+range_num+1]
+        neighbor_hist = neighbor_hist - np.min(neighbor_hist)
+
+        neighbor_hist = neighbor_hist/np.linalg.norm(neighbor_hist)
+        ego_tof_neighbor_hist.append(neighbor_hist)
+
+
+        neighbor_tof_ego_hist = raw_values[neighbor_tof-range_num:neighbor_tof+range_num+1]
+        neighbor_tof_ego_hist = neighbor_tof_ego_hist - np.min(neighbor_tof_ego_hist)
+
+        neighbor_tof_ego_hist = neighbor_tof_ego_hist/np.linalg.norm(neighbor_tof_ego_hist)
+
+        neighbor_tof_neighbor_hist = v[neighbor_tof-range_num:neighbor_tof+range_num+1]
+        neighbor_tof_neighbor_hist = neighbor_tof_neighbor_hist - np.min(neighbor_tof_neighbor_hist)
+
+        neighbor_tof_neighbor_hist = neighbor_tof_neighbor_hist/np.linalg.norm(neighbor_tof_neighbor_hist)
+        # print('neighbor_hist','ego_tof_hist',neighbor_hist,ego_tof_hist,np.dot(neighbor_hist,ego_tof_hist))
+        # print('neighbor_tof_ego_hist','neighbor_tof_neighbor_hist',neighbor_tof_ego_hist,neighbor_tof_neighbor_hist,np.dot(neighbor_tof_ego_hist,neighbor_tof_neighbor_hist))
+
+        ego_tof_neighbor_proj.append(np.dot(neighbor_hist,ego_tof_hist))
+        neighbor_tof_ego_proj.append(np.dot(neighbor_tof_ego_hist,neighbor_tof_neighbor_hist))
+        color = CUSTOM_COLORS[i % len(CUSTOM_COLORS)+1]
+        # fig.add_trace(go.Scatter(y=v, mode="lines+markers",line_color=color))
+
+        # fig.add_vline(
+        #     x=(neighbor_tof),
+        #     line_color=color,
+        #     line_dash=dash_styles[i % 4],
+        #     line_width=2
+        # )
+
+    fig.update_layout(
+        title=f"Pixel ({x}, {y}) 在所有 {raw_values.shape[0]} 帧的强度变化 {f'ToF: {sr_tof:.1f}  mm'} {f'RX: {fRX_Temp}  °C'} {f'TX: {fTx_Temp:.2f} °C'} {f'LDVCC: {LDVCC:.2f} V'} {f'BVD: {BVD} V'}",
+        xaxis_title="帧索引 (T)",
+        yaxis_title="强度值",
+        yaxis=dict(
+        range=[y_min, y_max])  # Set the min and max for y-axis
+    )
+    print('ego_tof_neighbor_proj',ego_tof_neighbor_proj)
+    print('neighbor_tof_ego_proj',neighbor_tof_ego_proj)
+
+    ego_tof_neighbor_hist = np.mean(np.array(ego_tof_neighbor_hist),axis=0)
+    print(ego_tof_neighbor_hist)
+    ego_tof_neighbor_hist = ego_tof_neighbor_hist/np.linalg.norm(ego_tof_neighbor_hist)
+    print('mean ',np.dot(ego_tof_neighbor_hist,ego_tof_hist))
+    fig.add_trace(go.Scatter(y=ego_tof_neighbor_hist, mode="lines"))
+    fig.add_trace(go.Scatter(y=ego_tof_hist, mode="lines+markers"))
+    return fig, img_tof_zoomed,img
+
+# def plot_depth(nor_hist):
+
+#     kernel = np.array([[1,1,1],[1,1,1],[1,1,1]])
+
+#     # Create an empty array to store the results
+#     output = np.zeros((96, 240, 254))
+
+#         # Perform the convolution along the first two axes (height and width)
+#     for i in range(254):
+#         output[:, :, i] = convolve(nor_hist[:, :, i], kernel, mode='constant', cval=0)
+
+#     modulate1 =  np.arange(1,181,1)
+#     modulate = modulate1 * modulate1 /(180*180)
+#     arr  = output[...,:180]  * modulate
+
+#     tc_bin = np.sum(arr,axis=(0,1))
+#     max_id = np.argmax(tc_bin[:-2])
+
+#     # modulate = np.concatenate([a, b,c])
+#     pad_head = np.ones(max_id-4)
+#     expand_kernel = np.arange(1,13,1) * 0.01
+#     pad_tail = np.ones((180-len(pad_head)-len(expand_kernel)))
+#     expand_filter = np.concatenate([pad_head, expand_kernel,pad_tail])
+
+
+#     arr_expandfilter  =  arr *   expand_filter
+#     tof  = np.argmax(arr,axis=2)
+#     tof_filter  = np.argmax(arr_expandfilter,axis=2)
+
+#     return tof, tof_filter
+
+
+
+def find_bimodal_threshold(data, bins=50, sigma=2):
+    """
+    查找双峰直方图的阈值
+    
+    参数:
+    data: 输入数据
+    bins: 直方图分组数
+    sigma: 高斯平滑参数
+    
+    返回:
+    threshold: 计算得到的阈值
+    peak_indices: 峰值位置
+    hist: 直方图数据
+    bin_edges: 分组边界
+    """
+    # 计算直方图
+    hist, bin_edges = np.histogram(data, bins=bins, density=True)
+    bin_centers = (bin_edges[:-1] + bin_edges[1:]) / 2
+    
+    # 对直方图进行高斯平滑
+    smoothed_hist = gaussian_filter1d(hist, sigma=sigma)
+    
+    # 查找峰值
+    peak_indices, peak_properties = find_peaks(smoothed_hist, height=0.01, distance=10)
+    
+    if len(peak_indices) >= 2:
+        # 找到两个主要峰值
+        peak_heights = smoothed_hist[peak_indices]
+        sorted_peaks = peak_indices[np.argsort(peak_heights)[-2:]]
+        sorted_peaks = np.sort(sorted_peaks)
+        
+        # 在两个峰值之间找到最低点作为阈值
+        valley_region = smoothed_hist[sorted_peaks[0]:sorted_peaks[1]]
+        if len(valley_region) > 0:
+            valley_index = np.argmin(valley_region) + sorted_peaks[0]
+            threshold = bin_centers[valley_index]
+        else:
+            threshold = bin_centers[sorted_peaks[0]]
+    else:
+        print("警告: 未检测到明显的双峰分布")
+        threshold = np.median(data)
+    
+    return threshold, peak_indices, hist, bin_edges
+
+def draw_histogram(evt: gr.SelectData,text, bins):
+    # 解析输入数据
+    try:
+        data = text.flatten()
+    except:
+        return None
+
+    x, y = evt.index  # Gradio SelectData 对象
+    x = x // 16
+    y = y // 16
+
+    # 使用OpenCV的Otsu阈值
+    _, otsu_thresh = cv2.threshold(data.astype(np.uint8), 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
+
+
+   # 查找双峰阈值
+    # threshold, peak_indices, hist, bin_edges = find_bimodal_threshold(data)
+    # print(f"检测到 {len(peak_indices)} 个峰值")
+    hist, bin_edges = np.histogram(data, bins=bins)
+    indices = np.arange(len(hist))
+    total_weight = np.sum(hist)
+    centroid = np.sum(indices * hist) / total_weight
+
+    print('ostu threshold:  ',_,'data std',np.std(data),' centroid, ',centroid, 'diff ', np.abs(_-centroid))
+
+    # plt.figure()
+    # # 绘制原始直方图
+    # plt.hist(data, bins=50, alpha=0.7, color='skyblue', edgecolor='black', 
+    #          label='数据分布', density=True)
+    
+    # # 画直方图
+    plt.figure()
+    plt.hist(data, bins=bins, density=False)
+    plt.xlabel("Value")
+    plt.ylabel("Count")
+    plt.title("Histogram")
+    # 绘制阈值线
+    plt.axvline(x=_, color='red', linestyle='--', linewidth=3, 
+               label=f'双峰阈值: {_:.2f}')
+    
+    plt.axvline(x=data[y*40+x], color='green', linestyle='--', linewidth=3, 
+               label=f'Seelcted: {_:.2f}')
+    # plt.legend()
+    return plt
+
+with gr.Blocks() as demo:
+    gr.Markdown("## 上传 31,40,64 int16 `.bin/.raw` 文件，点击图像像素查看该像素的 64 帧直方图")
+    
+    file_input = gr.File(label="上传 .raw/.bin 文件", file_types=[".raw", ".bin"])
+    image_tc_display = gr.Image(interactive=True, label="tc")
+    image_tof_display = gr.Image(interactive=True, label="tof")
+
+    histogram = gr.Plot(label="像素强度曲线")
+    raw_hist = gr.State()
+    nor_hist = gr.State()
+    img_state = gr.State()    # ✅ 保存你点击后的数组（替代原来的 img = []）
+
+    bins_slider = gr.Slider(5, 200, value=64, step=1, label="Bins")
+
+    image_sim_display = gr.Image(interactive=True, label="sim")
+    sim_histogram = gr.Plot(label="相似性直方图")
+ 
+    file_input.change(load_bin, inputs=file_input, outputs=[image_tc_display, image_tof_display, raw_hist, nor_hist])
+
+    image_tof_display.select(plot_pixel_histogram, inputs=[ raw_hist, nor_hist], outputs=[histogram,image_sim_display,img_state])
+
+
+    # 3️⃣ 用数组 + bins 重新画直方图
+    image_tof_display.select(
+        draw_histogram,
+        inputs=[img_state, gr.State(16)],
+        outputs=sim_histogram
+    )
+
+#  # 3️⃣ 用数组 + bins 重新画直方图
+#     bins_slider.change(
+#         draw_histogram,
+#         inputs=[img_state, bins_slider],
+#         outputs=sim_histogram
+#     )
+
+    # gr.Interface(
+    #     fn=draw_histogram,
+    #     inputs=[
+    #         img,
+    #         gr.Slider(5, 200, value=64, step=1, label="Bins")
+    #     ],
+    #     outputs=gr.Plot(),
+    # )
+
+# demo.launch(share=True)
+demo.launch(share=False)

z_tof2pointcloud.cpp

@@ -0,0 +1,174 @@
+constexpr float PI = 3.14159265358979323846f;
+
+void undistort_points_pinhole_cpp(
+    const std::vector<float> &K,
+    const std::vector<float> &D,
+    int num_points,
+    std::vector<float> &unit_vector)
+{
+    std::vector<float> distorted(num_points * 2, 0.f);
+    std::vector<float> undistorted(num_points * 2, 0.f);
+
+    // Generate distorted points (same as original)
+    for (int y = 0; y < 48; ++y)
+    {
+        for (int x = 0; x < 120; ++x)
+        {
+            int idx = (y * 120 + x) * 2;
+            distorted[idx] = (x + 0.5f);
+            distorted[idx + 1] = (y + 0.5f);
+        }
+    }
+
+    unit_vector.resize(num_points * 3);
+
+    float fx = K[0];
+    float fy = K[4];
+    float cx = K[2];
+    float cy = K[5];
+
+    float k1 = D[0], k2 = D[1], p1 = D[2], p2 = D[3], k3 = D[4];
+
+    for (int idx = 0; idx < num_points; ++idx)
+    {
+        int idx_two = idx * 2;
+        int idx_three = idx * 3;
+
+        float u = distorted[idx_two + 0];
+        float v = distorted[idx_two + 1];
+
+        //---------------------------------------------------------
+        // Step 1: Normalize distorted coords
+        //---------------------------------------------------------
+        float xd = (u - cx) / fx;
+        float yd = (v - cy) / fy;
+
+        //---------------------------------------------------------
+        // Step 2: Iteratively solve undistorted x,y (Brown–Conrady)
+        //---------------------------------------------------------
+        float x = xd;
+        float y = yd;
+
+        for (int iter = 0; iter < 5; iter++)
+        {
+            float r2 = x * x + y * y;
+            float r4 = r2 * r2;
+            float r6 = r4 * r2;
+
+            float radial = 1 + k1 * r2 + k2 * r4 + k3 * r6;
+
+            float x_tangential = 2 * p1 * x * y + p2 * (r2 + 2 * x * x);
+            float y_tangential = p1 * (r2 + 2 * y * y) + 2 * p2 * x * y;
+
+            float x_est = (xd - x_tangential) / radial;
+            float y_est = (yd - y_tangential) / radial;
+
+            x = x_est;
+            y = y_est;
+        }
+
+        //---------------------------------------------------------
+        // Step 3: Convert to undistorted pixel coords
+        //---------------------------------------------------------
+        float u_und = fx * x + cx;
+        float v_und = fy * y + cy;
+
+        undistorted[idx_two + 0] = u_und;
+        undistorted[idx_two + 1] = v_und;
+
+        //---------------------------------------------------------
+        // Step 4: Compute camera-frame unit vector
+        //---------------------------------------------------------
+        float X = x;
+        float Y = y;
+        float Z = 1.0f;
+
+        float norm = std::sqrt(X * X + Y * Y + Z * Z);
+
+        //---------------------------------------------------------
+        // Optional ellipse mask (unchanged)
+        //---------------------------------------------------------
+        // float ra = 1.f / (cx - 3);
+        // float rb = 1.f / (cy + 4);
+
+        // float mask = ((u - cx)*(u - cx)*ra*ra +
+        //               (v - cy)*(v - cy)*rb*rb - 1.f) < 0.f ? 1.f : 0.f;
+        float mask = 1;
+        unit_vector[idx_three + 0] = mask * (X / norm);
+        unit_vector[idx_three + 1] = mask * (Y / norm);
+        unit_vector[idx_three + 2] = mask * (Z / norm);
+    }
+}
+
+void tof_to_xyz(
+    const std::vector<float> &unit_vector,
+    const std::vector<float> &tof,
+    int num_points,
+    std::vector<float> &points3D)
+{
+    points3D.resize(num_points * 3);
+
+    for (int i = 0; i < num_points; ++i)
+    {
+        int idx3 = i * 3;
+        float d = tof[i];
+
+        points3D[idx3 + 0] = unit_vector[idx3 + 0] * d;
+        points3D[idx3 + 1] = unit_vector[idx3 + 1] * d;
+        points3D[idx3 + 2] = unit_vector[idx3 + 2] * d;
+    }
+}
+
+void depth_to_xyz(
+    const std::vector<float> &unit_vector,
+    const std::vector<float> &depth,
+    int num_points,
+    std::vector<float> &points3D)
+{
+    points3D.resize(num_points * 3);
+
+    for (int i = 0; i < num_points; ++i)
+    {
+        int idx3 = i * 3;
+        float d = depth[i];
+
+        points3D[idx3 + 0] = unit_vector[idx3 + 0]/ unit_vector[idx3 + 2] * d;
+        points3D[idx3 + 1] = unit_vector[idx3 + 1]/ unit_vector[idx3 + 2] * d;
+        points3D[idx3 + 2] = d;
+    }
+}
+
+int main()
+{
+    int WIDTH = 40;
+    int HEIGHT = 30;
+    int num_points = WIDTH * HEIGHT;
+
+    std::vector<float> unit_vector(num_points * 3, 0.f);
+    std::vector<float> depth(num_points , 0.f);
+    std::vector<float> points3D(num_points * 3 , 0.f); // 改成 *3
+
+    // 内参
+    float fx = 37;    
+    float fy = 37;
+    float cx = 20;
+    float cy = 15;
+
+    // 畸变参数
+    float k1 = 0.f;
+    float k2 = 0.f;
+    float p1 = 0.f;
+    float p2 = 0.f;
+    float k3 = 0.f;
+
+    std::vector<float> K = {fx , 0.f, cx , 0.f, fy , cy , 0.f, 0.f, 1.f};
+    std::vector<float> D = {k1, k2, p1, p2, k3};
+
+    // 生成 unit_vector
+    undistort_points_pinhole_cpp(K, D, num_points, unit_vector);
+
+    // 将 depth 转成相机坐标系
+    depth_to_xyz(unit_vector, depth, num_points, points3D);
+
+    return 0;
+}