Initial commit: Multi-stage LiDAR mapping fusion framework with SVD alignment...

Initial commit: Multi-stage LiDAR mapping fusion framework with SVD alignment and GTSAM optimization

.gitignore

+# Python
+__pycache__/
+*.py[cod]
+*$py.class
+*.so
+.Python
+env/
+venv/
+.env
+.venv
+
+# IDE
+.vscode/
+.idea/
+
+# Project Outputs
+results/
+*.log
+
+# Data
+# Ignore raw large PCDs, but keep Downsampled (DS) and Pose data as requested
+data/HK_PCD/
+!data/HK_PCD_DS/
+!data/HK_POSE/
+
+# System
+Thumbs.db
+Desktop.ini

README.md

+# 大规模 LiDAR 建图融合
+
+## 项目概述
+
+本项目实现了一个针对大规模 LiDAR 建图数据的融合框架。主要解决 SLAM 算法（如 Fast-LIVO2）生成的多个独立子图（Submaps）在全局坐标系下的对齐与一致性问题。
+
+本方案利用 GNSS 先验（模拟或真实）进行轨迹级粗配准，结合 ICP 精配准，最终通过构建位姿图并在 GTSAM 中求解，实现全局一致的地图融合。
+
+> **状态**: 阶段性研究原型。目前在HKisland03拆分的4张图上进行了测试。
+
+## 方法
+
+融合策略包含三个主要阶段：
+
+1. **粗配准 (Coarse Alignment)**:
+* 利用 **Kabsch 算法 (SVD)** 计算局部里程计轨迹与全局 GNSS 轨迹之间的刚性变换 。
+* 解决初始位姿估计及坐标系对齐问题。
+
+
+2. **精配准 (Fine Registration)**:
+* 基于质心距离门控 (Distance Gating) 和体素重叠检测筛选相邻子图。
+* 使用 **Point-to-Plane ICP** 优化子图间的相对位姿。
+
+
+3. **全局优化 (Global Optimization)**:
+* 构建因子图 (Factor Graph)，包含 **一元因子 (Unary Factors, GNSS Priors)** 和 **二元因子 (Binary Factors, ICP Constraints)**。
+* 利用 **GTSAM (iSAM2 / Levenberg-Marquardt)** 求解全局最优位姿。
+
+
+
+## 项目结构 (Structure)
+
+```text
+.
+├── data/                   # 输入数据 (PCDs 和 Pose 文件)
+├── scripts/                # 预处理工具 (坐标修正, 降采样, GNSS模拟)
+├── src/
+│   ├── global_optimizer.py # 融合流程主入口
+│   └── optimizer_modules/  # 核心模块: Loader, Matching (SVD/ICP), Optimization (GTSAM)
+└── results/                # 输出结果 (轨迹 *.csv 及 融合点云 *.pcd)
+
+```
+
+## 环境依赖 (Dependencies)
+
+* **Python 3.10**
+* **基础库**: `open3d`, `numpy`, `scipy`
+* **优化库**: `gtsam`
+
+```bash
+conda env create -f environment.yaml
+conda activate mapping_fusion
+
+```
+
+## 使用流程 (Workflow)
+
+### 1. 数据预处理 (Preprocessing)
+
+请确保原始子图数据 (PCDs) 和轨迹文件已置于 `data/` 目录。
+
+**坐标系标准化与 GNSS 模拟:**
+修正 LiDAR 坐标系（如需）并从 Fast-LIVO2 里程计生成全局先验。
+
+```bash
+# 修正坐标轴 (如需) 并生成模拟 GNSS 数据
+python scripts/fix_pcd_coords.py data/HK_PCD --map "z,y,-x" --out_dir data/HK_PCD_FIXED
+python scripts/stitch_fastlivo2_poses.py --input-dir data/HK_POSE --pattern "*_fixed.txt"
+
+```
+
+**点云降采样:**
+对点云进行降采样以加速 ICP 配准过程。
+
+```bash
+python scripts/preprocess_pcds.py --pcd-dir data/HK_PCD --out-dir data/HK_PCD_DS --voxel-size 0.5
+
+```
+
+### 2. 执行融合 (Execution)
+
+运行主程序执行分层优化。流程模块化为 5 个阶段 (Load -> SVD -> Overlap -> ICP -> GraphOpt)。
+
+```bash
+python src/global_optimizer.py --pcd_dir "data/HK_PCD_DS" --max_stage 5
+
+```
+
+**关键参数:**
+
+* `--max_stage`: 指定终止阶段 (例如 `2` 用于查看粗配准结果，`5` 执行完整优化)。
+* `--gating_distance_m`: 空间邻接检测的距离阈值 (默认: 500m)。
+
+### 3. 输出结果 (Output)
+
+结果将保存至 `results/` 目录：
+
+* `global_poses_optimized.csv`: 优化后的全局轨迹 (Timestamp, x, y, z, qw, qx, qy, qz)。
+* `Global_Map_Optimized.pcd`: 最终融合的全局点云地图。
\ No newline at end of file

data/HK_POSE/01_gnss.txt

+1669703478.399821 2.4994 -0.6846 3.2356
+1669703479.399979 7.5127 -1.4031 1.2449
+1669703480.499970 7.7772 3.4345 2.6644
+1669703481.499987 2.5740 -2.9328 5.2608
+1669703482.599954 1.1071 -10.3496 1.7007
+1669703483.699904 -2.8404 -6.0316 14.6003
+1669703484.699965 -4.5032 -8.1618 22.2657
+1669703485.799911 -1.0788 -0.7618 7.9036
+1669703486.899897 -2.6655 -0.5601 9.2635
+1669703487.899921 1.9248 -4.1326 13.5881
+1669703488.999930 -2.9649 8.1147 14.9976
+1669703490.099934 -5.2790 2.9479 8.9745
+1669703491.099989 1.0523 -10.9490 8.4438
+1669703492.199992 0.9994 2.5377 15.9469
+1669703493.299927 -0.5456 -2.6947 7.7011
+1669703494.299973 -3.5428 -3.5201 20.3788
+1669703495.399963 1.7764 -10.0437 16.6987
+1669703496.399964 -1.8924 -4.6201 18.1282
+1669703497.499878 5.1697 3.4259 10.8642
+1669703498.599916 -1.5359 0.4210 19.9216
+1669703499.699892 -2.4068 -2.1569 9.5040
+1669703500.799889 -5.9772 2.8693 21.8209
+1669703501.899888 -0.3614 3.7961 16.8424
+1669703502.899890 -3.2289 0.5557 22.7169
+1669703503.899927 -0.1773 6.5620 1.9344
+1669703504.899974 4.1226 -0.8453 13.5356
+1669703505.999897 0.4560 -11.2234 13.9290
+1669703507.099916 1.7701 6.0915 12.4414
+1669703508.199998 -4.0743 -3.7926 19.6170
+1669703509.299998 1.6027 -3.9069 17.6160
+1669703510.399818 0.4430 3.6011 11.5460
+1669703511.399832 -1.6590 -3.2151 7.7346
+1669703512.399998 1.4731 0.0674 15.0747
+1669703513.499876 -1.2665 -8.2959 12.9525
+1669703514.499900 -1.6788 -5.1897 14.3935
+1669703515.599910 2.2288 8.1494 18.1967
+1669703516.699889 1.6328 -1.8484 11.2188
+1669703517.699900 0.3190 -1.3592 36.2441
+1669703518.699911 -0.3503 -0.3375 26.8057
+1669703519.799904 -5.0916 3.6524 34.0604
+1669703520.899917 4.8899 -6.8040 40.6219
+1669703521.899977 -5.9325 0.5131 47.5578
+1669703522.999985 -3.7215 -5.4869 40.3990
+1669703524.099886 -1.1407 -10.6090 43.5368
+1669703525.099919 -3.7958 -0.6470 41.5954
+1669703526.199913 9.4762 -7.1200 47.8677
+1669703527.199986 5.9512 -9.5288 53.6105
+1669703528.299877 8.5927 -11.6124 56.6952
+1669703529.299980 3.4854 0.1571 52.5825
+1669703530.399963 -4.2738 -1.3566 63.5409
+1669703531.399978 3.7124 -2.4234 61.6441
+1669703532.399991 3.7810 -2.8465 64.4594
+1669703533.499881 12.1126 -2.1460 65.3631
+1669703534.499902 6.2322 -9.5612 78.2411
+1669703535.599901 8.9357 -8.9965 82.4058
+1669703536.599981 5.3722 -0.9552 90.0581
+1669703537.699986 2.2418 -9.0248 79.4164
+1669703538.799882 -0.4503 -5.8400 88.8630
+1669703539.799909 5.1128 -1.4095 88.8916
+1669703540.899986 11.0817 -6.9209 102.5033
+1669703541.899987 6.8714 -9.9751 83.4622
+1669703542.999880 6.0455 -6.8306 92.1803
+1669703544.099910 5.8664 -6.1154 84.3714
+1669703545.199914 -4.1221 -7.9361 92.8701
+1669703546.199920 4.5548 -11.8540 89.4486
+1669703547.299917 5.4493 -9.9649 89.3577
+1669703548.399913 3.8909 -11.1859 90.3825
+1669703549.499968 6.4014 -0.0533 93.8744
+1669703550.599906 -3.3346 -10.2026 91.1952
+1669703551.699988 6.0584 -2.9985 107.8851
+1669703552.799989 6.3983 0.1204 93.3928
+1669703553.900005 6.8527 -7.1083 92.4116
+1669703554.999919 -0.2055 -6.6672 86.1891
+1669703556.099901 4.0903 6.1167 79.2785
+1669703557.099918 7.0980 -13.5201 86.2504
+1669703558.199935 9.0518 -5.1920 83.2137
+1669703559.299886 -0.0416 -2.1896 84.9463
+1669703560.399910 4.6416 -5.3486 85.3375
+1669703561.499888 14.3301 -2.3886 78.4693
+1669703562.499997 4.5031 -8.8898 92.8573
+1669703563.599812 -0.3653 -6.1481 91.1208
+1669703564.599928 7.9425 -11.5709 86.9316
+1669703565.699991 1.2421 -8.8165 97.4266
+1669703566.699997 5.6680 -11.8381 93.1833
+1669703567.799875 14.2462 -0.3673 80.9949
+1669703568.799916 1.2237 0.8232 85.0563
+1669703569.899916 5.8508 -1.6413 83.9524
+1669703570.899989 3.3785 -21.7310 83.2326
+1669703571.999982 2.5605 -11.7577 96.2387
+1669703573.099877 -3.3091 -7.5465 88.8009
+1669703574.099919 11.3041 -10.9203 94.0704
+1669703575.099961 4.5110 -6.4540 90.7289
+1669703576.099972 5.9147 -1.8666 88.9611
+1669703577.199915 3.5866 5.1645 92.1498
+1669703578.299906 14.0501 1.8480 99.7340
+1669703579.399930 -3.0788 10.5783 92.2407
+1669703580.399985 8.6007 11.1627 88.4641
+1669703581.499887 5.2761 14.5630 93.9811
+1669703582.599887 10.0739 17.2632 94.4591
+1669703583.699902 10.4005 28.0280 93.3298
+1669703584.799926 5.2460 24.3767 94.1273
+1669703585.899907 13.0005 30.2920 91.1887
+1669703586.999950 16.9089 30.6637 93.5557
+1669703587.999977 10.0115 35.4531 96.5193
+1669703589.099999 15.7122 43.8459 97.7797
+1669703590.199978 12.2091 46.4168 89.9230
+1669703591.299996 14.2915 45.5939 92.1830
+1669703592.399988 16.2036 45.3834 102.3801
+1669703593.499985 8.7542 46.6570 97.9384
+1669703594.599969 18.3064 59.0804 95.5121
+1669703595.699976 14.8554 54.6942 92.9727
+1669703596.799992 12.0594 67.2621 92.0875
+1669703597.799996 11.8240 63.7286 95.0903
+1669703598.899975 13.7016 64.4491 94.4568
+1669703599.999884 18.2808 69.2184 91.0966
+1669703601.099904 18.7831 67.7619 86.6291
+1669703602.099922 14.5213 76.8746 95.4291
+1669703603.099934 25.9928 85.1842 93.2845
+1669703604.099991 19.0390 78.8010 94.1899
+1669703605.199962 18.2698 88.6587 90.3669
+1669703606.199980 22.7887 97.6294 94.1670
+1669703607.299970 22.7441 96.6408 92.9385
+1669703608.399981 22.4353 96.4774 95.6369
+1669703609.499896 17.9663 99.7735 97.8568
+1669703610.499982 29.5956 107.3964 106.3344
+1669703611.599896 19.1101 110.1570 96.6940
+1669703612.599913 34.4346 104.6744 91.7810
+1669703613.699902 20.9955 101.3374 93.6013
+1669703614.799916 20.7590 115.9112 98.1636
+1669703615.799946 34.4179 122.8539 93.7823
+1669703616.899875 21.1117 123.7797 90.2778
+1669703617.899902 35.4179 130.1066 94.7225
+1669703618.899988 18.6289 133.8071 96.6978
+1669703619.900016 34.5037 121.8552 94.4379
+1669703620.999877 29.9519 132.9749 95.3486
+1669703621.999914 35.0479 129.6707 97.0414
+1669703623.099936 35.0578 139.8206 101.4628
+1669703624.199911 31.5762 131.5228 104.4135
+1669703625.199916 41.5142 136.8421 105.8293
+1669703626.299912 43.5541 147.6505 98.4455
+1669703627.399882 56.5395 151.2410 96.8678
+1669703628.399891 54.1300 146.0127 104.9522
+1669703629.399904 47.4705 146.2330 103.3756
+1669703630.399974 46.4738 136.7458 87.8440
+1669703631.399982 56.8921 146.1244 105.5109
+1669703632.499818 61.8987 150.5411 91.0551
+1669703633.499893 55.9992 142.0176 99.8280
+1669703634.599985 67.6189 131.8249 97.4038
+1669703635.699996 64.5272 145.4040 99.6274
+1669703636.799885 69.6347 139.4079 92.4485
+1669703637.799990 77.0323 146.6529 92.7756
+1669703638.899981 83.1451 139.1159 93.6965
+1669703639.999894 83.3695 136.5082 94.8527
+1669703641.099901 81.1895 151.4155 97.7514
+1669703642.099913 86.6496 142.5831 96.9745
+1669703643.199918 92.3685 144.4966 101.2847
+1669703644.299925 94.1148 138.4467 96.0808
+1669703645.399942 88.5442 133.6430 104.2410
+1669703646.399985 111.2954 139.8571 100.2419

