多项式图搜索：Teaser++用于3D点云重定位

全天候定位是一个麻烦的问题，Agent重定位，自动驾驶有自动驾驶的方法，高速的定位方法是不能主要依赖主动视觉的，低速的方法倒可以。

3D点云的Align方法：Teaser++ fast & certifiable 3D registration ，hd_location使用的一个重定位方法，是个非常优秀的3D重定位方法。

论文中和RNASAC、ICP效果对比

3D配准实际效果：

在偏离了原点大概20米的拐角地方，Teaser++重定位位置成功。

box range box center : x is:0 y is:0 z is:0
[ INFO] [1714025281.618713153]: box range box size() : l is:130 w is:130 h is:130
[ INFO] [1714025281.618720805]: box 1 : x is:-65 y is:-65 z is:-65
[ INFO] [1714025281.618728484]: box 2 : x is:65 y is:65 z is:65
[ INFO] [1714025281.618743013]: getCrops: x is:130 y is:130 z is:130
[ INFO] [1714025281.623911746]: set_global_map: mapT size: 251995
[ INFO] [1714025281.626648261]: Normal Estimation: Radius(2)
[ INFO] [1714025281.704381826]: FPFH Extraction: Search Radius(4)
[ INFO] [1714025282.748073015]: set_global_map: this->global_map_slices x size: 93296
[ INFO] [1714025282.748219829]: set_global_map: mapT size: 251995
[ INFO] [1714025282.751002492]: GLE-FPFH_Teaser_Slice:Query slice ...start..
[ INFO] [1714025282.751054266]: query_slice: map_slice  size: 93296
[ INFO] [1714025282.751095817]: query : Query Global Loc_slice  :0
[ INFO] [1714025282.767617351]: Normal Estimation: Radius(2)
[ INFO] [1714025282.780611171]: FPFH Extraction: Search Radius(4)
[ INFO] [1714025282.836091769]: query : Query Global Loc_slice  :0
[ INFO] [1714025282.836320666]: Find Correspondences
Skipping Cross Check.
Skipping Tuple Constraint.
[ INFO] [1714025283.146781544]: Run Teaser
Starting scale solver.
Scale estimation complete.
Max core number: 109
Num vertices: 2035
Max Clique of scale estimation inliers:
Using chain graph for GNC rotation.
Starting rotation solver.
GNC rotation estimation noise bound:1
GNC rotation estimation noise bound squared:1
GNC-TLS solver terminated due to cost convergence.
Cost diff: 0
Iterations: 16
Rotation estimation complete.
Starting translation solver.
Translation estimation complete.
[ INFO] [1714025283.316156511]: GLE-FPFH_Teaser_Slice:Query slice idx...over!<< 0
[ INFO] [1714025283.324788303]: GLE-FPFH_Teaser_Slice:Query slice ...over!

地图起点位置：