###### tags: `progress`

# Motorcycle Tracking

* [Evaluation on kuang-fu rd](https://hackmd.io/uk_LeEneTIinvGUswygOHA)
* paper with code:
https://paperswithcode.com/task/3d-multi-object-tracking
* waymo open dateset challenge report:
https://sites.google.com/view/cvpr20-scalability/wod-reports?authuser=0
* center-based tracking
* 87趴像 可參考寫法
Dynamic Multi-LiDAR Based Multiple Object
Detection and Tracking
* [[Probability Data Association(pda)](https://zhuanlan.zhihu.com/p/176851546)]
**context**
[TOC]

[[data moto video demo](https://www.youtube.com/watch?v=LQPgMzRAOss&feature=youtu.be)]

## issue list
- [x] 1. multi-lidar grouping method => delete smaller, overlapped one
當側邊和上面照到同個物體（左右兩側車輛），側邊lidar照得比較完整，但因為存在時間沒有上面久，會一直被groupTarget()拿掉
=>有誤, 並不是被groupTarget()拿掉,而是本來就沒有產生
::: warning
**Doing**
- [ ] 2. motorbike merge
對於車機車,機車和機車很接近時，會merge成一個track
對於鑽車陣的機車而言,因為此時機車跟ego很近,不希望因為merge而沒看到, 或merge而亂連
[issue video](https://youtu.be/k74zGtsEJTw)
- [ ] 3. ground over-segmentation
用一個general slope去濾點, 會濾掉過多的點, 在追蹤上會讓點變少看不到
[ground segmentation](/sBvXv4W6QKGD-PWhmzL7OA)
:::
- [ ] 4. Shape-caused tracker update
在tracking時, 應該要考慮shape變化, 因為若用centroid, 則速度會受到看到的形狀影響, ex. 靜止物體因ego接近看到形狀越多而有假的速度（centroid在跑）
但要考慮多顆lidar時看到的perpective不同
(如何判斷split是否唯一台車？)
- [ ] 5. tracking過程可能不能只看distnance confidence, 考慮過去trajectory, associate應該要follow trjectory而smooth過去, 可以改善當物體移動時突然連到旁邊而有一個鋸齒狀的軌跡
::: spoiler **check points**:
* merge
1. 00:35
一開始機車　=> merge(機車還在) => (超過0.3s）只剩車，機車的merge track　=> 機車回來
2. 01:15
前方兩台機車merge
3. 03:35 - 03:57
連續機車往前行,經過３台停等車都merge(最遠20m處)

* others
1. 01:34
對向機車initialize剛出現時往ego衝，（將initialize時的點也加入trajectory看是不是因為後方消失而連錯）
2. 01:40
對向多台機車，連結凌亂（可能是某幾台在某幾frame消失連錯）
:::
## Issue 1 (closed)
想說若兩個不同sensor的target同時存在的話, 去判別top polygon的overlap情況, 把小的拿掉
### oberservation
* 對於side lidar而言, 蠻容易associate到top lidar的物件, 所以有時候會一直看不到side lidar的object, 因為他跟top都更新到同一個obj(top的)
* 我們只在top時發布marker, 所以同一個target的polygon只會顯示top的, 不會看到side obj的polygon


> 在termina顯示的是right_lidar拿到的obj數目,right_lidar的右後方車輛associate到應該是top的target id 33.（虛線polygon為right_filtered_obj)


> 右後方的車,機車都associate應該是top-lidar產生的target 42,33上面.

### conclusion
這個狀況主要在討論兩顆lidar打到同一物體不同面向時, 擔心會產生互相干擾association的問題（且會有redudant的target）,但發現側邊光達傾向一直associate到top-lidar的target,所以反而問題不大.
至少在兩顆lidar都看到的情況下, 可以得到穩定的tracker, 不影響原本追蹤功能.

**因此這個overlap的功能先閒置, 因為目前兩個lidar打到同一個物體時, 通常都只會associate到top的物體**

## Issue 2 Proposed methods－針對merge問題
real-time close object segmentation/tracking in urban area
[[Efficient Tracking of Closely Spaced Objects in Depth Data using
Sequential Dirichlet Process Clustering](https://cse.buffalo.edu/~jsyuan/papers/2017/Efficient%20Tracking%20of%20Closely%20Spaced%20Objects%20in%20Depth%20Data%20using%20Sequential%20Dirichlet%20Process%20Clustering.pdf)]
提到對real-time object tracking而言, 常常會有under-segment的問題
提到文獻13 probabilistic distance based clustering => 可能跟我分布分群的感覺有點像?
[[Generative Object Detection and Tracking in 3D Range Data, ICRA 2012](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=6224585)]

### I.改善分群方式 (closed)
description: 對於較近的region設為15m處的threshold, 較遠處的則完全靠range/distance來決定　[0928 ppt]

#### Result
癥結點: 並沒有直接解決issue 2的問題, 只是把較近的機車的threshold調低了，且只有在某些special case作用, 後面很多case還是沒有解決, 且還是需要吃15m這個條件

---
### II.透過tracking結果回饋回來 [Merge Detector]
若一開始知道是兩個物體，有沒有辦法透過這個線索讓後面不要只merge成一個，而還是顯示兩個track(即使我cluster成一個)？

用歷史hull predict的位置, 先跟進來的detection做比較, 若有one detection to multiple track ＝> 認為detection merge了, ~~拿predict的hull去update track(因為還是有看到, 只是merge了), 不用拿detection~~ [改善](## Merge Measurement Reconstruction)

#### 可能產生的問題：
* 用prediction來update不夠準, 因為prediction通常比較慢
* 這樣等於是相信detection**傾向於錯誤的merge**, 真正的merge反而會失效(也就是比較碎一點,但若都有追到應該還好)
> id 1, 22為一台車

* 若機車track剛產生沒多久,機車的predict會追太不上, 也就是沒辦法overlap問題（需要imm predict較精確）.確認若track產生很久, predict即使是機車也有很高的重疊(幾乎完全疊)

> 紅色為predict, 黃色為detection拿來做track的結果, id 26機車predict並沒有跟detection重疊

#### Result (prediction update)
Segment demo: [merge resolve by tracking](https://www.youtube.com/watch?v=9OFkEy1gct0&feature=emb_logo)
看起來幾乎所有原本會merge的機車都被保留下來了（粉紅色track為修改後的輸出,藍色為這個frame的detection）

---

### Check of [tracking feedback merge detector](###II.透過tracking結果回饋回來): whole route of kung-fu rd
:::info
almost done
:::
- [x] 重疊的track會影響merge detector的判斷, 用removeOverlappedTargets()移除就可以了

> id 19 兩個重疊的track

- [x] 大車一大部分看不到 (7 sec)
**判別merge是正常 or 不正常
當overlap, 要相信tracker還是相信meas**
因為lidar在遠處的cluster會比較碎, 越近, 會因為看到比較多而較完整merge
但若此時僅採用上個frame的破碎的track來update, 則反而看不到全貌，持續破碎，應該相信meas

> 增加對於merge track的一些假設 :
應該可以透過**所有track的交集的總面積**去限制, 若跟detection比例小於一個threshold **T(目前設0.3)**, 不啟用, 用拿到的detection
=>小於閥值，覺得應該是真的merge, 相信detection, 表示到這個frame形狀變化大, 相信分群的結果
=>針對機車或不正當的merge, 應該只是**因為物體距離太近**造成, 因此**merge前後我們看到的物體形狀沒有太大的改變, 則overlap的ratio應該要很高**
if ( sum(overlapped area) / area(det) > T ) :
merged = true


## Merge Measurement Reconstruction

#### 如何進行measurement的reconstruction?
對於measurement merge/split的問題
Extend targets: each target can generate multiple measurements
Extend tracking: target和measurement並不是一對一的hard association
[extended target tracking [ref](https://ieeexplore.ieee.org/document/9020858)]
現在大部分對於tracking的假設是one-to-one association, 也就是一個target只會產生一個measurement, 也變成是clustering時就要避免掉merge/split
雖然可以用pda(single)/jpda(multiple), 在association時就考慮多個mea對到一個track, 但若用multi-measurements to one track, time complexity會較高(取決於target及measurement數目), 在機車繁多的車流空間中可能會影響運算效能, **為了提高computational efficiency, 因此主張在clustering就先解決掉, 不要拿到association再做**

> **問題敘述**
> 已知merge tracks數量, 及其merged measurement, 如何去將measurement重新分出所需的cluster來update tracks
Reference:
1. ~~k-means~~
[Feature-Based Probabilistic Data Association (FBPDA)
for Visual Multi-Target Detection and Tracking
under Occlusions and Split and Merge Effects](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=5309694)
Intro提到可以用**k-means** <- 對我們的問題應該是最快且有效的（因為已知merge track群數，要分出這樣數目的measurement)
結果: 由於object unbalanced dimension, 傾向於對分成等大小的群, 沒有考慮物體本身的shape/geometry
2. Gaussian geometry model
3. ICP

> 當完成Merge detection, 我們須對merge的物件做重新分群的動作, 以便得到正確的measurement來update track.

### K-means (close)

發現kMeans在我們這種case下會分的不好, 因為物件形狀的關係, 用中心距離去分會有很大的問題.
車體上半部與機車的距離永遠會比較近
對離群點無法處理
車的中心到車側的距離反而比機車到車側大, 車側一定會分錯

### Gaussian distribution(simplied GMM)

#### Concept
* 希望可以考慮obj原本形狀的分佈, 不要只看點與點的距離去分, 這樣當機車靠近車尾端時車輛L尾端就不會被分給機車（K-Mean)

若用data分佈的方式, 將每個track的所有data點fit成Gaussian
再去計算merge群的每點屬於各個track的機率
就可以將track的幾何考慮進來

:::success

*p(z|trk_geomtery) ~ N(z|nu, sigma)*

z表laser點
nu, sigma 為 sample mean 及 sample variance
計算個別點**z**屬於track **trk**的likelihood p(z|trk_geometry), 歸給likelihood大的
沒有考慮到track prior, 可以視為假設merge track內每個prior(發生機率)相同

:::

> grid size = 1m, 紫色為各track的點計算sample variance而來

機車的data很聚集, covariance小, 再加上機動性高（predict不一定追的上）, 所以在機車離群點還是會有些點會分到車子
**（但我覺得只有1.2個點還好, 大致兩群有出來就可以）**


> 可看得出來機車離群點反而會因為車子geometry而被分錯（斜）

遇過的BUG: 260 + 17 SEc 黑色機車銀色車
明明前幾個frame都有分成兩群,表示tracker應該會一直update
但到某個frame機車卻不見了,導致detect不到
=>確定是因為track不見而沒偵測到（沒有黃色poly）, 不是有偵測到而分錯（橘色粗poly）
**雖然obj有一直被正確的分開, 但被groupTarget移除惹(距離很近）,導致track不見,下個frame detect不到**

#### Adjustment (best now) [[1026 merge detector](https://docs.google.com/presentation/d/1Q7_AOScKGnqDntxlXJ4nUmDtsLmgEc8IF289lzu7zPE/edit#slide=id.ga4f359fb72_0_10)]
* **groupTarget**內增加限制若為MergeDetector分開的merge_tracking, **不要移除** 移除會有太多的原本破碎產生的false alarm, 導致後面一直判定為merge, 整體太碎
* 因為有時點會橫跨兩群, 像是上圖機車離群點可能會分給車輛, 車輛poly會橫跨, 所以polygon可能會overlap, 導致成功分開的track又被刪掉, 先把removeOverlap拿掉(不知道ratio調大是否能改善, 因為機車物件本來就小)

**對於std_x, std_y < 30m 調整cov scale 1.3就好, 1.5會讓機車太大, 調整1.3就分開了, 且機車也不會在L型尾端時被分給車子**


**目前看來, 這個分法可以分的不錯, 只要detector有偵測到, 都可以蠻準的分開**

### ICP
把比較大的target對整個merge measurement做ICP, 測試結果可以match的很好
但**無法考慮到形狀變化**的情況, (物體本身因occlusion或FOV而導致前後frame形狀變化)
> if true_target(t) < target(t|t-1)
> 好辦, 把predict(t|t-1)和merge detection重疊部份分出來就好, 剩下另外一群
> else if true_target(t) > target(t|t-1)
> 麻煩, 因為對於多出來的part無法分類到某群


>黃色為merge detection,綠色為Gaussian分出來的, 桃紅色為ICP基於上個predict frame做完的結果. 因為上個frame只看到後車屁股, 會導致在這個frame車體前端多出來的部份無法歸類為哪群, 無法處理因occlusion等造成的形狀改變

把target和merge detection做ICP, 看起來所有target都對的蠻好的
Note: 先挑大個target做, 做完記得挖掉重疊的detection, 再做小的, 要不然小的target在match時cost會被大車的拉走, match會歪

## tracking-based Merge Detector:

有些決定merge是看track和measurement的CoG, 看是否落在track的某個gate範圍內, 容易fail
我是考慮track/mea的大小形狀去看有沒有重疊, 幾乎不會fail

以我的方式而言, 當我決定merge時, 其實同時已經做完追蹤, 知道現在這個det內有幾個track
做point-to-track cluster相當於做的是point的追蹤(決定每點屬於那個track)
做point-to-track cluster是用來產生meas 來 update track state

但因為lidar點多, 若每個物體都改成point-to-track追蹤, 應該耗資源大, 又不一定效果好, 因此對於track state還是只做centroid的update

**問題在於, 當我可以抓出所有merge時, 如何決定哪些merge是需要被處理的?(真正的merge)
會發生同物體因上個frame occlusion等split 而在下個frame被我判斷為merge強制分開**

valid merge rule 相信是正確的merge
1. total_overlapped_areas/area_obj <= 0.25
若重疊比例佔detection範圍極小, 表示可能是真的有些點在上個frame occlusion或lose, 才碎掉, 到這個frame回來, 才能merge在一起, 是有效merge.

2. ~~（待加） total_overlapped_areas/area_obj >= 0.9 (極高)
若重疊比例極高, 可能是發生在靜止物體在上個frame碎掉, 這個frame救回來, 所以相信是有效merge~~

（若一動一靜, 或兩者不同物體, 則overlap比例不可能這麼高, 因為中間會有些merge detection面積是空的）~~
不好, 因為有時比例就會這麼高（當物體真的很近, 或者前個frame有發生像前面離群點分錯, 則overlap面積就會很高）

3. 若考慮碎掉的物體移動應該要相同, 去做split或merge猜測？
4. rigid body, 把前面碎掉的tracker merge起來, 跟現在merge detection比較相似度？ 若是物體碎掉, 不是兩個, 應該不會有形變, 相似度高
相似度的量化, ICP? polygon?

---

想法: ICP去match target, 大小車可以得到還不錯的match(移動小車會有一點點shift)
若可用ICP去算每個target前後兩frame的transformation(移動量, rot+trans)
去比較target移動量的相似程度, 來判別是否視同一個物體
若同一個, 移動量應該很接近 （rigid body)
**除非現在有兩物體同motion且merge, 才有可能偵測不到
可能停很近機車, 或並行的車輛**

ICP locate target at time t => calculate moving from t1-1 to t => compare

跟直接用UKF的估的motion比起來, 應該會比較robust, 因為我們是直接定位這個frame的物體(measurement), 有考慮物體的幾何, 然後跟上個frame算轉換. UKF估的還是質心, 可能會有一點飄.
希望由ICP來得到物體更精確的transform來當rigid motion依據, 而不是用filter的motion estimation

### trying
:::warning
Doing
:::
[1109 meeting](https://docs.google.com/presentation/d/1-yt1-pM196lBhJubPPJdMZ62Xw8dhTER_1EFdxaKARM/edit?usp=sharing)
在量化不同target transformation的差異, 可以只看traslation difference或看整個transformation norm
但我發現當把rotation考慮進來, 也就是看transformation norm, 會比較難找到一個rigid_motion_的threshold, 因此先只看移動量即可, 目前設0.1看起來尚可

**等於是我判斷了merge前先判斷了上一個frame target是不是split**

影響：
路邊停止中的機車車輛會被排除
=> 反正都在靜止中, 我覺得merge比較沒關西, 當其中一方動起來就會自然分開了
=> 主要是兩方運動不一但被merge, 就會有一方看不到
機車並行看起來不會受影響

思考：若是三個物體, 若其中兩個靜一個動, 還是要分開
=> 只要有一個target和其他不是rigid 就分開

---
**rigid_motion_threshold_** (unit: m/sec)
| value | 成果 |
| ----- | -------------------------------------------- |
| 1.5 | 整段看起來還行, 大部分rigid都有分對, 動可以維持 |
| 1 | ~~有些機車通過時會誤判~~ |
| 0.5 | 都不會誤判,且碎掉的也能維持住 |
gan 0.1 itri 在 s270 s260 有時blue guy分不開
260比較穩定
其他機車都分的開

人一般行走速度為**3.75 - 5.43km/hr**, 因此我認為不要大於1.5m/sec（5.4km/hr）較好

### Occlusion
對於一般urban地區而言, occlusion通常是一兩個frame的把物體截斷, 因此在下個frame時segmentation形狀會回來, 在運作上不會造成大影響
但當要處理merge時, 若判斷是否merge, 則occlusion的case就會使我造成誤判, 把不應該分開的物體分開.
因此, 如何在處理merge時，還要考量到occlusion與否. 直接從occlusion的角度.
如果從motion的角度, 又可能使同時動或同時靜的物體分不開（fail）, 治標不治本.

### Problem
有時候總有一台blue guy會到尾巴merge,看起來是merge detect沒抓到,但不確定原因（因為時有時無）

bag play rate 0.8必有兩台機車掛掉
play rate 0.5都可以分的開

---
* progress/1103/abrupt_v_from_bus_s190.mp4

在 s=190sec處路邊大巴士跟機車merge時, 會有一個大速度往後衝, 主要是因為中間某個frame大巴後面一小塊碎掉, 導致那塊後來被我重新分群後形狀變大而有一個平行車體的速度, 調整分群點數從10->15拿掉那小塊就行了
（調10是因為在測試路段停等機車很多看不到, 預設是20）

### 對於偵測錯誤的case
源自於前個frame split, 當終於得到完整的detection時, 卻被我另外分開了
#### occlusion
* 比例極低, 整段只有最後停等機車通過時有可能, 1-2 frame, 好像只發生在離很近的機車遮擋到後方車輛的case (5%)

#### 遠方車輛漸近時並不是一塊, 而會先碎成多塊, 隨著距離越近merge成一塊 (to be confirmed)
1. 因為地面over-segment, 導致前方對向來車輛較易分成兩塊 (80%)
2. 跟點本身稀疏度/密度有關, 都是比較遠的, 較長的bus會, 稀疏再加上over-segment （15%）

- [x] 拿新的ground filter測試, 若能real-time, 應該就不會over-segment了
- 比較不會over-seg 好很多

地板過度濾的部份, 發現比較有問題的都是對象來車, 尤其黑車

在引擎蓋到擋風玻璃處本來就點少, 加上那塊容易被當成地板, 容易使車碎成前後塊.
若新的地板可以real-time, 應該可以解決（新的包在非地板區塊的效果不錯, 不會濾掉太多車體）

其中, 對象來車又以黑車碎的比較嚴重, 主要是黑車相對於白車在側面車身的點稀疏（引擎蓋到擋風玻璃處差不多）, 更容易斷成兩截, 白車側面處還算密集, 可保持一台。

**巴士除了前後碎成兩塊外, 還有在另一側會有一些小碎塊干擾, 是因為玻璃折射？**

#### 1112
對原本的scan match, 好像很多都是沒有match到, 造成motion diff很大, 才會一直分開
若只考慮有match到的, 去算motion diff, 沒有match到的直接讓他merge（用原detection), 則問題好很多（check moving cars)

沒有match到通常發生在移動速度比較快的車/大車, 我猜可能是predict太慢讓他追不上, match不好

這樣修改後, 若是遠方漸進車子碎掉的case, 幾乎都不會被我誤抓出來, 可以正常merge了
!!!! 260+23sec黑色大大經過前面第二台車時, 尾巴有一點merge, 因為match不到 NO
-看起來是機車大大 missing?

好像只有在play rate 0.8才會？？？？
0.5看正常
乾用0.8但從275播放又match得到 啥鬼
入果跟播放時間點有關系, 應該跟missing有關, 但都離開始播放時間點很遠阿？？？（8sec和23秒）

#### 1124
策略: 用ICP去得到precise motion, 來判別是不是應該要split
若一個detection內發現有多個track並不是以相似motion運動, 則應該本來就是不同track/object, 應該要分開

:::warning
**ICP有時會因為以下而無法converge**

    1.點不足 (可能是因為前一個frame沒有分好, track拿到很遠離群點, 導致我在用涵蓋面積去exclude掉track時, 把所有點都排除, 沒有剩餘點做ICP)
    2.兩個frame間點缺漏 (scan difference from occlusion..etc.)
    3.Prediction的速度還沒追上來, 導致initial guess不准（還要確認）

**ICP即使converge**

    4.速度大的東西（10m/s)基本上都match的不太好 （即使點沒有闕漏或速度差夠。看起來是initial（predict）給的位置會有一點點差，大概1m，rot沒問題，但icp後騎士會翻轉？？）

:::

- [ ] 因為前個frame recluster沒有分好, 導致離群點會把所有點排除, 可以改成不要排除所有在已經match的track的點, 而只排除相對於此track geometry信賴區間(馬式距離去算)較大的點, 就不會算到離群點的面積惹
- [x] 分群過程中，增加條件，prob(pt|T)需大於一定值才會被分進track，否則拿掉當成outlier, 此舉可避免模稜兩可的邊界點分錯而影響之後的重疊判斷，且物體也分的比較開。

:::success
Merge detector Rule
若確定可以converge(match的到), 表示點數夠多, 能夠當成正確的match, 則估出來的motion才會拿來計算是否為rigidMotion.
(converge & ~~**fitnessScore < thres**~~ 才能相信icp match結果, 拿來算motionDiff)
因此若要能夠被我分開, 須滿足：
MergeDetection內有**多個track的ICP都能夠match的到**
且
**至少有一組(pair)他們的motionDiff超過我的threshold**

則判定merge 要分開

if ( overlapped_IOU > overlap_threshold && 
    ICP motionDiff > threshold)
    mergeDetection = true

if (mergeDetection == true)
    reClusterByGaussianShapeDis(Det)
    
def reClusterByGaussianShapeDis(Det):
    if (prob(pt|T) < outlier_threshold)
        T <- pt
    else
        outlier <- pt

:::

Check overlapping -> ICP match between (t-1) and t -> Converge => check difference
-> Non-converge => final transform remain original prediction (initial guess)
-> Tend to have large transformation between prediction & detection => 容易分開

一般來說, fail converge會導致最後transformation和initial一樣, 變成去算prediction和上一個frame x_merge_的transformation
而因為KF的prediction通常都會慢一點點, 因此兩者間的motion difference會大一點點, 通常會傾向分開

**LOST ASSOCIATION ISSUE**
lost association的問題, 應該是因為一開始unstable時速度還沒上來, 一直追不上, 所以一直產生新的track
若把unstable的track hide起來, 只看已經stable的track, 就不會這麼亂了
但是否需要second initialize？
用second initial由兩個frame的瞬間差強制給值x_merge_ p_merge_, 說不定就追的上了？ （不確定kalman filter update的速度, 我猜可能比較慢, 因為考慮3個model的weighted average, 所以對於初速大的可能追不太上）

**WRONG ASSOCIATION FOR CLOSE OBJECT**
過交流道候的光復路 （200), 對向大群機車直行

目前看到速度會連錯的問題, 有些都不是在unstable發生, 而是stable後, 所以可能在stable時需要用當下的cov… 重新initialize?

*checking list*
- [x] 畫出covariance確認, 有些連錯有點遠
- [ ] second initialization的必要性
- [x] check covariance p_merge_ 會因為lose frame而增加
> x,y 標準差, 單位:m
- [x] 即使lost, update時的值也是拿predict來算, 因此最終x_merge_/p_merge_還是會往前走

> 在mainFrame時確認有lose, 且速度 > 8m/sec 的track (lose可能來與自己frame lost或是與其他顆lidar frame lost)
> 因此對於移動快的物體, 不會因為沒associate到x_merge_/p_merge_就不變, 還是會變, 只是相對慢一些（因為有rm, ctrv weight掉, 不會像cv那麼快）

這個應該是因為中間有幾個frame lose而被砍掉了, 把0.3s拉長就有了(1s 可以保留)

*Example*
1124/moving_motors_wrong_association_s200_0.5x: 38 sec (top)
1125/moving_motors_s200_0.5x_remove_unstable_trajectory: 36 sec (down)
可以看到moto亂連的情況發生在剛產生/unstable時

> 兩個岔開的箭頭, 其後軌跡不同. 上面是所有軌跡, 下面是只show stable tracking的軌跡
撇開軌跡不談, 其實就是速度的問題, 軌跡的凌亂表示他在unstable時速度是亂的, 所以會刺出來

**Mask掉unstable的tracker後, 軌跡什麼的好很多, 所以大部份是在剛開始追時因為uncertainty大, 比較容易不穩, 會連錯**

#### lidar ghost/duplicate in single frame
Lidar distorsion from vehicle motion(linear & rotation)
對於在lidar y軸方向容易會有track不連續的問題, 尤其在相對速度高時
源自於lidar由velodyne coordinage y軸(azimuth = 0)開始轉, 因此在y軸方向, 若在0.1s(10hz)內一物體通過y軸, 或者因為lidar轉超過360度,則會造成同一個frame/timestamp 掃到同一個物體兩次.
=> 移動物體：機車會duplicate, 車子會加長

> Note: In VeloView, one rotation can be referred to as a single “frame” of data, **beginning and ending at approximately 0°
> azimuth**. The number of frames per second of data generated depends entirely on the RPM setting, e.g. 600 RPM / 60
> s/min = 10 frames per second.
[source : [vpl-32c manual 8.3](https://icave2.cse.buffalo.edu/resources/sensor-modeling/VLP32CManual.pdf)]

> 此例與機車相對速度大約為18m/sec, 車子本身移動大約9.5m/sec
> 機車和電線桿都有ghost的現象
> 電線桿和樹應該共只有兩隻, 背景有一些疊影

#### visulization rule (1125 update)
在unstable(< 4 frames)時, 只show出紅色箭頭, 並不紀錄軌跡
stable後show出所有marker, 看起來trajectory有好一點（在unstable時會比較亂, 因為uncertainty大, 鄰近機車容易亂連）

把unstable階段視為noise smoothing, 不輸出tracker -> 缺點, 以系統來講, 4 frames, 10m/s的車, online會有4m會看不到（尚在updata) , local smoother? second initialization?

### 1130 個meeting

keypoint
若不要做單個scan的tracking
1.不要等一個scan 收到就做
2.做scan內的tracking, 若考慮每個點的時間戳, 去在scan內argue是否是同一個物體, 等於在scan內又tracking
argue在相對速度高的情況下, 以scan為單位的tracking會有object duplicate的問題

old points
smoothing tracjectory（post-process)

[[GitHub: assign timestamp to each point in one scan](https://github.com/kaarta/velodyne/commit/c884af4b8d89d6d585bf354dcdfdb98053e95348#diff-5abe564ce2c4cba7c9ad177d6078dcb9dcc050ebb054a7dd65a1ca93b5fe762a)]

[source : [vpl-32c manual 9.3: data definition](https://icave2.cse.buffalo.edu/resources/sensor-modeling/VLP32CManual.pdf)]

目前看來, 少部份提到lidar distorsion的問題, 都是討論當ego有速度, 周圍環境靜止, 主要是在localization/mapping會產生的問題.
但對於我的問題而言, 比較著重在本車和移動物體間因為有相對速度，在經過boundary時會因為duplicate對tracking產生問題. 不太需要對整個frame做distorsion correction？

ref:
1. LiDAR point clouds correction acquired from a moving car based on CAN-bus data
supplement: [Runge-Kutta Localization](https://www.dis.uniroma1.it/~oriolo/amr/slides/Localization1_Slides.pdf)
3. [[https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9128372](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9128372)]

裡面提到當ego有angular/linear motion時, 在mapping/localization的時候會因為靜止物體的distorsion而降低localization的accuracy, 因此需要做correction

~~看不太懂他lidar在各個ray是怎麼估的, 用本車和轉的角度比例去估ray的motion(為何會與轉的角度有比例關係？）~~

=> 因為裡面所有delta表示的是displacement, 並非velocity!!!

根據估出來的ray的motion, 再進一步去求此motion對ray產生的translation&rotation, 就可以用這個把measurement轉回去, 但它看起來很不像是對360度的frame轉回去（∆xα, ∆θα 正比 α , 應該要 （1-α)吧？）


不要以一個scan看lidar問題, 因為現在lidar都不是掃一圈的,所以若以這個角度看, 似乎在未來不是問題? 所以不需要像localization做校正的方式去做, 因為整個scan比例低, 未來又不會有

若以這個想法來看, 改善tracking的方式不用以一個scan為單位去update來解決目前的問題, 會比較通用

### Object duplicate
- [x] duplicate, 不用scan update, 分區塊update
> 若用一般lidar在靜態場景的補償方式去correct point cloud
> 1.去算相對速度, 還是需要知道每個點的timestamp, 且比較難補償（time gap難抓）
> 補償後,在複雜場景中很難判定到底是不是真的新物件或舊的物件, 貿然把補償後重疊的東西拿掉會有風險
> 2.因為物體本身也在移動, 因此這個現象也無法由slam端就先補償掉, 因為slam只能補償ego motion的distorsion, 沒有tracking資訊並不知道object速度
> =>看起來, 加快tracking的rate, 而不要用一個累積scan再做, 是比較合理的

### Issue : center-based tracking
**velocity來自於物體移動還是centroid改變?**
若用cluster center當作tracking state, 會因為看到視角不同, shape形狀改變, 而對object velocity會誤判

最常發生就是靜止物體因為ego靠近, 看到的形狀變化, 導致有一些微速度.

若用box-model去fit cluster, 問題也是一樣, 因為車體變長, box也會變長
若是能夠預先知道他的類別, 並套用相對應的大小的box model, 或許是可行的

- [ ] 找data paper 驗 比較

### Current consideration
* Algorithm:
在只有點的資訊, 很多unknown的情況下, 很難再對tracking的algorithm做處理.
Pure clustering:
1. Object Completeness
Easy to cause split/merge cases, especially in cluttered, urban area, which would influence tracking stability.
3. Object information
Cannot definitely know if the object is of our interest. That is, we need **track ALL kinds of OBJECTs include NOISE/NOT-MATTERS** ones as many as we can to ensure security, making association situation chanllenged.

若可以知道segmentation或detection資訊（類別）, 會改善很多.

* Data:
目前幾乎沒有看到dataset是有很多機車的, 也沒什麼paper在討論點雲上的機車追蹤, 因此在驗證比較'機車追蹤'這塊不確定可行性, 若主打創新, 或許得有一些新的idea來專門處理機車追蹤.

若用自己的data, 可能lay起來不好實行（多,且有些即使在cam也看不太出來）, 因為lidar擺放的關係, 在市區其實大部份車輛都處於盲區, 對於機車也就可能只看得到頭部幾個點, 若要在我們的data上用geometry去分析機車可能不太有利.

fit object bounding box: 對於小型機車/行人改善效果不顯著（小,可model成point tracker), 對於車輛, 因為本身看到的點雲會改變, 就算去fit polygon成box, 還是會有box center displacement的問題.

用SemanticKitti有point label的追追看, 把一些不要的類別拿掉, 然後做cluster-track
可以看看cluster和tracking對於較complete點雲的情況. 但相較在real-data這塊的觀點就比較薄弱.

觀察：
機車的追蹤, 感覺跟大多數追蹤問題一樣, 很仰賴detection/segmentation的情形.

**Delima: how to segment motor out base on partial and few point geometry, under the unknown/cluttered circustance.
As we want more motors to be seen in 30m+, more cluttered/surrounding object would emerge and track would be messed up**

Big covariance can cause wrong association:
1229 tracking
2:16 -> bag 139sec
confine covariance in tracker, not explode to param [prevent_explosion_thres_]

[rviz_cov_eigen_vector](https://answers.ros.org/question/11081/plot-a-gaussian-3d-representation-with-markers-in-rviz/)
[eigen](https://stackoverflow.com/questions/50458712/c-find-eigenvalues-and-eigenvectors-of-matrix)

## merge detector on SemanticKitti
還沒看到動靜物體merge case 因此都是因為車體車窗透射產生的小破碎 誤判為merge而錯誤分開

這種車體小破碎在icp match時都可收斂，但會發現有時match根本錯誤，因此需要再判定fitnessScore來決定此次收斂是對或錯

因此現在分開條件更嚴苛
1. 在物體icp都可以很好match（converge & **fitnessScore < thres**)
2. 判斷他們的motion差異大
才會分開

根據觀察 motionDiff > 0.15的他們的fitnessScore大部分都太大（有0.4+ 至少都0.1起跳）
match_score_threshold_ 可設安全值0.1濾掉因為錯誤match產生的錯誤motionDiff

!!! 要確保, 真正要分開的機車和車, 他們都有滿足fitnessScore夠小才行 !!!

測試rigid_motion_threshold_ 0.15有點小 可以到0.2

## Issue: association due to object disappear
對於已經追蹤一陣子的物體, 行進時會因為object消失而連結到旁邊有在association範圍內的物體
產生一個不合理的shift association
常發生, 較遠的物體看不到, 連到近的物體, 產生一個不合理的速度



因為association object仍在0.99信賴區間範圍內, 因此會associate過去, 但就常理來說, 這麼大的速度不可能有一個這麼突然的轉向
若只用距離去associate會不足

但要確認
1. 轉彎時會不會fail
2. 因為用質心追會有些抖,怕這種會因為左右抖而不在fov內（確認光復路高架橋下那段對象來車
3. 速度小的不考慮association fov 因為轉角可以很大

用association fov加上限制（對速度較大的物體,較小的轉向比較沒限制）

1.用歷史velocity去smooth, 計算smooth_v的p, 進而推算smooth後的範圍smooth_S_
=> v算完的結果過於smooth, 因此smooth_S_範圍非常小, 若加上x_merge_p和measurement noise, 就完全被這兩項吃掉, smooth_S_幾乎沒貢獻
覺得算法有點過於coupling, 等於是拿kf濾過得東西再算一次, 幾乎是certain了
2.由角度著手, 去看state dyaw的變化
發現因為dyaw在rm和cv都是0,只有ctrv會承襲上一個state, 所以幾乎都是呈現0（1e-3), 若smooth這個沒有太大意義

## 0222-邊邊雜點det限制thres觀察
**但過只是邊邊雜點, 加上map好像比較快？**
S cov初始為identity, association region = 1 * 6 m(方圓)
最後region會收斂到3.5-4m左右（R measurement noise的std dev給定植0.5, 0.5*6=3）, 其中3m是r貢獻的固定值.

觀察旁邊分隔島或靜止物體, 容易因為association亂跳而有比較大的region
發現p_merge_ determinant世界小, 10(-2)等級, 但有時候S cov很小確有50(初始值?)
因此限制association/explosion應該看S比較準
S det收斂會在10(-2)~10(-1)左右, 初始會1(identity)
> cylinder為S,會顯示過去三個associate的S, 其上數值為相對應的S_det,右上邊欄長到6.67

## Covariance study
> tracking_num and corresponding p_v

**tracker pv很小=> 很穩定一直往那個方向收斂 => 不代表這個方向associateion是正確的**

只要有持續associate到, tracker會持續收斂, p的位置或速度var都會變小, 因此不能單靠var來判斷這個track是不是有效, 有可能亂連得到很大的速度但var很小（一直associate到, confidence大）

以位置為例, 最後s_x, s_y會收斂到只有r的值（預設std = 0.5m), p_x, p_y極小

## 4D Panoptic seg
* 現在seg的效率會大大影響tracking的結果, 因此若只考慮單一frame去segment, tracking結果會很敏感, 是否可加入時間的考量, 做多個frame的累積?
* 一般tracking-by-detection paradime建立在比較穩定, 且不會有碎掉/occlusion的detection上, 因此在做object-based association問題是只單純考慮一對一的關係, 不會有seg產生的多對一的關係
* 在urban case, 很容易因為partial/fully occlusion的關係而
* partial: 形狀改變/碎裂, 導致速度/associate估測不准
* fully: 機車可能完全被遮擋, 遮擋後的track不穩associate到鄰近object, 速度戳

碎掉的case而言, 可以透過疊frame再seg的方式救回來
但形狀改變的部分, 雖然tracker associate是穩的, 但速度會有些微變化

(Point-to-track association) As in merge detector by (may not need)
tracker accumulated GMM model on spatial and temporal aspect (tracking and detection are coupled) - not cleared, still need initial segmentation to build tracker Gaussian model

*透過tracking可得知下一個geometry model所在, 將過去看預測的model用time decay weight組成一個GMM(on geometry)的軌跡(spatial+temporal),　用這個軌跡去對多個frame clustering, 應該可以解決遮擋造成的split問題(split前是單個tracker, 碎塊應會分在同一個tracker), 但這個方法怎麼產生新的track?, 沒分到的是outlier or new track? 因為我們等於是把過去tracker當成binary的objectness map, 但過去沒追到的地方我們怎麼initial新的track?

seg的問題可分為
* 單個frame去做可能會受到sparsity.penetration的影響而有碎塊 -> sparsity可用疊frame救回來
* 因為occlusion而碎掉

## Lost tracker issue from cracks diturbance
檢查累積2禎的clustering時
1. **2020-09-11-17-31-33.bag** s=27sec
前方車輛因為碎掉而一直連不到, 一直掉
23-27
看原video不會掉阿
阿我改回groupTargets用update time或不累積frame做結果都是掉,待確認
推測, 碎塊間的距離太大(大於groupTargets()的0.5), 導致無法被close, 一直互相連而拖著長尾巴

1. **2020-09-11-17-31-33.bag** s=280sec
黑衣騎士在穿過前面兩輛車中間時merge未偵測到, 因此只分成兩輛車的track而非加上騎士的三個track
原本可以分粗乃的阿!! (cluster_min？outlier？)

## 0421 - 檢視現在merge detector

[link]( ####1124 )
針對很近的object，當segmentation無法正確分割出的情況下，可以透過以往的tracking救回來。
目前是考慮前一禎的結果去做判斷（還不確定如何多禎去決定）

分出來的東西：有motion差(motion-based)，且跟tracker做ICP可以match到的object(appearance based)
對於機車在穿梭車陣時有不錯的效果

** 盲點 **
發現對於速度快的object, 分的不太好, 原因應該在**icp那步沒有converge**, 導致不會啟動或分不出來，**比較常發生在機車,尤其是移動較快或較遠的機車**
icp: 把initial guess畫出來，會跟predict_polygon(&measurement)有落差，不確定是哪部份出問題，明明tracker的cloud和hull是用同樣的方式去轉得到pred的阿...


三個點雲為initial guess, 紅色polygon和marker為pred_x_cv的結果。hull透過x_merge_和x_cv_轉回的結果跟meas很吻合，但對點雲一樣先轉回物件原點，在給initial guess的pred_x_cv時用一樣方式去轉得到的cloud有差距。（點雲應該要跟red polygon吻合，在polygon內）
（黃色polygon為重新分群結果，這個case因為三個object做icp都有收斂，所以分群有重分出來）

基本上雖然initial guess有些為差距，但看結果都會收斂且貼到measurement，只是有時候會歪歪的（？），然後機車發散的機率比較高。

example:
bag：2020-09-11-17-42-38
[wierd_icp_initial_and_final_match.mp4](https://drive.google.com/file/d/1dOB31jfand6QngxvkREGDRoJxbQ_EvrY/view?usp=sharing)
較貼近object的紅藍綠等有色點雲，表示tracker對這個scan object做icp的結果。 落在後面的有色點雲表相對應給定的initial guess。
有收斂的話才有icp結果，因次大部份車子都有收斂。但機車容易不收斂，導致merge detector不啟動。

### 更動
這裡為debug過程，可略，結論在[這裡]( ###Bug fix for _cloud storage ) 。
:::success
發現把icp的initial guess用track的x_merge_直接去cv predict, 得到的initial guess超準！！！直接貼著measurement 即使是動的很快的機車也可以給到很準，上面機車lose的case也可以收斂，match的很好！！！（上面example現在可以完全分開）
看起來之前initial guess用IMMprediction後的x_cv_超不准，但不確定為什麼，因為我predict_polygon也是用x_cv_但很準
:::

initial guess很好疊合在measurment，也使得後面ICP match結果都會收斂且對的很好，merge都能分開。
example_revised:
bag：2020-09-11-17-42-38
[x_merge_pred_good_icp_initial.mp4](https://drive.google.com/file/d/10Iqwc_27IdKfyj7IrRSiv0p-s9grYtU5/view?usp=sharing)

！！重新檢視之前動的物體分不好的case, 並確定原本動靜分的好的不影響！！
解決2020-09-11-17-31-33_bug_fix_stdabs_pt10.mp4 18:40(s280)沒偵測到的case

**不確定為何採用cv model predict的x會不准，但用x_merge_直接去predict 0.1s超準**
:::spoiler debug close

好像不是x_merge_ predict比較準，而是若用0.1s而非任兩frame去predict會比較準。若用dt對x_merge_去predict，效果跟原本用IMMprediction得到的x_cv_同。

為何？即使收到不是top lidar, x_merge_應該也會一直update，是累積起來的誤差比較大？
猜測：

還是對tracker來說，當你每次update都不一定有associate的情況下（收到旁邊lidar時，其他tracker是沒有associate到的，形同lose），~~因此估出來的v並不是0.1s內真正的平均速度（走走停停的感覺，速度會因為沒有associate到時而被拉下來），因此用這個v去做很小的dt估測（不同lidar間收到的時間差小）會比較慢，幾乎不動？~~
但若直接在main frame時用0.1去估下一個main frame位置，可以很精準得到，所以速度好像也是夠的？
所以＝>感覺在side lidar的時候x_merge_幾乎沒有update，可能估成rm之類的，到top時才又回來cv
因為如果在lose時判成靜止model，則x_merge_經model加權後結果幾乎不會動，**但cv model速度還是維持正確的估測**，變成我merge若用dt去估就等於不會動（cv model速度對但前一個frame時update得到的x_merge_位置不準，等同於維持上個main frame的位置，但dt不是與上個main frame的距離差），必須用0.1去估（top lidar freq)，**當成在過程中x_merge_不動**
!!!本來以為IMMprediction也調成0.1s 得到的x_cv_應該就準，但結果還是跟原來用dt預測一樣不准阿 why!!!
:::

(add GIT COMMIT!!)
### Bug fix for _cloud storage
:::success
結論：x_merge_去predict和x_cv_pred_基本上會一樣(對cv的車子而言，幾乎沒有差)
**這裡initial會不一樣的原因在於target的"cloud"儲存錯誤** 而非predict值的問題
原本我的cloud是直接接global frame下associate到的點雲，所以是global座標
但因為對於top lidar object, 只有0.1s的速率會associate到，但我們predciton和update model是frame來就做（<0.1s) (來自side lidar)
所以如果要維持cloud正確，應該每次也都要對cloud位置prediction，這樣到下個top lidar的cloud位置才會是正確的位置，要不然會是0.1s前的位置，而非上個frame的。
為了避免每次接到lidar時predict cloud都要對點雲shift到原點再predict這樣耗資源的動作，乾脆直接存cloud shift到原點的點雲，這樣只要確保model有prediction, cloud的預測位置就直接由predict的pose去轉就可以了。
這樣得到的cloud每次用x_cv_或x_merge_ predict當initial都會是準的（很好的貼到measurement)
:::

### parameter setting
因為原本一直拿錯的點雲來做icp, 做完的結果不那麼準確，因此參數設定方面可能需要重新設定。
1. overlapping_threshold_
原0.25 -> 0.15
主要因為現在疊2 frame去做clustering, 所以相鄰移動的object有可能associate一起，比如車子和機車，這樣的話他真正overlapping的IOU可能會不到0.25, 要調到0.15這個case才可以分開
（原本會調這個thres是因為怕如果大車...等在上個frame因為碎掉還是什麼在下個frame救回來，則他的面積變化應該會很大（<overlapping_threshold_)，可以判定為occlusion的碎因此不會進入merge, 但現在疊frame去cluster就比較不會這樣了，應該可以放寬/調小)
2. rigid_motion_threshold_ 0.15m/0.1s = 1.5m/s = 5.4km/hr （約人行走速度）
發現調成正確的cloud及準確的initial guess後，merge detector變得很敏感，不確定是不是因為現在看相鄰兩個frame的距離很小（可能0.03-0.05sec), 換算motion difference很小，icp算出來很容易超過的關係。
靜止的東西或移動的車輛有碎片也會持續被我分開。icp變得沒什麼用了，應該要把rigid_motion_threshold_設大（1.8-2)，（但我覺得原本值很合理，1.5m/s是人步行速度，2m/s快走了）.

> 右方待轉區停等機車一直被分開

- [ ] 靜止物體initila guess(cv)是不是對的上的
- [ ] 靜止物體motion difference ds (換算成速度值看 ds/dt)

31
s124 前方兩台機車有比以前容易分開，但還是會merge

##
over-segment for from car: 目前看到討論segmentation的都是用比較dense的data, 看不太到會有這種前車分開的問題
InsClustering: Instantly Clustering LiDAR Range Measures for Autonomous Vehicle
這跟我們的情況幾乎一樣, 是直接用vlp32c去做, 看他的fail case也跟我們一樣(大車碎片, 前車over-seg), 沒有解法.

occlusion:
直接去算occluded region: 用map去算free/occluded space(沒有太大的想法)

由association角度去看:
避免不同類別object互連, 每次associate到跟tracker ICP算score, 把score也納入associate metric, 不只用motion gating region.

## merge detector
1. 若側邊和上面打到同一台車,當track間沒有associate成一個而是同時存在兩個時，會把這台車持續分開，因為看上方的重疊，會判斷為碎塊而把一台車切碎
**對於不同光達的merge, 難以分辨是打到同一個東西（不要分開）還是打到不同東西（要分開）**

若disable from different lidar，有一種想要的case是機車穿過我和路邊東西，若機車在側邊merge但在上面有獨立出來，那這種情況應該要透過不同光達間分開就會被關掉了（交大下光復42 s17-27原本分的好)

## 0701 LShape fitting
參考[An Efficient L-Shape Fitting Method for Vehicle Pose Detection with 2D LiDAR](https://ieeexplore.ieee.org/document/8665265)方法, 對Cluster進行LShpae fitting
會進行這個步驟主要考量:
1.比起用質心, 得到接近車體中心的box中心, 對於跟gt比對也會比較準確
2.再者，對於穿透車體產生的小碎塊，也可以用這個fit後把convex沒overlap但fitting後box有overlap的小物件拿掉(原本只拿掉overlap convex, 像巴士的碎塊)
可以拿掉巴士碎塊或穿透車體的碎塊

> 空心框為參考paper的方法算出的box，實心框為修改其中vertices選法得到。paper的選法比較適用在L-Shape很明顯的車輛上，若選不好orientation會有點歪。修改後的方法較貼合車輛heading。

> 圈起來部分為lidar穿透車輛而在車輛另一側產生一小碎塊。

::: warning
最後box fitting方式採先用RANSAC對點雲(BEV, 2d)找線段, 當成一邊, 再用垂直線當另一邊, 這樣fit效果比上面兩者好, car和機車的orientation也不會歪掉
:::

問題:
如何把邊緣點挑掉?這些東西屬於FP. FP會影響cluster甚至tracking(ex. 建築, 分隔島等靜態非欲追蹤物體)
如何在不分動靜的情況下, 能夠把所有車機車行人保留.(希望可以像DL做到動靜都有, 而非只抓動的)

想法
1.由map著手(較耗時)
semantic label建築物等東西的geomertic map, 在running的時候動態把這附近的拿掉(可能拿掉離建築很近的行人等)
建置geometric map, 比對grid有無東西拿掉? (則這個map要乾淨, 不能有周圍想偵測的東西, 只能有靜態場景)
2.由algo著手(不確定對於雜點的分類效果好不好)
看能不能對偵測出來的東西分類, 把分為人,車,機車以外的東西拿掉(outlier)

目前有看到針對人車機車分類, 是看axis # 還要再看看效果, 能不能判別出這些類別以外的東西.

## merge detector
發現更新頻率會影響tracker merge_x 位置
若是三顆lidar去predict/update, 則更新較快, 得到的merge_x位置較準(離measurement近), 但若只用一顆lidar去update, x_merge更新較慢, 可能會稍微偏掉, 就會影響merge detector重分群的效果
(nctu轉彎下來三台車)

## Geometry-cov
現在是直接用上個associate object的geometry, 可以試試看也放入KF用constant update?(像一般lwh放入KF去predict)
這樣可能不會因為det變化而有劇烈的geo改動, 且不會過分依賴上一個associate到的物體的object, 而有tracking smooth

問巧華怎麼將cylinder3D pre-train model inference在不同resolution的lidar (64 to 32 beam)

## 0920 ground filter
測試發現原本的Gaussian Process filter會把部份車身接近地面處當成ground, **(尤其是在occlusion較多, 地面點相對不足的市區)**, 過多的FP會導致ICP matching點不足而diverge.
改成PCL ransac fit更不好, 連地面都找不太出來.
後改成這篇方法，則在接縫處的segment好很多, 但因為是透過高度z選取initial seed點, 這個方法在坡度大的case容易fail, 但在urban效果蠻好.
([git](https://github.com/VincentCheungM/Run_based_segmentation/blob/master/nodes/ground_filter/groundplanfit.cpp))

因為segment出來的ground FP變少, 也就是非地面點變多, 因此更改cluster min_pt_num參數 5 -> 10, 避免noise.

## Running issue note
### IMM running issue


Solution:
[[explanation](https://answers.ros.org/question/209452/exception-thrown-while-processing-service-call-time-is-out-of-dual-32-bit-range/)]
因為使用simulation time跑, /clock發之前ros::Time::now()都是0, 才會使的相減負數, 導致abort
[[ros::Time, ros::Duration](https://blog.csdn.net/u013834525/article/details/83863992)]

**若只是要跑, 只要用try,catch 包起來, catch印個error讓程式可以繼續運行就好.**

### TF issue
While using bag data, stop but get following message, and rviz abort.
> Lookup would require extrapolation into the future. Requested time 1599817098.592198000 but the latest data is at time 1599817098.492717000, when looking up transform from frame [base_link] to frame [map]

會發生明明topic已經收到1599817098.5的msg, 但tf只有1599817098.4的資料, 這是因為[ROS tf的發布時間會比topic晚](https://blog.csdn.net/lemonxiaoxiao/article/details/108715176), 所以若要準確地接收到bag內的tf, 必須使用/clock的時間而非system時間, 所以要use_sim_time = true

等同當pause bag時, node內也一併暫停, 因此不能在while loop裡面做事情.

## 0218_Object local frame (Occupancy grid occlusioin)
object local frame of point cloud & polygon:
Local frame is located by inverse transforming global pose (map) **x_merge_** tracking result, NOT by Object measurement positioins.
This two would differ because x_merge_ is filtering/smooth result and would respond little delay to measurement.

- [x] Pred_poly和cloud_在local frame也會match在一起

## Occlu
1. frame-by-frame occupancy comparison有時候會不容易抓出來(相較track mapping), 因為若經過occluder速度不夠快或track predict不夠, 在overlap那階段就會抓不出split的情況
> 但Predict Overlap得判斷不知道要用甚麼換, 甚至在image上也都是這box overlap來判斷. 會受到Predition的uncertainty 影響
2. 有時split不是因為occlu, 但會因為tracker多少會被遮擋而判定成要remerge, 但**用occlu_ratio又不準**(occlu_ratio多或少不代表遮擋over-seg的絕對)
- [ ] 可以看occlude grid in track是否沒有落在object(mea) grid來決定?

有很多部分的split其實不是因為遮擋, 而遮擋是多少都會發生

Try:
Occupation prob model:
對每個grid的state做Gaussian blur

1.限點數下有點(為了避免單獨一個點的noise) => ptExist = 1 (原本GridState的occupied)
透過3x3 Gaussian kernel
將周圍ptExist值 {0,1}做convolve
convolve結果值([0, 1])就是這個center grid的p(O=1)

然後p > 0.5?? 決定最後GridState真正occupied (所以有些free可能會變occupied, occupied變free, 會參考周圍grid的情況而定)

2. 訂一個floating point. 約為3x3大小的Gaussian kernel(在R和Theta的六倍標準差約為3x3 grid resolution大小, 因為六倍標準差內所得到的約為99%的資料點, 其CDF約為1) , 對kernel內**所有點**進行計算, 結果即為P(O=1)
會比較耗時, 但會以較容易體現grid內pt的distribution情況, 越邊緣點貢獻少

## evaluation
segmentation方面:
對於over- under-可以由precision/recall變化著手
tracking方面:
則是直接看最後mota值, ids程度(碎掉或Merge就會可能有ids)

Segmentation問題: 如何處理FP(非traffic particiant, building, pole, traffic sign...,ect.)

1. point-unit based
DL-based
以semantic labeling的基礎的話, 在意的是**每個點有沒有分對** 因此metrics會以point-unit based為主, 像是point-wise去比對IOU

2. object-based metric
但以我的觀點以trackng為出發, 著重的是在object/instance-based角度, **分出的數目, over- under- seg的程度**, 以往的跟clustering相關的paper使用的evaluation metric:

a. 以下使用同一種metric(同1), 符合我想要的比法, 但這裡對於FP的定義我不太了解, 像是**buildings, bushes等不可數的stuff, 並不算在ground truth內**, 看起來演算法也會output各式各樣的cluster, 並沒有特別過濾, 但FP卻不高, precision也很高
* Real-Time and Accurate Segmentation of 3-D Point Clouds Based on Gaussian Process Regression, IEEE Transactions on Intelligent Transportation Systems 2017. [[video](https://www.youtube.com/watch?v=6MMlw8uGlcI)]

* InsClustering: Instantly Clustering LiDAR Range Measures for Autonomous Vehicle, ITSC 2020.

b.有些**沒有提供任何quantative evaluation**:
* Fast Segmentation of 3D Point Clouds: A Paradigm on LiDAR Data for Autonomous Vehicle Applications, ICRA 2017.

c.有些**不考慮FP, 只看TP rate(TP#/GT#)**, 這樣對於我這種seg方法來說比較吃香, 因為我這個方法在TP, FN都會表現得比較好, 但無法區分FP(classification)
* Fast Range Image-Based Segmentation
of Sparse 3D Laser Scans for Online Operation, IROS 2016.
semantically label各式各樣的東西, tree. bushes...,ect.

## confidnece-based
[score refinement for confidence-based 3d multi-object tracking](https://arxiv.org/abs/2107.04327)