data/HK_POSE/02_gnss.txt

+1669703647.399909 105.8303 156.4711 102.9426
+1669703648.499995 106.7547 136.2220 89.3013
+1669703649.599998 111.6887 137.1241 90.1838
+1669703650.699975 110.7521 135.3686 105.6814
+1669703651.799969 121.7097 140.6667 104.6122
+1669703652.899806 120.9960 136.4923 104.7886
+1669703653.899959 126.2704 135.8604 96.1965
+1669703654.899989 122.1593 134.5937 101.7750
+1669703655.999992 139.3005 135.2201 99.9641
+1669703657.099993 129.6608 140.6684 94.2179
+1669703658.199979 131.7408 144.3929 93.0654
+1669703659.299811 140.4680 145.0737 96.0737
+1669703660.299834 137.3057 143.6820 101.1081
+1669703661.399897 147.2764 143.1539 97.8966
+1669703662.399907 151.1413 141.1812 100.1849
+1669703663.499985 146.8462 154.1679 93.7342
+1669703664.599982 143.7561 145.1804 97.8990
+1669703665.699970 156.5960 150.9186 100.9702
+1669703666.799988 149.4204 157.3066 91.8355
+1669703667.899974 156.8545 160.1220 93.7313
+1669703668.999979 160.6137 167.1505 95.2869
+1669703670.099987 169.1408 158.6902 100.9554
+1669703671.199890 174.6110 154.2157 94.4552
+1669703672.299910 172.7540 167.7680 100.3985
+1669703673.399889 169.2702 171.4020 97.8656
+1669703674.499978 180.5603 174.4563 100.4585
+1669703675.599987 175.1847 172.8845 96.7178
+1669703676.599989 181.4031 175.3120 100.4500
+1669703677.699887 191.4197 184.5920 97.5482
+1669703678.699908 187.0527 181.1119 89.1892
+1669703679.699922 174.6200 176.4992 97.8195
+1669703680.799913 176.8522 196.1315 101.3702
+1669703681.899968 172.1609 192.3369 88.7462
+1669703682.999934 170.9821 191.7513 92.4194
+1669703683.999973 172.4711 189.3197 97.7435
+1669703684.999994 166.9124 198.9012 100.7551
+1669703686.099985 161.6661 185.2635 95.6360
+1669703687.199949 161.2835 183.2897 100.2752
+1669703688.199978 151.7349 189.9797 101.6228
+1669703689.199991 156.3218 185.0219 98.6487
+1669703690.299983 142.9469 185.3261 101.9320
+1669703691.399966 148.2925 182.9274 104.4271
+1669703692.499788 147.9939 189.5992 109.4888
+1669703693.499840 134.3477 181.2906 91.2294
+1669703694.499968 142.6977 190.7521 99.8709
+1669703695.499975 133.6460 181.7790 112.4036
+1669703696.599975 129.6034 187.8876 103.8493
+1669703697.699974 128.0874 188.4111 96.3137
+1669703698.699988 125.0415 196.6899 107.0309
+1669703699.799977 127.3688 184.6764 95.3797
+1669703700.899969 115.4638 187.4559 105.8184
+1669703701.999985 104.3192 194.7747 99.5892
+1669703703.099997 107.3170 182.0097 92.1331
+1669703704.199997 110.2601 187.4015 93.9884
+1669703705.299985 96.3617 185.3054 108.8089
+1669703706.399988 98.2197 179.4516 99.2718
+1669703707.499986 94.8458 173.4408 100.2643
+1669703708.599967 91.7407 190.3648 109.8267
+1669703709.599986 95.5417 185.4764 95.0746
+1669703710.699896 99.4743 187.0493 100.7040
+1669703711.699942 83.5116 187.6776 99.9440
+1669703712.799926 77.4440 183.9067 100.5366
+1669703713.899926 74.2749 183.0476 101.2661
+1669703714.999978 72.4281 194.0875 87.5193
+1669703715.999997 76.1813 192.7394 90.4391
+1669703717.099912 65.6970 184.6059 93.8005
+1669703718.199994 60.1909 180.8704 109.6272
+1669703719.299893 65.4691 192.7463 104.5109
+1669703720.299924 52.1673 183.7524 103.3819
+1669703721.299974 48.7050 190.0577 99.7845
+1669703722.399875 49.6609 190.5362 103.2795
+1669703723.399996 46.8713 193.6678 95.7358
+1669703724.499981 48.8352 192.4271 99.7217
+1669703725.599975 44.7842 185.3877 106.0376
+1669703726.699982 40.2049 191.6673 106.8658
+1669703727.799986 36.1063 190.2106 94.0596
+1669703728.899889 41.8290 194.0740 110.8724
+1669703729.999921 28.1431 212.2523 103.4116
+1669703731.099991 38.2715 204.3759 104.5671
+1669703732.199883 39.1041 215.9175 103.3772
+1669703733.199899 40.5717 211.5109 102.7243
+1669703734.199922 41.8355 207.6844 96.4657
+1669703735.299911 44.6543 220.2514 102.4785
+1669703736.399909 41.9266 224.2422 100.8667
+1669703737.399970 39.2097 226.1835 98.8344
+1669703738.499986 47.0060 219.4866 109.0540
+1669703739.499989 49.3027 231.5654 108.9567
+1669703740.599976 57.7532 237.5518 94.9672
+1669703741.599995 45.5061 231.0356 104.3776
+1669703742.699882 57.4496 232.0029 105.2529
+1669703743.699905 53.9780 233.5416 97.3219
+1669703744.799903 56.3960 230.4862 99.4994
+1669703745.799919 69.3144 232.7354 117.5286
+1669703746.899890 69.8260 238.3901 100.0344
+1669703747.899913 73.8927 234.4669 104.8938
+1669703748.899982 86.1549 236.7495 109.9865
+1669703749.900002 72.7996 236.9632 113.0487
+1669703750.900004 76.1557 246.1460 107.7010
+1669703751.999964 79.7681 240.1546 108.9790
+1669703752.999981 88.6889 229.0789 100.4388
+1669703754.099992 87.6268 236.5070 107.1409
+1669703755.199983 93.0390 230.1289 105.8855
+1669703756.299878 98.5125 239.5307 109.3473
+1669703757.299921 102.6176 238.9718 103.5673
+1669703758.299960 93.1302 238.7391 104.9202
+1669703759.399977 106.1015 230.1418 98.4380
+1669703760.399985 112.9983 236.2852 107.2150
+1669703761.499988 111.1859 236.5673 108.4579
+1669703762.599885 111.7857 236.8270 101.4210
+1669703763.599998 115.2142 234.7068 104.8093
+1669703764.699875 118.2757 242.4592 99.1778
+1669703765.799919 123.0295 233.9197 110.8292
+1669703766.799989 127.9468 241.2656 96.1249
+1669703767.799993 131.3157 240.5004 103.9246
+1669703768.899882 138.6031 233.4014 110.5260
+1669703769.899906 147.7507 234.0839 101.2171
+1669703770.899917 146.5168 243.7348 100.7883
+1669703771.999898 140.4512 234.3872 96.6392
+1669703773.099896 141.3030 230.6974 99.4687
+1669703774.199920 149.0191 236.8082 111.0186
+1669703775.299916 147.5096 240.4960 102.1667
+1669703776.399912 155.5651 238.8614 97.5827
+1669703777.499987 161.0147 236.2382 105.6189
+1669703778.599827 163.8506 247.6184 99.9302
+1669703779.599877 162.7611 232.1821 100.3028
+1669703780.599888 165.2429 241.1769 110.1381
+1669703781.699930 168.8819 230.9883 105.3513
+1669703782.699962 171.9806 238.2419 103.9665
+1669703783.699967 170.1490 242.7613 111.9017
+1669703784.799931 177.9796 233.0040 104.3045
+1669703785.900004 186.0142 238.1774 112.8911
+1669703786.999915 186.7347 230.8436 95.9139
+1669703788.099930 187.2882 234.5786 111.7370
+1669703789.199907 191.6499 235.8739 102.6567
+1669703790.199913 203.1581 247.5879 100.0655
+1669703791.299883 198.0350 240.7120 106.1273
+1669703792.399929 212.9038 239.3371 100.9481
+1669703793.399942 209.4263 243.0851 106.8878
+1669703794.399995 211.6463 244.2348 108.7107
+1669703795.499830 218.8033 243.8811 102.1324
+1669703796.599891 215.3271 248.6416 98.9705
+1669703797.599899 218.6767 242.6397 103.3038
+1669703798.599929 225.2765 247.7468 103.5197
+1669703799.699910 214.0789 253.1046 106.6817
+1669703800.699963 233.3263 250.3364 104.6955
+1669703801.699976 225.1836 262.3788 103.2824
+1669703802.799879 226.6390 260.5984 104.9127
+1669703803.799893 224.1976 261.2967 109.5437
+1669703804.799913 222.6001 263.6947 100.8343
+1669703805.899902 235.0899 258.4050 101.7250
+1669703806.999877 232.4175 276.8301 104.3624
+1669703807.999880 229.0352 281.1067 97.6080
+1669703808.999896 221.9576 276.6421 102.5321
+1669703809.999916 228.7918 280.8275 106.4284
+1669703811.099890 231.4670 274.1740 109.8014
+1669703812.199962 237.0146 283.2973 114.9424
+1669703813.299978 226.9973 279.1117 102.4554
+1669703814.299998 230.0714 290.1181 104.8709
+1669703815.399975 229.0390 284.2868 113.4500
+1669703816.499991 223.5892 296.8544 108.0010
+1669703817.499997 223.1663 294.2415 108.5552
+1669703818.599987 220.7964 297.9058 107.3082
+1669703819.699999 217.7885 290.6468 113.5573
+1669703820.799971 207.5721 299.9458 106.1823
+1669703821.899964 200.5212 291.4792 102.9957
+1669703822.899967 208.8697 287.4816 104.1984
+1669703823.900003 192.1967 288.3979 94.3304
+1669703824.999995 190.4165 294.3878 110.4079
+1669703826.099993 197.1612 285.6588 106.2692
+1669703827.199977 191.7061 284.6334 114.0596
+1669703828.299894 192.8240 289.2001 106.7260
+1669703829.299929 186.4861 280.0139 105.3961
+1669703830.399907 178.7418 285.0857 105.2553
+1669703831.499897 187.8163 293.1946 103.8235
+1669703832.599911 178.3920 296.8600 111.3424
+1669703833.699925 172.4545 286.5272 110.2396
+1669703834.799910 170.8457 294.1247 109.9676

data/HK_POSE/03_gnss.txt

+1669703835.299882 174.1260 286.9725 113.3787
+1669703836.399938 166.7499 299.9296 103.3064
+1669703837.399957 171.4547 290.4137 103.5267
+1669703838.399970 157.3337 287.6768 108.9198
+1669703839.499984 159.0915 286.2469 106.7010
+1669703840.599811 163.8900 297.6165 109.0288
+1669703841.599850 150.3797 289.4934 109.9324
+1669703842.599992 142.0651 287.4673 98.5464
+1669703843.699984 145.8608 291.8362 104.4942
+1669703844.799997 148.4297 282.3591 99.6052
+1669703845.899889 138.4009 293.5891 115.3747
+1669703846.999907 131.6859 293.6049 106.7846
+1669703847.999943 130.1136 292.4847 110.2546
+1669703848.999977 120.0928 295.2833 113.9106
+1669703849.999993 121.8658 289.7473 109.4888
+1669703851.099972 122.9892 284.8957 103.6781
+1669703852.099988 118.5518 293.4227 109.7756
+1669703853.099998 113.8082 291.3051 92.8389
+1669703854.199984 118.1104 278.6243 105.2860
+1669703855.299979 103.4976 280.7259 112.9258
+1669703856.399890 103.4695 288.8004 106.9155
+1669703857.499912 104.8685 287.2931 100.7661
+1669703858.599886 104.1732 284.3022 99.4300
+1669703859.699922 93.7428 294.1310 111.4906
+1669703860.799921 90.8469 284.7608 105.6654
+1669703861.799969 99.9628 288.2846 109.4449
+1669703862.899795 91.4091 287.4214 106.0249
+1669703863.899912 82.2639 285.7238 107.1583
+1669703864.999920 83.5684 286.1971 111.0243
+1669703865.999935 76.5281 286.1884 97.3848
+1669703867.099895 78.2396 287.3854 112.3886
+1669703868.099967 56.1835 295.9955 106.7261
+1669703869.199933 72.8879 280.2010 110.5062
+1669703870.199987 59.0732 282.1415 117.0326
+1669703871.299991 60.0781 286.2099 111.8427
+1669703872.399928 60.2024 285.7407 109.3383
+1669703873.499963 66.4439 284.5582 108.5574
+1669703874.599983 56.3856 290.2466 113.5071
+1669703875.699991 51.0384 278.8515 115.7640
+1669703876.799888 38.8184 285.9616 103.8490
+1669703877.899888 40.9182 285.9639 109.2510
+1669703878.899927 40.3630 292.2799 109.9418
+1669703879.899976 34.0430 288.9207 113.6437
+1669703880.999997 25.8383 286.9361 111.2223
+1669703882.099970 27.1919 290.6705 107.0004
+1669703883.099970 26.3150 282.7856 107.8573
+1669703884.099980 17.2034 276.3184 115.7705
+1669703885.099982 15.4459 278.5149 97.0545
+1669703886.099992 13.4909 277.0034 116.0704
+1669703887.199875 18.5138 280.7052 119.2236
+1669703888.199888 15.4802 283.2902 95.5753
+1669703889.399973 18.6303 279.1747 99.9644
+1669703890.399976 9.5751 299.6058 115.2381
+1669703891.499979 4.3867 292.0115 112.7226
+1669703892.499996 2.8676 292.1721 109.1277
+1669703893.599891 -3.7541 289.1316 107.4085
+1669703894.699901 -14.2600 294.7631 104.0607
+1669703895.799918 -13.3842 298.6733 116.1682
+1669703896.899885 -5.0477 292.6991 104.4963
+1669703897.999909 -6.4641 298.5450 108.5694
+1669703899.099947 -9.8221 301.5699 110.4692
+1669703900.199997 -20.6510 306.3594 111.7894
+1669703901.299975 -23.4116 314.4681 110.3241
+1669703902.299975 -14.0074 316.0341 117.4648
+1669703903.299990 -17.5633 315.3961 111.4323
+1669703904.399967 -16.8742 320.6955 111.2731
+1669703905.499883 -24.4150 320.1746 104.3132
+1669703906.599939 -13.1348 320.3890 108.0954
+1669703907.699965 -25.8988 334.3798 110.3191
+1669703908.799970 -15.7244 329.7844 113.3828
+1669703909.899886 -24.7758 347.0139 106.3157
+1669703910.999914 -17.0461 342.2088 114.5134
+1669703911.999925 -17.0967 340.3836 112.8194
+1669703913.099906 -13.8253 347.3439 123.7313
+1669703914.099928 -12.9186 336.5140 112.2531
+1669703915.199982 6.0517 339.9332 120.8855
+1669703916.299805 4.8397 341.6001 115.5287
+1669703917.299960 7.5426 342.8941 111.6705
+1669703918.399898 -0.2302 340.1877 109.4438
+1669703919.499910 -0.1875 345.9290 108.8433
+1669703920.499920 10.0388 338.4576 114.3492
+1669703921.599932 16.2726 336.7845 114.5651
+1669703922.599988 21.2258 341.1949 121.0676
+1669703923.699824 20.4646 344.2379 112.4150
+1669703924.699969 24.1410 348.6619 115.3841
+1669703925.799980 19.9627 349.6518 105.6250
+1669703926.899969 27.2546 344.7956 109.0190
+1669703927.999915 37.2429 355.3104 115.7558
+1669703929.099972 28.8731 348.8010 118.7739
+1669703930.199983 38.3405 362.5084 114.6768
+1669703931.299991 41.8468 349.8867 113.3772
+1669703932.399808 43.4066 355.5086 117.4852
+1669703933.399810 43.2083 345.8990 103.5149
+1669703934.399962 52.5233 347.8805 101.9627
+1669703935.499984 54.9732 367.1358 112.7507
+1669703936.599974 60.1398 356.6891 112.3733
+1669703937.599984 63.0113 356.5292 114.3086
+1669703938.699786 62.5717 356.7928 118.0502
+1669703939.699937 62.2082 358.4946 115.9057
+1669703940.799970 60.7280 364.9751 113.3155
+1669703941.899972 72.5100 369.9019 113.4073
+1669703942.899989 72.5676 377.1053 116.5039
+1669703943.999911 69.7364 366.6347 108.6135
+1669703945.099924 69.0698 373.2756 113.2548
+1669703946.199985 71.5691 382.1753 114.0893
+1669703947.299977 74.3806 383.0772 104.7947
+1669703948.399954 67.8950 385.2551 108.8635
+1669703949.399983 76.2067 396.5862 121.2317
+1669703950.499906 72.8519 399.5847 116.0917
+1669703951.599913 72.9464 392.2458 114.0434
+1669703952.699918 73.2826 399.3924 116.5977
+1669703953.699971 75.8193 403.6958 121.9543
+1669703954.699976 80.9836 396.9977 103.4363
+1669703955.799972 80.8749 400.8344 115.3282
+1669703956.899799 69.5503 401.8387 119.4320
+1669703957.899833 77.5849 410.5709 104.0193
+1669703958.899979 68.8619 417.5982 109.7657
+1669703959.999779 69.6899 417.2765 120.2106
+1669703960.999818 66.3936 413.3221 123.3931
+1669703961.999949 61.8941 418.1629 112.8035
+1669703963.099922 57.0837 413.4555 116.6173
+1669703964.199970 64.5801 410.3873 109.3936
+1669703965.299980 60.1470 415.5371 114.0923
+1669703966.399816 54.7538 412.9289 126.5602
+1669703967.399991 49.9758 412.9960 128.2203
+1669703968.499882 40.3089 418.5780 118.9298
+1669703969.599919 46.5776 422.1070 117.5925
+1669703970.699928 33.0416 416.0174 114.0350
+1669703971.799927 43.5647 413.8232 115.5912
+1669703972.799986 31.4422 405.8882 112.5469
+1669703973.899876 33.2833 408.7175 112.1547
+1669703974.899877 27.0127 417.3867 120.9826
+1669703975.899932 26.3712 407.1498 110.2673
+1669703976.999927 25.6341 408.7271 116.1162
+1669703977.999990 21.7920 412.5648 119.6442
+1669703979.099894 22.7363 405.9734 122.0045
+1669703980.199906 8.9619 409.9370 125.0198
+1669703981.199976 8.2611 410.7399 122.7365
+1669703982.299977 0.6316 403.2557 112.1294
+1669703983.399996 -10.6505 417.1213 111.8357
+1669703984.499998 -5.1721 404.8300 118.8770
+1669703985.599824 -3.2332 409.9043 114.4109
+1669703986.699902 -11.4116 406.3959 116.0742
+1669703987.799911 -5.7891 406.8339 120.9890
+1669703988.799955 -13.4704 409.6774 118.2213
+1669703989.900006 -26.4928 405.2193 114.0673
+1669703990.999884 -25.8880 405.5452 123.5072
+1669703991.999895 -26.0321 403.3866 125.7987
+1669703992.999927 -27.7685 413.7639 120.0628
+1669703993.999976 -40.3854 403.0118 119.0584
+1669703995.099995 -27.6454 400.4682 116.3388
+1669703996.199984 -42.2977 393.4629 112.5971
+1669703997.299879 -40.5830 394.5041 124.9987
+1669703998.299944 -36.6730 382.6924 118.6511
+1669703999.399896 -33.1078 382.3165 115.5193
+1669704000.499910 -38.8251 378.1366 119.1875
+1669704001.599899 -38.8465 380.9032 111.2616
+1669704002.599986 -39.5532 379.5570 113.6268
+1669704003.699987 -27.7229 370.8047 116.3571
+1669704004.799980 -30.7088 362.5865 106.1804
+1669704005.899957 -42.1260 372.8514 111.9568
+1669704006.899988 -20.8791 360.8681 115.4109
+1669704007.999891 -25.0838 370.4950 124.4945
+1669704008.999935 -25.8692 362.7350 116.9105
+1669704010.099980 -27.0282 351.6921 109.5934
+1669704011.199966 -29.8723 350.4725 111.2134
+1669704012.299978 -25.4099 352.9921 113.6922
+1669704013.299999 -20.5650 339.6364 112.0853
+1669704014.399897 -30.7117 332.8284 112.3473
+1669704015.399905 -26.8540 346.8730 115.7018
+1669704016.399913 -30.4343 345.7521 118.4548
+1669704017.499913 -21.4751 332.1673 107.2693
+1669704018.499922 -25.3844 335.9087 121.2474
+1669704019.599884 -14.8881 324.8703 105.9532
+1669704020.599904 -20.9947 315.6667 112.9721
+1669704021.699935 -21.5024 319.0479 103.1389
+1669704022.699983 -17.3038 315.8875 105.0526

data/HK_POSE/04_gnss.txt

+1669704023.499821 -34.1580 318.1095 119.7380
+1669704024.499975 -10.7439 313.2464 107.6263
+1669704025.599918 -15.6730 307.4439 111.9426
+1669704026.699906 -13.2128 315.4560 109.7128
+1669704027.799994 -17.1389 301.7683 105.8259
+1669704028.899891 -16.3294 304.8540 114.3002
+1669704029.999917 -18.2918 307.3427 108.1194
+1669704031.099921 -13.4357 292.9991 97.0953
+1669704032.199917 -7.9431 297.2916 107.5631
+1669704033.199927 -11.1657 293.3359 100.9300
+1669704034.299920 -13.2291 291.6243 102.2317
+1669704035.399901 -8.2061 277.7277 110.5725
+1669704036.499977 -8.9655 273.2456 113.7938
+1669704037.499986 -11.5677 274.4186 108.7659
+1669704038.599882 -1.1785 270.2367 107.8119
+1669704039.699899 -3.8077 275.6259 108.1354
+1669704040.799887 -6.8401 264.9095 107.4390
+1669704041.799936 -9.4867 251.6104 108.9924
+1669704042.799971 1.2597 251.5722 101.5684
+1669704043.799995 0.5985 270.8059 106.4658
+1669704044.900003 -5.2196 256.4673 112.9711
+1669704045.999912 -3.8110 250.2903 103.4832
+1669704046.999996 -4.4628 252.0509 113.8098
+1669704048.099977 2.8545 246.9985 103.9510
+1669704049.099981 -5.7814 249.7917 104.7632
+1669704050.199797 2.0864 258.2077 106.4068
+1669704051.199961 0.0659 237.2703 110.4539
+1669704052.299969 5.1594 229.0245 109.0219
+1669704053.399886 2.0552 242.4152 109.6218
+1669704054.499959 3.7885 236.2466 104.3171
+1669704055.599778 10.7997 229.0043 99.4776
+1669704056.599992 3.8403 228.8670 101.8784
+1669704057.699885 5.5063 215.7346 105.4561
+1669704058.699990 18.0925 212.1477 103.1611
+1669704059.799897 15.0442 213.8028 104.9682
+1669704060.899914 2.9222 213.0646 101.5410
+1669704061.899922 13.4055 213.8576 115.2560
+1669704062.999894 11.5376 206.2821 103.0834
+1669704064.099918 6.6790 203.0865 100.3033
+1669704065.199892 9.1940 204.7270 96.5179
+1669704066.199898 13.9605 201.7693 100.7908
+1669704067.299903 -2.5116 188.8450 107.6268
+1669704068.399889 7.8887 183.5972 98.4176
+1669704069.399916 12.0530 188.3148 99.5010
+1669704070.499935 5.8496 180.4053 95.8424
+1669704071.499982 15.5148 177.8057 106.2653
+1669704072.599983 14.7882 181.1630 97.7145
+1669704073.599993 9.1081 173.0315 95.1608
+1669704074.699988 8.5905 164.0029 105.3960
+1669704075.799975 4.5439 167.6313 100.6720
+1669704076.899987 5.5245 166.2766 96.6880
+1669704077.899987 11.0271 153.6476 99.4762
+1669704078.999983 16.1257 152.1906 95.1950
+1669704080.099779 8.6696 154.9237 100.0299
+1669704081.099838 8.4138 154.2209 103.6130
+1669704082.099974 7.8625 146.8524 101.1859
+1669704083.199974 15.2964 144.0344 92.7456
+1669704084.299967 11.2212 140.7395 86.5240
+1669704085.399985 9.7078 135.4730 96.6804
+1669704086.499878 6.9353 135.8402 100.4353
+1669704087.499881 2.8825 134.5847 95.3700
+1669704088.499925 8.5160 132.1480 91.7527
+1669704089.499933 10.1621 123.3713 100.2823
+1669704090.499988 6.5059 132.2205 94.9222
+1669704091.599974 2.8174 119.6327 94.2837
+1669704092.599985 8.4290 108.6271 90.3001
+1669704093.699892 -1.0316 118.1769 103.8853
+1669704094.799912 3.6284 109.3165 95.4424
+1669704095.799965 9.7614 102.6156 96.0430
+1669704096.900001 8.6433 106.5069 95.5484
+1669704097.999914 8.6432 87.2030 97.7568
+1669704099.099892 0.1787 88.3192 96.6873
+1669704100.099898 10.1459 93.8283 98.6869
+1669704101.099904 11.2031 98.1010 101.8918
+1669704102.099922 6.7007 88.9222 85.4660
+1669704103.199906 1.6297 81.0602 104.0775
+1669704104.299933 1.8177 82.6434 95.7420
+1669704105.399957 2.7759 87.2229 89.6263
+1669704106.399989 3.3799 72.0795 85.9221
+1669704107.499990 -0.8080 72.3488 90.1293
+1669704108.599825 -1.3932 62.2214 91.6237
+1669704109.599983 -5.6161 70.0954 92.2344
+1669704110.699884 -5.5178 61.5087 87.8171
+1669704111.699944 3.2101 66.7972 81.9877
+1669704112.699973 2.0307 56.3108 94.5068
+1669704113.799818 2.3504 50.0602 89.3139
+1669704114.799965 -2.8217 56.2180 90.7839
+1669704115.799983 10.5421 47.5660 86.9266
+1669704116.899908 -6.6963 35.5973 82.3320
+1669704117.899921 2.1101 44.1786 82.1283
+1669704118.999926 5.0895 35.9512 90.5338
+1669704120.099928 8.4910 39.5947 88.2258
+1669704121.199882 -4.2073 21.0911 85.7213
+1669704122.199903 -0.9516 23.9363 88.4364
+1669704123.199977 0.3195 24.7588 85.6183
+1669704124.199993 0.6463 25.1368 83.2049
+1669704125.299969 6.6957 19.1794 88.5303
+1669704126.399793 3.0151 17.5658 92.9641
+1669704127.399971 0.7485 10.3726 89.7856
+1669704128.499964 -6.5741 10.6977 87.5606
+1669704129.499984 -2.0351 6.5313 78.0843
+1669704130.599986 -3.8893 4.3822 88.0541
+1669704131.699991 -6.4414 6.9861 82.6860
+1669704132.799996 3.3687 -0.4725 83.5357
+1669704133.899886 7.2474 -11.7134 91.1186
+1669704134.899912 3.3594 -9.9974 93.3935
+1669704135.999900 -2.8972 -5.4074 87.7129
+1669704136.999923 -2.1446 -2.5891 90.4127
+1669704137.999927 -10.2467 0.0882 78.1684
+1669704138.999987 3.5404 -0.6841 81.0404
+1669704140.099999 -2.8843 -11.1551 85.4608
+1669704141.199990 -1.6431 -1.2107 93.1285
+1669704142.299954 7.2408 -7.6990 85.5442
+1669704143.399928 -8.0869 -8.8033 91.4124
+1669704144.499901 8.4423 -10.9301 83.6288
+1669704145.499919 0.1797 -3.3900 82.2938
+1669704146.599906 2.3801 -7.0541 84.8868
+1669704147.599909 3.0010 -4.4853 80.4627
+1669704148.699991 -0.2734 -4.8751 77.5046
+1669704149.799977 -1.0895 3.7365 78.8449
+1669704150.899969 -1.3551 -0.5735 71.0786
+1669704151.999887 5.8954 -1.9114 68.9439
+1669704152.999944 2.2708 -11.3427 60.8548
+1669704154.099885 -6.0097 -2.4924 71.9043
+1669704155.199925 11.0861 0.2123 62.4241
+1669704156.199981 -9.0988 -5.4982 53.0354
+1669704157.199991 -3.2736 -3.3684 51.6047
+1669704158.299977 -10.1014 1.4091 45.3439
+1669704159.399888 0.9830 8.3064 42.8291
+1669704160.399900 -12.9143 -3.2603 49.9383
+1669704161.399918 9.4961 -7.7268 43.9102
+1669704162.399926 -2.7031 -1.9436 51.6819
+1669704163.499898 -7.8963 0.0633 37.5282
+1669704164.599892 4.9835 -0.4204 33.7081
+1669704165.699929 -2.4450 0.5788 27.7947
+1669704166.799912 2.5982 -2.3247 28.9799
+1669704167.899932 -0.6672 5.3907 24.7331
+1669704168.999940 -9.4136 6.1796 18.5291
+1669704170.099931 -3.8787 -3.0466 28.2070
+1669704171.199917 -11.3258 15.7112 25.0779
+1669704172.299909 0.4558 -0.1197 19.8604
+1669704173.399909 1.9173 3.4761 19.9738
+1669704174.499896 -0.2674 -0.6546 15.3154
+1669704175.499909 -1.7141 6.0871 18.0491
+1669704176.499924 -15.4609 3.7794 15.0610
+1669704177.499967 -0.8867 -1.3465 18.7182
+1669704178.599904 3.9325 -3.1930 14.0383
+1669704179.599970 -5.6607 -6.1655 7.8813
+1669704180.699979 3.5024 5.5757 12.4891
+1669704181.799986 2.8124 3.4692 5.6318
+1669704182.799991 1.3434 4.9718 11.6083
+1669704183.899905 5.0698 4.6030 8.5048
+1669704184.899916 4.4062 0.9728 1.3279
+1669704185.999935 -1.7955 -2.5589 1.2608
+1669704186.999973 -1.6776 -2.7274 2.0793
+1669704188.099875 -10.3963 2.3430 2.5292
+1669704189.099928 -5.0301 4.6539 6.2179
+1669704190.199918 -9.0022 -2.4078 -3.4269
+1669704191.199985 -5.7001 5.6395 -1.5384
+1669704192.299878 -2.3775 -8.3544 1.5156

data/HK_POSE/simulated_gnss.json

+{
+  "block_1": {
+    "origin_xyz": [
+      2.4993797650561635,
+      -0.6846325058559233,
+      3.2355736905034624
+    ],
+    "centroid_xyz": [
+      15.829788763082405,
+      37.26244717552509,
+      69.92080353871927
+    ],
+    "gnss_file": "01_gnss.txt",
+    "pose_file": "01_fixed.txt"
+  },
+  "block_2": {
+    "origin_xyz": [
+      105.83028777271768,
+      156.4710830422762,
+      102.94255855717289
+    ],
+    "centroid_xyz": [
+      139.1792727802072,
+      217.27496360342877,
+      102.41166299309933
+    ],
+    "gnss_file": "02_gnss.txt",
+    "pose_file": "02_fixed.txt"
+  },
+  "block_3": {
+    "origin_xyz": [
+      174.12603632245285,
+      286.9725421758249,
+      113.37868934096325
+    ],
+    "centroid_xyz": [
+      35.35315766439178,
+      343.45354686262317,
+      112.31677216823138
+    ],
+    "gnss_file": "03_gnss.txt",
+    "pose_file": "03_fixed.txt"
+  },
+  "block_4": {
+    "origin_xyz": [
+      -34.158001026935196,
+      318.1094585373276,
+      119.73798859845078
+    ],
+    "centroid_xyz": [
+      0.6193251567352909,
+      102.52960493102732,
+      79.28539667256473
+    ],
+    "gnss_file": "04_gnss.txt",
+    "pose_file": "04_fixed.txt"
+  }
+}
\ No newline at end of file

environment.yaml

+name: mapping_fusion
+channels:
+  - conda-forge
+  - defaults
+dependencies:
+  - python=3.10
+  - pip
+  - numpy
+  - matplotlib
+  - jupyterlab
+  - ipykernel
+  - gtsam
+  - pip:
+    - open3d
+    - scipy
+    - tqdm
\ No newline at end of file

scripts/align_trajectory_svd.py

+#!/usr/bin/env python3
+import argparse
+import numpy as np
+import open3d as o3d
+from pathlib import Path
+from scipy.spatial.transform import Rotation as R
+
+def load_pose_txt(path):
+    """
+    Load pose txt file. 
+    Format: timestamp x y z [qx qy qz qw]
+    Returns: timestamps, positions (N, 3)
+    """
+    try:
+        data = np.loadtxt(path)
+    except Exception as e:
+        raise ValueError(f"Failed to load {path}: {e}")
+        
+    if data.ndim == 1:
+        data = data.reshape(1, -1)
+        
+    timestamps = data[:, 0]
+    positions = data[:, 1:4]
+    return timestamps, positions
+
+def interp_positions(target_times, src_times, src_positions):
+    """
+    Linear interpolation of positions at target_times.
+    Assumes times are sorted.
+    """
+    # Use numpy interp for each axis
+    x_interp = np.interp(target_times, src_times, src_positions[:, 0])
+    y_interp = np.interp(target_times, src_times, src_positions[:, 1])
+    z_interp = np.interp(target_times, src_times, src_positions[:, 2])
+    return np.stack([x_interp, y_interp, z_interp], axis=1)
+
+def compute_initial_alignment_svd(local_poses, global_gnss):
+    """
+    Compute T_local_to_global using SV (Kabsch Algorithm).
+    local_poses: (N, 3) array in Local Frame
+    global_gnss: (N, 3) array in Global Frame
+    """
+    # 1. Data Alignment
+    n = min(len(local_poses), len(global_gnss))
+    local_p = local_poses[:n]
+    global_p = global_gnss[:n]
+    
+    if n < 3:
+        raise ValueError("Need at least 3 points for SVD alignment.")
+
+    # 2. Centroids
+    centroid_local = np.mean(local_p, axis=0)
+    centroid_global = np.mean(global_p, axis=0)
+    
+    # 3. Center Data
+    A = local_p - centroid_local
+    B = global_p - centroid_global
+    
+    # 4. Covariance Matrix
+    H = np.dot(A.T, B)
+    
+    # 5. SVD
+    U, S, Vt = np.linalg.svd(H)
+    R_mat = np.dot(Vt.T, U.T)
+    
+    # 6. Reflection Case (Fix for determinant < 0)
+    if np.linalg.det(R_mat) < 0:
+        print("[Info] Reflection detected, fixing rotation matrix...")
+        Vt[2, :] *= -1
+        R_mat = np.dot(Vt.T, U.T)
+        
+    # 7. Translation
+    t_vec = centroid_global - np.dot(R_mat, centroid_local)
+    
+    # 8. Assemble T
+    T_init = np.eye(4)
+    T_init[:3, :3] = R_mat
+    T_init[:3, 3] = t_vec
+    
+    return T_init
+
+def main():
+    parser = argparse.ArgumentParser(description="Traj Alignment SVD: Align PCD using Local Pose and Global GNSS trajectory.")
+    parser.add_argument("--local-pose", required=True, help="Path to Local Pose TXT (High Freq)")
+    parser.add_argument("--global-gnss", required=True, help="Path to Global GNSS TXT (Low Freq)")
+    parser.add_argument("--pcd-in", required=True, help="Input PCD file path")
+    parser.add_argument("--pcd-out", required=True, help="Output PCD file path")
+    
+    args = parser.parse_args()
+    
+    # 1. Load Trajectories
+    print(f"Loading Local Poses: {args.local_pose}")
+    local_ts, local_xyz = load_pose_txt(args.local_pose)
+    
+    print(f"Loading Global GNSS: {args.global_gnss}")
+    gnss_ts, gnss_xyz = load_pose_txt(args.global_gnss)
+    
+    # 2. Sync Data (Interpolate Local Poses to GNSS times)
+    print("Synchronizing Trajectories...")
+    # Only use GNSS times that are within Local Pose time range to avoid extrapolation errors
+    valid_mask = (gnss_ts >= local_ts.min()) & (gnss_ts <= local_ts.max())
+    valid_gnss_ts = gnss_ts[valid_mask]
+    valid_gnss_xyz = gnss_xyz[valid_mask]
+    
+    if len(valid_gnss_ts) < 10:
+        print(f"Warning: Only {len(valid_gnss_ts)} overlapping points found. Alignment might be unstable.")
+    
+    local_xyz_interp = interp_positions(valid_gnss_ts, local_ts, local_xyz)
+    
+    # 3. Compute Transform
+    print(f"Computing Alignment with {len(local_xyz_interp)} points...")
+    T = compute_initial_alignment_svd(local_xyz_interp, valid_gnss_xyz)
+    
+    print("\nCalculated Transform (Local -> Global):")
+    print(T)
+    
+    # Calculate Yaw rotation for verification
+    yaw_deg = np.degrees(np.arctan2(T[1, 0], T[0, 0]))
+    print(f"\nEstimated Yaw Rotation: {yaw_deg:.2f} degrees")
+    print(f"Estimated Translation: {T[:3, 3]}")
+
+    # 4. Apply to PCD
+    print(f"\nTransforming PCD: {args.pcd_in} -> {args.pcd_out}")
+    if not Path(args.pcd_in).exists():
+        print(f"Error: Input PCD {args.pcd_in} not found.")
+        return
+
+    pcd = o3d.io.read_point_cloud(args.pcd_in)
+    if pcd.is_empty():
+        print("Error: Empty PCD file.")
+        return
+        
+    pcd.transform(T)
+    
+    Path(args.pcd_out).parent.mkdir(parents=True, exist_ok=True)
+    success = o3d.io.write_point_cloud(args.pcd_out, pcd)
+    
+    if success:
+        print("Done. Saved transformed PCD.")
+    else:
+        print("Error: Failed to write output PCD.")
+
+if __name__ == "__main__":
+    main()

scripts/fix_pcd_coords.py

+#!/usr/bin/env python3
+import argparse
+import os
+from pathlib import Path
+import numpy as np
+import open3d as o3d
+
+def parse_mapping(mapping_str: str):
+    """
+    解析映射字符串，例如 'z,y,-x'
+    返回一个变换矩阵或映射逻辑
+    """
+    mapping_str = mapping_str.lower().replace(" ", "")
+    parts = mapping_str.split(",")
+    if len(parts) != 3:
+        raise ValueError("映射关系必须包含三个部分，用逗号分隔，例如 'z,y,-x'")
+    
+    # 构造变换矩阵
+    # [new_x, new_y, new_z, 1]^T = T * [x, y, z, 1]^T
+    transform = np.zeros((4, 4))
+    transform[3, 3] = 1.0
+    
+    axis_map = {'x': 0, 'y': 1, 'z': 2}
+    
+    for i, part in enumerate(parts):
+        sign = 1.0
+        if part.startswith('-'):
+            sign = -1.0
+            axis = part[1:]
+        elif part.startswith('+'):
+            axis = part[1:]
+        else:
+            axis = part
+            
+        if axis not in axis_map:
+            raise ValueError(f"无效的轴名称: {axis}. 必须是 x, y, 或 z")
+            
+        transform[i, axis_map[axis]] = sign
+        
+    return transform
+
+def process_pcd(file_path, transform, output_path):
+    print(f"正在处理: {file_path.name} ...", end="\r")
+    pcd = o3d.io.read_point_cloud(str(file_path))
+    if pcd.is_empty():
+        print(f"\n[跳过] {file_path.name} 为空或无法读取")
+        return False
+    
+    # 应用变换
+    pcd.transform(transform)
+    
+    # 保存
+    success = o3d.io.write_point_cloud(str(output_path), pcd)
+    return success
+
+def main():
+    parser = argparse.ArgumentParser(description="PCD 坐标修复工具 - 批量转换坐标轴")
+    parser.add_argument("input_dir", help="输入 PCD 文件夹路径")
+    parser.add_argument("--map", required=True, help="坐标映射关系，例如 'z,y,-x' (新X=原Z, 新Y=原Y, 新Z=原-X)")
+    parser.add_argument("--out_dir", help="输出文件夹路径（如果不指定，将直接覆盖原文件）")
+    parser.add_argument("--ext", default=".pcd", help="要处理的文件后缀 (默认: .pcd)")
+
+    args = parser.parse_args()
+    
+    input_path = Path(args.input_dir)
+    if not input_path.is_dir():
+        print(f"错误: {args.input_dir} 不是一个有效的目录")
+        return
+
+    try:
+        transform_matrix = parse_mapping(args.map)
+        print("解析到的变换矩阵:")
+        print(transform_matrix[:3, :3])
+    except ValueError as e:
+        print(f"映射解析错误: {e}")
+        return
+
+    output_dir = Path(args.out_dir) if args.out_dir else input_path
+    output_dir.mkdir(parents=True, exist_ok=True)
+
+    pcd_files = list(input_path.glob(f"*{args.ext}"))
+    if not pcd_files:
+        print(f"在 {args.input_dir} 中没有找到 {args.ext} 文件")
+        return
+
+    print(f"找到 {len(pcd_files)} 个文件，准备开始处理...")
+    
+    count = 0
+    for pcd_file in pcd_files:
+        out_file = output_dir / pcd_file.name
+        if process_pcd(pcd_file, transform_matrix, out_file):
+            count += 1
+            
+    print(f"\n完成！成功处理并保存了 {count} 个文件。")
+
+if __name__ == "__main__":
+    main()

scripts/fix_pose_coords.py

+#!/usr/bin/env python3
+import argparse
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+import sys
+
+def parse_map_string(map_str):
+    """
+    解析映射字符串，例如 "y,z,-x" -> 3x3 变换矩阵
+    """
+    mapping = map_str.strip().split(',')
+    if len(mapping) != 3:
+        raise ValueError("Map string must have 3 components, e.g., 'y,z,-x'")
+    
+    axis_map = {'x': 0, 'y': 1, 'z': 2}
+    matrix = np.zeros((3, 3))
+    
+    for i, axis_str in enumerate(mapping):
+        axis_str = axis_str.strip().lower()
+        sign = 1
+        if axis_str.startswith('-'):
+            sign = -1
+            axis_str = axis_str[1:]
+        
+        if axis_str.startswith('+'):
+            axis_str = axis_str[1:]
+        
+        if axis_str not in axis_map:
+            raise ValueError(f"Unknown axis: {axis_str}")
+            
+        src_idx = axis_map[axis_str]
+        matrix[i, src_idx] = sign
+        
+    return matrix
+
+def transform_poses(pose_data, map_str):
+    """
+    对 Pose 数据 (t, x, y, z, qx, qy, qz, qw) 应用坐标变换
+    """
+    if pose_data.shape[1] < 8:
+        raise ValueError(f"Input data must have at least 8 columns (t, x, y, z, qx, qy, qz, qw), got {pose_data.shape[1]}")
+
+    # 1. 解析变换矩阵
+    trans_mat = parse_map_string(map_str)
+    det = np.linalg.det(trans_mat)
+    
+    print(f"[Info] Transformation Matrix for '{map_str}':")
+    print(trans_mat)
+    print(f"[Info] Determinant: {det:.2f}")
+    
+    is_valid_rotation = np.isclose(det, 1.0)
+    is_reflection = np.isclose(det, -1.0)
+
+    # 2. 变换位置 (Position)
+    # 原始位置: columns 1, 2, 3
+    positions = pose_data[:, 1:4]
+    # P_new = R * P_old (注意转置处理：(matrix @ vectors.T).T)
+    positions_new = (trans_mat @ positions.T).T
+    
+    # 3. 变换姿态 (Orientation)
+    quats = pose_data[:, 4:8] # (qx, qy, qz, qw)
+    quats_new = quats.copy()
+    
+    if is_valid_rotation:
+        print("[Status] Valid rotation detected. Transforming quaternions...")
+        # 将四元数转为旋转矩阵 R_old
+        r_old = R.from_quat(quats).as_matrix()
+        # R_new = T * R_old
+        # scipy Rotations expect [N, 3, 3] usually, but let's handle batch properly
+        # matmul auto broadcasts if shapes align properly, but here T is (3,3) and r_old is (N,3,3)
+        # We want R_new[i] = T @ R_old[i]
+        r_new = np.matmul(trans_mat, r_old)
+        
+        # 转回四元数
+        quats_new = R.from_matrix(r_new).as_quat()
+        
+    elif is_reflection:
+        print("\n[WARNING] ⚠️ 你的变换包含镜像 (Determinant = -1)！")
+        print("          四元数无法表示镜像坐标系。")
+        print("          位置 (XYZ) 已变换，但姿态可能在可视化中显示错误（由内向外翻转）。")
+        print("          建议: 尝试修改 map 字符串为合法的旋转，例如 'z, y, -x' 或 'z, -y, -x'")
+        
+        # 对于镜像，旋转部分如果不处理，会导致轨迹虽然位置对了，但朝向看起来很怪
+        # 这里仅保留原始四元数或者不做特定处理，单纯变换位置是极其常见的 "Fix" 方式
+        pass 
+    else:
+        print("[Error] Transformation is not a standard orthogonal basis change.")
+
+    # 4. 组合结果
+    # 保持时间戳不变
+    new_data = np.hstack((pose_data[:, 0:1], positions_new, quats_new))
+    return new_data
+
+def main():
+    parser = argparse.ArgumentParser(description="Pose Coordinate Fixer - Transforms TUM format poses.")
+    parser.add_argument("input_file", help="Path to input pose file (TUM format: t x y z qx qy qz qw)")
+    parser.add_argument("output_file", help="Path to output pose file")
+    parser.add_argument("--map", required=True, help="Mapping string, e.g., 'y,z,-x' or 'z,y,-x'")
+
+    args = parser.parse_args()
+
+    # 读取数据
+    try:
+        print(f"Reading {args.input_file}...")
+        data = np.loadtxt(args.input_file)
+    except Exception as e:
+        print(f"Error reading input file: {e}")
+        return
+
+    if data.ndim == 1:
+        data = data.reshape(1, -1)
+
+    if data.shape[0] == 0:
+        print("Error: Input file is empty.")
+        return
+
+    # 执行变换
+    try:
+        new_data = transform_poses(data, args.map)
+    except ValueError as e:
+        print(f"Error: {e}")
+        return
+
+    # 保存结果
+    print(f"Saving to {args.output_file}...")
+    # 格式化: timestamp keep 6 decimal, others 6-9 is enough usually.
+    # %.6f for all assumes time is in seconds.
+    np.savetxt(args.output_file, new_data, fmt='%.6f')
+    print("Done.")
+
+if __name__ == "__main__":
+    main()

scripts/preprocess_pcds.py

+#!/usr/bin/env python3
+from __future__ import annotations
+
+import argparse
+import re
+from pathlib import Path
+from typing import Iterable, List, Tuple
+
+import numpy as np
+import open3d as o3d
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description="Preprocess raw PCDs by downsampling and persist them for the optimizer."
+    )
+    parser.add_argument(
+        "--pcd-dir",
+        default="data/HK_PCD",
+        help="Input raw PCD folder (default: data/HK_PCD).",
+    )
+    parser.add_argument(
+        "--out-dir",
+        default="data/HK_PCD_DS",
+        help="Output folder for processed PCDs (default: data/HK_PCD_DS).",
+    )
+    parser.add_argument(
+        "--pattern",
+        default="*_all_raw_points_*.pcd",
+        help="Glob pattern for input PCDs (default: *_all_raw_points_*.pcd).",
+    )
+    parser.add_argument(
+        "--voxel-size",
+        type=float,
+        default=0.5,
+        help="Voxel size for voxel_down_sample in meters (default: 0.5).",
+    )
+    parser.add_argument(
+        "--uniform-every-k",
+        type=int,
+        default=1,
+        help="Uniform downsample every k points, 1 disables (default: 1).",
+    )
+    parser.add_argument(
+        "--overwrite",
+        action="store_true",
+        help="Overwrite existing processed PCDs.",
+    )
+    return parser.parse_args()
+
+
+def sort_pcd_paths(paths: Iterable[Path]) -> List[Path]:
+    pat = re.compile(r"(\d+)\.pcd$", re.IGNORECASE)
+
+    def key(path: Path) -> int:
+        match = pat.search(path.name)
+        if match:
+            return int(match.group(1))
+        return 1_000_000
+
+    return sorted(paths, key=key)
+
+
+def preprocess_one(
+    input_path: Path,
+    output_path: Path,
+    voxel_size: float,
+    uniform_every_k: int,
+) -> Tuple[int, int, int]:
+    pcd = o3d.io.read_point_cloud(str(input_path))
+    n_raw = int(np.asarray(pcd.points).shape[0])
+
+    if uniform_every_k > 1:
+        pcd = pcd.uniform_down_sample(every_k_points=uniform_every_k)
+    n_uniform = int(np.asarray(pcd.points).shape[0])
+
+    pcd = pcd.voxel_down_sample(voxel_size=voxel_size)
+    n_ds = int(np.asarray(pcd.points).shape[0])
+
+    output_path.parent.mkdir(parents=True, exist_ok=True)
+    ok = o3d.io.write_point_cloud(str(output_path), pcd)
+    if not ok:
+        raise RuntimeError(f"Failed to write processed PCD: {output_path}")
+
+    return n_raw, n_uniform, n_ds
+
+
+def main() -> None:
+    args = parse_args()
+    pcd_dir = Path(args.pcd_dir)
+    out_dir = Path(args.out_dir)
+
+    if args.voxel_size <= 0.0:
+        raise ValueError("voxel_size must be > 0")
+    if args.uniform_every_k < 1:
+        raise ValueError("uniform_every_k must be >= 1")
+    if not pcd_dir.exists():
+        raise FileNotFoundError(f"Input PCD folder not found: {pcd_dir}")
+
+    pcd_paths = sort_pcd_paths(pcd_dir.glob(args.pattern))
+    if not pcd_paths:
+        raise FileNotFoundError(f"No PCDs matched {args.pattern} under {pcd_dir}")
+
+    out_dir.mkdir(parents=True, exist_ok=True)
+    print(f"[Input ] {pcd_dir} ({len(pcd_paths)} files)")
+    print(f"[Output] {out_dir}")
+    print(f"[Config] voxel_size={args.voxel_size}, uniform_every_k={args.uniform_every_k}")
+
+    processed = 0
+    skipped = 0
+    for path in pcd_paths:
+        out_path = out_dir / path.name
+        if out_path.exists() and not args.overwrite:
+            skipped += 1
+            print(f"[Skip ] {path.name} already exists.")
+            continue
+
+        n_raw, n_uniform, n_ds = preprocess_one(
+            input_path=path,
+            output_path=out_path,
+            voxel_size=args.voxel_size,
+            uniform_every_k=args.uniform_every_k,
+        )
+        processed += 1
+        print(
+            f"[Done ] {path.name}: raw={n_raw} -> uniform={n_uniform} -> voxel={n_ds}"
+        )
+
+    print(f"[Summary] processed={processed}, skipped={skipped}")
+
+
+if __name__ == "__main__":
+    main()

scripts/preview_raw_map.py

+#!/usr/bin/env python3
+"""
+scripts/preview_raw_map.py
+
+快速预览工具：仅使用 simulated_gnss.json 中的 origin_xyz 对 PCD 进行简单平移拼接。
+不进行任何旋转配准，也不进行 ICP 优化。
+用于检查 GNSS 坐标是否大体正确，以及 PCD 加载是否正常。
+
+Usage:
+  python scripts/preview_raw_map.py --pcd_dir data/HK_PCD_DS --out results/raw_preview.pcd
+"""
+
+import argparse
+import json
+import re
+import numpy as np
+import open3d as o3d
+from pathlib import Path
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--pcd_dir", type=str, default="data/HK_PCD")
+    parser.add_argument("--json", type=str, default="data/HK_POSE/simulated_gnss.json")
+    parser.add_argument("--out", type=str, default="results/raw_preview.pcd")
+    parser.add_argument("--voxel", type=float, default=0.5, help="Downsample voxel size for preview")
+    args = parser.parse_args()
+
+    json_path = Path(args.json)
+    if not json_path.exists():
+        print(f"Error: JSON not found {json_path}")
+        return
+
+    with open(json_path, "r", encoding="utf-8") as f:
+        info = json.load(f)
+
+    # Sort blocks
+    block_ids = sorted(info.keys(), key=lambda x: int(x.split("_")[-1]))
+    print(f"Found blocks: {block_ids}")
+
+    # Map PCDs
+    pcd_dir = Path(args.pcd_dir)
+    pcd_files = list(pcd_dir.glob("*.pcd"))
+    
+    merged = o3d.geometry.PointCloud()
+
+    print(f"\nProcessing {len(block_ids)} blocks...")
+    
+    for bid in block_ids:
+        # 1. Get Origin (Translation)
+        origin = info[bid].get("origin_xyz", [0,0,0])
+        t = np.array(origin, dtype=np.float64)
+        
+        # 2. Find PCD
+        # Match ..._01.pcd -> block_1
+        idx = int(bid.split("_")[-1])
+        found_pcd = None
+        for p in pcd_files:
+            m = re.search(r"(\d+)\.pcd$", p.name)
+            if m and int(m.group(1)) == idx:
+                found_pcd = p
+                break
+        
+        if not found_pcd:
+            print(f"  [Warn] PCD not found for {bid} in {pcd_dir}")
+            continue
+
+        # 3. Load & Transform
+        print(f"  [Load] {bid} -> {found_pcd.name} | Origin={t}")
+        pcd = o3d.io.read_point_cloud(str(found_pcd))
+        
+        if pcd.is_empty():
+            print("    -> Empty PCD, skip.")
+            continue
+            
+        # Optional: pre-downsample to save memory
+        if args.voxel > 0:
+            pcd = pcd.voxel_down_sample(args.voxel)
+
+        # Apply pure translation T = [I | t]
+        # Assuming original PCDs are local (starts near 0,0,0 relative to their own start)
+        pcd.translate(t)
+        
+        # Colorize (random color per block for visualization)
+        # color = np.random.rand(3)
+        # pcd.paint_uniform_color(color)
+        
+        merged += pcd
+
+    # Save
+    out_path = Path(args.out)
+    out_path.parent.mkdir(parents=True, exist_ok=True)
+    
+    print(f"\nSaving merged preview to {out_path} ...")
+    o3d.io.write_point_cloud(str(out_path), merged)
+    print(f"Done. Total points: {len(merged.points)}")
+
+if __name__ == "__main__":
+    main()

src/global_optimizer.py

+#!/usr/bin/env python3
+"""
+global_optimizer.py
+
+大规模子图（Submaps / Blocks）分块融合 + 全局优化核心脚本 (Refactored & Modularized & Timed)
+
+Usage:
+  python src/global_optimizer.py --pcd_dir data/HK_PCD_DS --max_stage 5
+"""
+
+import argparse
+from pathlib import Path
+
+# Import Modules
+from optimizer_modules.common import Config, OptimizerContext, TimingLogger
+from optimizer_modules.loader import run_module0_metadata, run_module1_load_pcd
+from optimizer_modules.matching import (
+    run_module2_svd_coarse_alignment,
+    run_module3a_distance_gating,
+    run_module3b_overlap_check,
+    run_module4_icp
+)
+from optimizer_modules.optimization import run_module5_gtsam
+from optimizer_modules.exporter import save_pose_table_and_preview, export_global_map
+
+def parse_args() -> argparse.Namespace:
+    ap = argparse.ArgumentParser()
+    ap.add_argument("--pcd_dir", type=str, default="data/HK_PCD_DS")
+    ap.add_argument("--pose_root_dir", type=str, default="data/HK_POSE")
+    ap.add_argument("--gnss_json", type=str, default="data/HK_POSE/simulated_gnss.json")
+    ap.add_argument("--out_dir", type=str, default="results")
+    ap.add_argument("--max_stage", type=int, default=5, choices=[1, 2, 3, 4, 5])
+
+    # SVD
+    ap.add_argument("--min_svd_pairs", type=int, default=20)
+    
+    # Overlap
+    ap.add_argument("--gating_distance_m", type=float, default=500.0)
+    ap.add_argument("--overlap_ratio_threshold", type=float, default=0.01)
+
+    # ICP
+    ap.add_argument("--icp_rmse_threshold", type=float, default=0.5)
+
+    # GTSAM
+    ap.add_argument("--gnss_sigma_xyz", type=float, default=50.0)
+
+    return ap.parse_args()
+
+def main():
+    args = parse_args()
+    
+    # Init Config & Context
+    cfg = Config()
+    cfg.pcd_dir = Path(args.pcd_dir)
+    cfg.pose_root_dir = Path(args.pose_root_dir)
+    cfg.gnss_json = Path(args.gnss_json)
+    cfg.out_dir = Path(args.out_dir)
+    cfg.max_stage = args.max_stage
+    
+    # Override
+    cfg.min_svd_pairs = args.min_svd_pairs
+    cfg.gating_distance_m = args.gating_distance_m
+    cfg.overlap_ratio_threshold = args.overlap_ratio_threshold
+    cfg.icp_rmse_threshold = args.icp_rmse_threshold
+    cfg.gnss_sigma_xyz = args.gnss_sigma_xyz
+
+    ctx = OptimizerContext()
+    timer = TimingLogger()
+
+    print("\n================ Global Optimizer (Modular) ================")
+    print(f"[Run] max_stage      = {cfg.max_stage}")
+    print("==========================================================\n")
+
+    # Module 0
+    timer.start("Module 0: Load Metadata")
+    run_module0_metadata(cfg, ctx)
+    timer.stop("Module 0: Load Metadata", count=1, unit="step")
+
+    # Module 1
+    if cfg.max_stage >= 1:
+        timer.start("Module 1: Load PCD")
+        run_module1_load_pcd(cfg, ctx)
+        timer.stop("Module 1: Load PCD", count=len(ctx.block_ids), unit="block")
+
+    # Module 2
+    if cfg.max_stage >= 2:
+        timer.start("Module 2: SVD Alignment")
+        run_module2_svd_coarse_alignment(cfg, ctx)
+        timer.stop("Module 2: SVD Alignment", count=len(ctx.block_ids), unit="block")
+        
+        timer.start("Aux: Save Init Preview")
+        save_pose_table_and_preview(cfg, ctx, mode="initial")
+        timer.stop("Aux: Save Init Preview", count=1, unit="step")
+
+    # Module 3
+    if cfg.max_stage >= 3:
+        timer.start("Module 3a: Dist Gating")
+        run_module3a_distance_gating(cfg, ctx)
+        timer.stop("Module 3a: Dist Gating", count=1, unit="all_pairs")
+
+        timer.start("Module 3b: Overlap Check")
+        run_module3b_overlap_check(cfg, ctx)
+        timer.stop("Module 3b: Overlap Check", count=len(ctx.pairs_gated), unit="pair")
+
+    # Module 4
+    if cfg.max_stage >= 4:
+        timer.start("Module 4: ICP")
+        run_module4_icp(cfg, ctx)
+        timer.stop("Module 4: ICP", count=len(ctx.candidate_edges), unit="edge")
+
+    # Module 5
+    if cfg.max_stage >= 5:
+        timer.start("Module 5: GTSAM Opt")
+        run_module5_gtsam(cfg, ctx)
+        timer.stop("Module 5: GTSAM Opt", count=1, unit="graph")
+
+        timer.start("Aux: Export Map")
+        save_pose_table_and_preview(cfg, ctx, mode="optimized")
+        export_global_map(cfg, ctx, cfg.out_dir / "Global_Map_Optimized.pcd")
+        timer.stop("Aux: Export Map", count=len(ctx.block_ids), unit="block")
+
+    # Save Timing Report
+    timer.save_report(cfg.out_dir / "timing_log.txt")
+
+if __name__ == "__main__":
+    main()

src/optimizer_modules/common.py

+from __future__ import annotations
+import numpy as np
+import open3d as o3d
+from dataclasses import dataclass, field
+from pathlib import Path
+from typing import Dict, List, Tuple, Optional
+import math
+import time
+
+class TimingLogger:
+    def __init__(self):
+        self.records = [] # list of dict
+        self.start_times = {}
+        self.global_start = time.time()
+    
+    def start(self, name):
+        self.start_times[name] = time.time()
+        
+    def stop(self, name, count=1, unit="block"):
+        if name not in self.start_times: return
+        duration = time.time() - self.start_times[name]
+        self.records.append({
+            "name": name, 
+            "duration": duration, 
+            "count": count, 
+            "unit": unit
+        })
+        return duration
+
+    def save_report(self, path: Path):
+        total_time = time.time() - self.global_start
+        lines = [
+            "================ Performance Report ================",
+            f"Total Execution Time: {total_time:.4f} sec",
+            "----------------------------------------------------",
+            f"{'Stage Name':<35} | {'Total(s)':<10} | {'Avg(ms)':<10} | {'Count':<5} {'Unit'}",
+            "----------------------------------------------------"
+        ]
+        
+        for r in self.records:
+            avg_ms = (r['duration'] / max(1, r['count'])) * 1000.0
+            lines.append(f"{r['name']:<35} | {r['duration']:<10.4f} | {avg_ms:<10.2f} | {r['count']:<5} {r['unit']}")
+            
+        with open(path, "w", encoding="utf-8") as f:
+            f.write("\n".join(lines))
+        print(f"\n[Log] Timing report saved to {path}")
+
+
+# -----------------------------
+# 工具函数
+# -----------------------------
+def ensure_dir(p: Path) -> None:
+    p.mkdir(parents=True, exist_ok=True)
+
+def make_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
+    """构造 4x4 SE(3) 矩阵"""
+    T = np.eye(4, dtype=np.float64)
+    T[:3, :3] = R
+    T[:3, 3] = t.reshape(3)
+    return T
+
+def transform_points(pts: np.ndarray, T: np.ndarray) -> np.ndarray:
+    """对 Nx3 点进行 SE(3) 变换：p' = R p + t"""
+    R = T[:3, :3]
+    t = T[:3, 3]
+    return (pts @ R.T) + t
+
+def yaw_from_R(R: np.ndarray) -> float:
+    """yaw(rad)"""
+    return float(np.arctan2(R[1, 0], R[0, 0]))
+
+def euclidean(a: np.ndarray, b: np.ndarray) -> float:
+    return float(np.linalg.norm(a - b))
+
+def estimate_normals(pcd: o3d.geometry.PointCloud, voxel_size: float) -> None:
+    """ICP(Point-to-Plane) 需要法向"""
+    radius = max(1e-3, voxel_size * 3.0)
+    pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=30))
+    pcd.normalize_normals()
+
+def load_pose_txt(path: Path) -> Tuple[np.ndarray, np.ndarray]:
+    """
+    Load pose txt file.
+    Format: timestamp x y z [qx qy qz qw] (>=4 columns)
+    Returns: timestamps (N,), positions (N,3)
+    """
+    try:
+        data = np.loadtxt(str(path))
+    except Exception as e:
+        print(f"[Error] Failed to load {path}: {e}")
+        return np.array([]), np.array([])
+
+    if data.ndim == 1:
+        data = data.reshape(1, -1)
+
+    if data.shape[0] == 0 or data.shape[1] < 4:
+        return np.array([]), np.array([])
+
+    ts = data[:, 0].astype(np.float64)
+    xyz = data[:, 1:4].astype(np.float64)
+
+    # sort + unique timestamps
+    order = np.argsort(ts)
+    ts = ts[order]
+    xyz = xyz[order]
+    ts_u, idx_u = np.unique(ts, return_index=True)
+    return ts_u, xyz[idx_u]
+
+def interp_positions(target_times: np.ndarray, src_times: np.ndarray, src_positions: np.ndarray) -> np.ndarray:
+    """Linear interpolation of positions at target_times."""
+    if len(src_times) < 2:
+        return np.zeros((len(target_times), 3), dtype=np.float64)
+    x = np.interp(target_times, src_times, src_positions[:, 0])
+    y = np.interp(target_times, src_times, src_positions[:, 1])
+    z = np.interp(target_times, src_times, src_positions[:, 2])
+    return np.stack([x, y, z], axis=1)
+
+def compute_svd_alignment(local_pts: np.ndarray, global_pts: np.ndarray) -> np.ndarray:
+    """
+    Kabsch/SVD: find T such that global ≈ R*local + t
+    local_pts/global_pts: (N,3) point pairs with same timestamp
+    """
+    n = min(len(local_pts), len(global_pts))
+    if n < 3:
+        return np.eye(4, dtype=np.float64)
+
+    A = local_pts[:n]
+    B = global_pts[:n]
+
+    ca = A.mean(axis=0)
+    cb = B.mean(axis=0)
+    AA = A - ca
+    BB = B - cb
+
+    H = AA.T @ BB
+    U, S, Vt = np.linalg.svd(H)
+    R = Vt.T @ U.T
+
+    if np.linalg.det(R) < 0:
+        Vt[2, :] *= -1
+        R = Vt.T @ U.T
+
+    t = cb - (R @ ca)
+    return make_T(R, t)
+
+def voxel_keys(pts: np.ndarray, voxel: float) -> np.ndarray:
+    """
+    pts -> voxel indices -> structured keys for fast intersect
+    """
+    idx = np.floor(pts / voxel).astype(np.int32)
+    dtype = np.dtype([("x", np.int32), ("y", np.int32), ("z", np.int32)])
+    keys = idx.view(dtype).reshape(-1)
+    return np.unique(keys)
+
+
+# -----------------------------
+# Configuration
+# -----------------------------
+@dataclass
+class Config:
+    # Paths
+    pcd_dir: Path = Path("data/HK_PCD_DS")
+    pose_root_dir: Path = Path("data/HK_POSE")
+    gnss_json: Path = Path("data/HK_POSE/simulated_gnss.json")
+    out_dir: Path = Path("results")
+
+    # Common
+    voxel_size: float = 0.5
+    max_stage: int = 5 
+
+    # SVD
+    min_svd_pairs: int = 20
+    max_svd_pairs: int = 20000
+
+    # Module 3a: Distance Gating
+    gating_distance_m: float = 500.0
+
+    # Module 3b: Overlap Check
+    overlap_voxel_size: float = 0.5
+    overlap_max_points: int = 200000
+    overlap_ratio_threshold: float = 0.01
+
+    # ICP
+    icp_max_corr_dist: float = 2.0
+    icp_max_iter: int = 50
+    icp_rmse_threshold: float = 0.5
+    icp_fitness_threshold: float = 0.20
+
+    # GTSAM
+    gnss_sigma_xyz: float = 50.0
+    gnss_sigma_rot_deg: float = 60.0
+    icp_sigma_xyz: float = 0.10
+    icp_sigma_rot_deg: float = 5.0
+
+    # Output
+    preview_points_per_block: int = 30000
+
+
+# -----------------------------
+# Shared Context
+# -----------------------------
+@dataclass
+class OptimizerContext:
+    # Metadata
+    gnss_info: Dict[str, Dict] = field(default_factory=dict)
+    block_ids: List[str] = field(default_factory=list)
+    
+    # File Paths
+    pcd_paths: Dict[str, Path] = field(default_factory=dict)
+    pose_paths: Dict[str, Path] = field(default_factory=dict)
+    gnss_kf_paths: Dict[str, Path] = field(default_factory=dict)
+
+    # Loaded Data
+    pcd_ds: Dict[str, o3d.geometry.PointCloud] = field(default_factory=dict)
+    local_traj: Dict[str, Tuple[np.ndarray, np.ndarray]] = field(default_factory=dict)
+    gnss_traj: Dict[str, Tuple[np.ndarray, np.ndarray]] = field(default_factory=dict)
+    
+    # Results
+    svd_global_poses: Dict[str, np.ndarray] = field(default_factory=dict) # Initial Guess
+    pairs_gated: List[Tuple[str, str, float]] = field(default_factory=list)
+    candidate_edges: List[Tuple[str, str, float]] = field(default_factory=list)
+    icp_edges: Dict[Tuple[str, str], Dict] = field(default_factory=dict)
+    optimized_poses: Dict[str, np.ndarray] = field(default_factory=dict)
+
+    def get_pose(self, bid: str, mode: str = "optimized") -> np.ndarray:
+        # Helper to get best available pose
+        if mode == "optimized" and bid in self.optimized_poses:
+            return self.optimized_poses[bid]
+        if bid in self.svd_global_poses:
+            return self.svd_global_poses[bid]
+        
+        # Fallback to origin
+        if bid in self.gnss_info:
+            origin = np.array(self.gnss_info[bid]["origin_xyz"], dtype=np.float64)
+            return make_T(np.eye(3), origin)
+            
+        return np.eye(4)

src/optimizer_modules/exporter.py

+import numpy as np
+import math
+import open3d as o3d
+import matplotlib.pyplot as plt
+from .common import Config, OptimizerContext, transform_points, yaw_from_R
+
+def save_pose_table_and_preview(cfg: Config, ctx: OptimizerContext, mode: str = "initial"):
+    """
+    mode: "initial" or "optimized"
+    """
+    csv_path = cfg.out_dir / f"global_poses_{mode}.csv"
+    png_path = cfg.out_dir / f"global_preview_{mode}.png"
+    print(f"\n=== [Output] Save pose table & preview ({mode}) ===")
+    print(f"[Save] {csv_path}")
+
+    # Pose table
+    try:
+        from scipy.spatial.transform import Rotation as R
+    except Exception:
+        R = None
+
+    lines = ["block_id,tx,ty,tz,roll_deg,pitch_deg,yaw_deg"]
+    for bid in ctx.block_ids:
+        T = ctx.get_pose(bid, mode)
+        t = T[:3, 3]
+        if R is not None:
+            rpy = R.from_matrix(T[:3, :3]).as_euler("xyz", degrees=True)
+            roll, pitch, yaw = float(rpy[0]), float(rpy[1]), float(rpy[2])
+        else:
+            roll, pitch = 0.0, 0.0
+            yaw = math.degrees(yaw_from_R(T[:3, :3]))
+        lines.append(f"{bid},{t[0]:.6f},{t[1]:.6f},{t[2]:.6f},{roll:.3f},{pitch:.3f},{yaw:.3f}")
+
+    csv_path.write_text("\n".join(lines), encoding="utf-8")
+
+    # Preview plot (XY top-down)
+    print(f"[Save] {png_path}")
+    plt.figure(figsize=(10, 10), dpi=140)
+
+    for bid in ctx.block_ids:
+        if bid not in ctx.pcd_ds:
+            continue
+        pts = np.asarray(ctx.pcd_ds[bid].points, dtype=np.float64)
+        if pts.shape[0] > cfg.preview_points_per_block:
+            sel = np.random.choice(pts.shape[0], cfg.preview_points_per_block, replace=False)
+            pts = pts[sel]
+
+        T = ctx.get_pose(bid, mode)
+        pts_g = transform_points(pts, T)
+        plt.scatter(pts_g[:, 0], pts_g[:, 1], s=0.2, alpha=0.35, label=bid)
+
+        tg = T[:3, 3]
+        plt.scatter([tg[0]], [tg[1]], s=40)
+
+    plt.axis("equal")
+    plt.grid(True, linestyle="--", alpha=0.3)
+    plt.title(f"Global Preview ({mode})")
+    plt.legend(markerscale=10, fontsize=8, loc="best")
+    plt.tight_layout()
+    plt.savefig(png_path)
+    plt.close()
+
+def export_global_map(cfg: Config, ctx: OptimizerContext, out_path):
+    print(f"\n=== [Output] Export Global Map: {out_path} ===")
+    merged = o3d.geometry.PointCloud()
+
+    # Prefer optimized if available
+    mode = "optimized" if ctx.optimized_poses else "initial"
+    print(f"[Info] Using mode='{mode}' poses.")
+
+    for bid in ctx.block_ids:
+        if bid not in ctx.pcd_ds: 
+            continue
+        T = ctx.get_pose(bid, mode)
+        pcd = o3d.geometry.PointCloud(ctx.pcd_ds[bid])
+        pcd.transform(T)
+        merged += pcd
+        print(f"  [Merge] {bid} -> points={len(merged.points)}")
+
+    # optional final voxel
+    # merged = merged.voxel_down_sample(voxel_size=max(0.1, cfg.voxel_size))
+    o3d.io.write_point_cloud(str(out_path), merged)
+    print(f"[Save] {out_path} final_points={len(merged.points)}")

src/optimizer_modules/loader.py

+import json
+import re
+import numpy as np
+import open3d as o3d
+from pathlib import Path
+from .common import Config, OptimizerContext
+
+def run_module0_metadata(cfg: Config, ctx: OptimizerContext):
+    print("\n=== [Module 0] Load Metadata & Map Files ===")
+    if not cfg.gnss_json.exists():
+        raise FileNotFoundError(f"JSON not found: {cfg.gnss_json}")
+
+    with open(cfg.gnss_json, "r", encoding="utf-8") as f:
+        ctx.gnss_info = json.load(f)
+
+    def get_idx(k: str) -> int:
+        try:
+            return int(k.split("_")[-1])
+        except Exception:
+            return 999999
+
+    ctx.block_ids = sorted(ctx.gnss_info.keys(), key=get_idx)
+    print(f"[Info] Blocks found: {ctx.block_ids}")
+
+    # Map PCD files
+    pcd_files = list(cfg.pcd_dir.glob("*.pcd"))
+    if len(pcd_files) == 0:
+        raise FileNotFoundError(f"No pcd found in {cfg.pcd_dir}")
+
+    mapped_count = 0
+    for bid in ctx.block_ids:
+        idx = get_idx(bid)
+        found = None
+        for p in pcd_files:
+            m = re.search(r"(\d+)\.pcd$", p.name, re.IGNORECASE)
+            if m and int(m.group(1)) == idx:
+                found = p
+                break
+        
+        if found:
+            ctx.pcd_paths[bid] = found
+            mapped_count += 1
+            print(f"[Map] {bid} -> PCD {found.name}")
+        else:
+            print(f"[Warn] No PCD found for {bid}")
+
+    if mapped_count == 0:
+        raise RuntimeError("No PCD files matched to blocks!")
+
+    # Map Trajectories
+    for bid in ctx.block_ids:
+        info = ctx.gnss_info[bid]
+        if "pose_file" not in info or "gnss_file" not in info:
+            raise RuntimeError(f"{bid} missing pose_file/gnss_file in json.")
+        
+        ctx.pose_paths[bid] = cfg.pose_root_dir / info["pose_file"]
+        ctx.gnss_kf_paths[bid] = cfg.pose_root_dir / info["gnss_file"]
+        print(f"[Map] {bid} -> pose {ctx.pose_paths[bid].name}, gnss {ctx.gnss_kf_paths[bid].name}")
+
+def run_module1_load_pcd(cfg: Config, ctx: OptimizerContext):
+    print("\n=== [Module 1] Load Preprocessed PCDs ===")
+    for bid in ctx.block_ids:
+        if bid not in ctx.pcd_paths:
+            continue
+        path = ctx.pcd_paths[bid]
+        pcd = o3d.io.read_point_cloud(str(path))
+        n = np.asarray(pcd.points).shape[0]
+        print(f"[Load] {bid}: {path.name} points={n}")
+        ctx.pcd_ds[bid] = pcd

src/optimizer_modules/matching.py

+import numpy as np
+import open3d as o3d
+import math
+from .common import (
+    Config, OptimizerContext, 
+    load_pose_txt, interp_positions, compute_svd_alignment, 
+    make_T, transform_points, yaw_from_R, euclidean,
+    estimate_normals, voxel_keys
+)
+
+def run_module2_svd_coarse_alignment(cfg: Config, ctx: OptimizerContext):
+    print("\n=== [Module 2] Trajectory SVD Coarse Alignment ===")
+    ctx.svd_global_poses.clear()
+
+    for bid in ctx.block_ids:
+        pose_file = ctx.pose_paths[bid]
+        gnss_file = ctx.gnss_kf_paths[bid]
+
+        print(f"\n[SVD] {bid}")
+        if not pose_file.exists() or not gnss_file.exists():
+            print(f"  [Skip] missing files.")
+            ctx.svd_global_poses[bid] = _fallback_pose(ctx, bid)
+            continue
+
+        l_ts, l_xyz = load_pose_txt(pose_file)
+        g_ts, g_xyz = load_pose_txt(gnss_file)
+
+        if len(l_ts) < 2 or len(g_ts) < 2:
+            print(f"  [Warn] empty/too short trajectories, fallback.")
+            ctx.svd_global_poses[bid] = _fallback_pose(ctx, bid)
+            continue
+
+        ctx.local_traj[bid] = (l_ts, l_xyz)
+        ctx.gnss_traj[bid] = (g_ts, g_xyz)
+
+        # Sync
+        mask = (g_ts >= l_ts.min()) & (g_ts <= l_ts.max())
+        g_ts_v = g_ts[mask]
+        g_xyz_v = g_xyz[mask]
+
+        if len(g_ts_v) < cfg.min_svd_pairs:
+            print(f"  [Warn] < {cfg.min_svd_pairs} pairs, fallback centroid.")
+            c_l = l_xyz.mean(axis=0)
+            c_g = g_xyz.mean(axis=0)
+            T = make_T(np.eye(3), c_g - c_l)
+            ctx.svd_global_poses[bid] = T
+            continue
+
+        l_xyz_i = interp_positions(g_ts_v, l_ts, l_xyz)
+
+        # Sample
+        if len(g_ts_v) > cfg.max_svd_pairs:
+            sel = np.linspace(0, len(g_ts_v) - 1, cfg.max_svd_pairs).astype(int)
+            l_xyz_i = l_xyz_i[sel]
+            g_xyz_v = g_xyz_v[sel]
+
+        T = compute_svd_alignment(l_xyz_i, g_xyz_v)
+        
+        # Eval
+        pred = transform_points(l_xyz_i, T)
+        res = np.linalg.norm(pred - g_xyz_v, axis=1)
+        rmse = math.sqrt(float(np.mean(res**2)))
+        yaw_deg = math.degrees(yaw_from_R(T[:3, :3]))
+        t = T[:3, 3]
+        
+        print(f"  [OK] pairs={len(l_xyz_i)}, rmse={rmse:.3f}m, yaw={yaw_deg:.2f}deg")
+        ctx.svd_global_poses[bid] = T
+
+def run_module3a_distance_gating(cfg: Config, ctx: OptimizerContext):
+    print("\n=== [Module 3a] Distance Gating (Centroid) ===")
+    ctx.pairs_gated.clear()
+    
+    # Use GNSS info for rough centroid check
+    centroids = {}
+    for bid in ctx.block_ids:
+        # If we have better SVD pose, update centroid? 
+        # Actually SVD pose * local_pcd_center is better.
+        # But let's stick to json centroid for 'coarse' gating as requested or use SVD if avail.
+        if bid in ctx.svd_global_poses and bid in ctx.pcd_ds:
+            T = ctx.svd_global_poses[bid]
+            c_local = ctx.pcd_ds[bid].get_center()
+            c_global = (T[:3,:3] @ c_local) + T[:3,3]
+            centroids[bid] = c_global
+        else:
+            centroids[bid] = np.array(ctx.gnss_info[bid]["centroid_xyz"], dtype=np.float64)
+
+    for i in range(len(ctx.block_ids)):
+        for j in range(i + 1, len(ctx.block_ids)):
+            bi, bj = ctx.block_ids[i], ctx.block_ids[j]
+            d = euclidean(centroids[bi], centroids[bj])
+            if d < cfg.gating_distance_m:
+                ctx.pairs_gated.append((bi, bj, d))
+                print(f"  [Pass] {bi}<->{bj} dist={d:.2f}m")
+
+    print(f"[Done] Gated pairs: {len(ctx.pairs_gated)}")
+
+def run_module3b_overlap_check(cfg: Config, ctx: OptimizerContext):
+    print("\n=== [Module 3b] Geometric Overlap Check (Voxel) ===")
+    ctx.candidate_edges.clear()
+
+    if not ctx.pairs_gated:
+        print("[Warn] No gated pairs.")
+        return
+
+    for (bi, bj, dist) in ctx.pairs_gated:
+        if bi not in ctx.pcd_ds or bj not in ctx.pcd_ds:
+            continue
+
+        Ti = ctx.get_pose(bi, mode="initial")
+        Tj = ctx.get_pose(bj, mode="initial")
+
+        pts_i = np.asarray(ctx.pcd_ds[bi].points)
+        pts_j = np.asarray(ctx.pcd_ds[bj].points)
+
+        # Sample
+        pts_i = _sample(pts_i, cfg.overlap_max_points)
+        pts_j = _sample(pts_j, cfg.overlap_max_points)
+
+        pts_i_g = transform_points(pts_i, Ti)
+        pts_j_g = transform_points(pts_j, Tj)
+
+        vs = cfg.overlap_voxel_size
+        kA = voxel_keys(pts_i_g, vs)
+        kB = voxel_keys(pts_j_g, vs)
+        inter = int(np.intersect1d(kA, kB).size)
+        denom = max(1, min(kA.size, kB.size))
+        ratio = float(inter / denom)
+
+        keep = ratio >= cfg.overlap_ratio_threshold
+        print(f"[Check] {bi}<->{bj} ratio={ratio:.4f} => {'KEEP' if keep else 'DROP'}")
+        
+        if keep:
+            ctx.candidate_edges.append((bi, bj, ratio))
+
+    print(f"[Done] Candidate edges: {len(ctx.candidate_edges)}")
+
+def run_module4_icp(cfg: Config, ctx: OptimizerContext):
+    print("\n=== [Module 4] ICP Fine Matching ===")
+    ctx.icp_edges.clear()
+    
+    if not ctx.candidate_edges:
+        print("[Warn] No candidate edges.")
+        return
+
+    for (bi, bj, ratio) in ctx.candidate_edges:
+        print(f"\n[ICP] {bi} -> {bj} (overlap={ratio:.4f})")
+        
+        src = o3d.geometry.PointCloud(ctx.pcd_ds[bi]) # copy
+        tgt = o3d.geometry.PointCloud(ctx.pcd_ds[bj]) # copy
+
+        Ti = ctx.get_pose(bi, mode="initial")
+        Tj = ctx.get_pose(bj, mode="initial")
+
+        # Initial relative guess: p_j = T_rel * p_i
+        # T_rel = inv(Tj) * Ti
+        T_rel_init = np.linalg.inv(Tj) @ Ti
+
+        estimate_normals(src, cfg.voxel_size)
+        estimate_normals(tgt, cfg.voxel_size)
+
+        reg = o3d.pipelines.registration.registration_icp(
+            src, tgt,
+            cfg.icp_max_corr_dist,
+            T_rel_init,
+            o3d.pipelines.registration.TransformationEstimationPointToPlane(),
+            o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=cfg.icp_max_iter),
+        )
+
+        fitness = float(reg.fitness)
+        rmse = float(reg.inlier_rmse)
+        valid = (rmse <= cfg.icp_rmse_threshold) and (fitness >= cfg.icp_fitness_threshold)
+        
+        print(f"  -> {'VALID' if valid else 'FAIL'} (fit={fitness:.4f}, rmse={rmse:.4f})")
+
+        if valid:
+            ctx.icp_edges[(bi, bj)] = {
+                "T_ij": reg.transformation,
+                "fitness": fitness,
+                "rmse": rmse,
+                "overlap_ratio": ratio
+            }
+
+    print(f"[Done] Valid ICP edges: {len(ctx.icp_edges)}")
+
+
+def _fallback_pose(ctx, bid):
+    origin = np.array(ctx.gnss_info[bid]["origin_xyz"], dtype=np.float64)
+    return make_T(np.eye(3), origin)
+
+def _sample(pts, max_n):
+    if pts.shape[0] <= max_n: return pts
+    sel = np.random.choice(pts.shape[0], max_n, replace=False)
+    return pts[sel]

src/optimizer_modules/optimization.py

+import numpy as np
+import math
+from .common import Config, OptimizerContext
+
+try:
+    import gtsam
+    from gtsam import Pose3, Rot3, Point3
+    GTSAM_AVAILABLE = True
+except ImportError:
+    GTSAM_AVAILABLE = False
+
+def run_module5_gtsam(cfg: Config, ctx: OptimizerContext):
+    print("\n=== [Module 5] Global Optimization (GTSAM) ===")
+    if not GTSAM_AVAILABLE:
+        print("[Error] GTSAM not available. Skipping optimization.")
+        ctx.optimized_poses = ctx.svd_global_poses.copy()
+        return
+
+    graph = gtsam.NonlinearFactorGraph()
+    initial = gtsam.Values()
+
+    # Noise
+    rot_sigma = math.radians(cfg.gnss_sigma_rot_deg) # large sigma = weak prior
+    trans_sigma = cfg.gnss_sigma_xyz
+    prior_sig = np.array([rot_sigma]*3 + [trans_sigma]*3, dtype=np.float64)
+    prior_noise = gtsam.noiseModel.Diagonal.Sigmas(prior_sig)
+
+    icp_rot_sigma = math.radians(cfg.icp_sigma_rot_deg)
+    icp_trans_sigma = cfg.icp_sigma_xyz
+    icp_sig = np.array([icp_rot_sigma]*3 + [icp_trans_sigma]*3, dtype=np.float64)
+    icp_noise = gtsam.noiseModel.Diagonal.Sigmas(icp_sig)
+
+    def key_of(bid: str) -> int:
+        return gtsam.symbol("x", int(bid.split("_")[-1]))
+
+    # 1. Priors (Weak GNSS/SVD)
+    print(f"[Info] Add priors (sigma_xyz={trans_sigma}m)...")
+    for bid in ctx.block_ids:
+        k = key_of(bid)
+        
+        # We assume SVD pose is "Prior"
+        T = ctx.get_pose(bid, mode="initial")
+        
+        pose = Pose3(Rot3(T[:3, :3]), Point3(T[0, 3], T[1, 3], T[2, 3]))
+        graph.add(gtsam.PriorFactorPose3(k, pose, prior_noise))
+        initial.insert(k, pose)
+
+    # 2. Between Factors (ICP)
+    print(f"[Info] Add ICP factors ({len(ctx.icp_edges)})...")
+    for (bi, bj), info in ctx.icp_edges.items():
+        ki = key_of(bi)
+        kj = key_of(bj)
+        
+        # ICP T_ij: p_j = T_ij * p_i
+        # Factor: Between(j, i, meas) where meas = Pose_j^{-1} * Pose_i = T_ij
+        T_ij = info["T_ij"]
+        meas = Pose3(Rot3(T_ij[:3, :3]), Point3(T_ij[0, 3], T_ij[1, 3], T_ij[2, 3]))
+        
+        graph.add(gtsam.BetweenFactorPose3(kj, ki, meas, icp_noise))
+
+    # 3. Optimize
+    print("Optimizing...")
+    try:
+        err_before = graph.error(initial)
+        print(f"  Error Before: {err_before:.6f}")
+
+        params = gtsam.LevenbergMarquardtParams()
+        optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
+        result = optimizer.optimize()
+
+        err_after = graph.error(result)
+        print(f"  Error After:  {err_after:.6f}")
+
+        # 4. Save
+        ctx.optimized_poses.clear()
+        for bid in ctx.block_ids:
+            k = key_of(bid)
+            if result.exists(k):
+                ctx.optimized_poses[bid] = result.atPose3(k).matrix()
+    except Exception as e:
+        print(f"[Error] GTSAM Optimization failed: {e}")
+        ctx.optimized_poses = ctx.svd_global_poses.copy()
+
+    print("[Done] Optimization finished.")