任何一個系統(tǒng)的感知算法里，僅僅有深度學(xué)習(xí)是不夠的，一定要有數(shù)據(jù)驅(qū)動的模型計(jì)算（模式較重，需要收集數(shù)據(jù)），同時也有面向下游的、后處理的計(jì)算（模式輕，見效快）。

后處理的計(jì)算主要分3種，分別是2D-to-3D的幾何計(jì)算、時序計(jì)算、多相機(jī)環(huán)視融合。

2D-to-3D的幾何計(jì)算（分割的方法）：

受相機(jī)pose的影響（相機(jī)自身仰俯、消失點(diǎn)判斷）

接地點(diǎn)（前提：相機(jī)水平、地面水平、相機(jī)高度已知情況下，可通過三角形解答，同時也可看出相機(jī)pitch角對回3D的影響），2D框（假設(shè)每輛車有8個頂點(diǎn)，將每個頂點(diǎn)都投到圖像當(dāng)中，最終形成的最小外接矩陣即2D框，可用于給障礙我約束邊界），絕對尺寸多條途徑回3D（最終融合得出精準(zhǔn)的結(jié)果）的影響

穩(wěn)定性至關(guān)重要（安全駕駛的保證）

時序信息計(jì)算（跟蹤要求至少15fps，高速情況下要求更高）：

對相機(jī)幀率和延時有要求（必須很低）（總耗時=分耗時×障礙物個數(shù)）

充分利用檢測模型的輸出信息：特征（feature map可理解為對圖像高層語義的刻畫，再將feature map做roi pooling后可得到障礙物級別的feature，將這些feature用來做跟蹤和關(guān)聯(lián)，就可以消化掉傳統(tǒng)跟蹤中計(jì)算量很大的提取特征步驟）、類別（輸出是人還是車，在跟蹤關(guān)聯(lián)是也是有幫助的，因相應(yīng)來說這一幀后下一幀的類別不會發(fā)生跳變，所以這種信息要加以利用）

可以考慮輕量級Metric Learning（因?yàn)镃NN feature并未對instance做出區(qū)分，所以在這其中可加入一些輕量級Metric Learning來做專門針對產(chǎn)品的feature）

多相機(jī)的環(huán)視融合：

相機(jī)布局決定融合策略（環(huán)式布局嚴(yán)重影響融合策略，做好視野重疊，即做好冗余才能保證回3D和識別質(zhì)量）

在后處理的計(jì)算中，2D-to-3D的幾何計(jì)算、時序計(jì)算、多相機(jī)環(huán)視融合等因素密不可分，模塊之間的算法需相通。這也要求開發(fā)團(tuán)隊(duì)積極配合，形成一個有機(jī)的整體。

以下是Apollo社區(qū)開發(fā)者朱炎亮在Github-Apollo-Note上分享的《HM對象跟蹤》，感謝他為我們在tracking這一步所做的詳細(xì)注解和釋疑。

面對復(fù)雜多變、快速迭代的開發(fā)環(huán)境，只有開放才會帶來進(jìn)步，Apollo社區(qū)正在被開源的力量喚醒。

HM對象跟蹤器跟蹤分段檢測到的障礙物。通常，它通過將當(dāng)前檢測與現(xiàn)有跟蹤列表相關(guān)聯(lián)，來形成和更新跟蹤列表，如不再存在，則刪除舊的跟蹤列表，并在識別出新的檢測時生成新的跟蹤列表。 更新后的跟蹤列表的運(yùn)動狀態(tài)將在關(guān)聯(lián)后進(jìn)行估計(jì)。 在HM對象跟蹤器中，匈牙利算法(Hungarian algorithm)用于檢測到跟蹤關(guān)聯(lián)，并采用魯棒卡爾曼濾波器(Robust Kalman Filter) 進(jìn)行運(yùn)動估計(jì)。

上述是Apollo官方文檔對HM對象跟蹤的描述，這部分意思比較明了，主要的跟蹤流程可以分為:

預(yù)處理；(lidar->local ENU坐標(biāo)系變換、跟蹤對象創(chuàng)建、跟蹤目標(biāo)保存)

卡爾曼濾波器濾波，預(yù)測物體當(dāng)前位置與速度；(卡爾曼濾波階段1：Predict階段)

匈牙利算法比配，關(guān)聯(lián)檢測物體和跟蹤物體；

卡爾曼濾波，更新跟蹤物體位置與速度信息。(卡爾曼濾波階段2：Update階段)

進(jìn)入HM物體跟蹤的入口依舊在LidarProcessSubnode::OnPointCloud中：

在這部分，總共有三個比較繞的對象類，分別是Object、TrackedObject和ObjectTrack，在這里統(tǒng)一說明一下區(qū)別：

Object類：常見的物體類，里面包含物體原始點(diǎn)云、多邊形輪廓、物體類別、物體分類置信度、方向、長寬、速度等信息。全模塊通用。

TrackedObject類：封裝Object類，記錄了跟蹤物體類屬性，額外包含了中心、重心、速度、加速度、方向等信息。

ObjectTrack類：封裝了TrackedObject類，實(shí)際的跟蹤解決方案，不僅包含了需要跟蹤的物體(TrackedObject)，同時包含了跟蹤物體濾波、預(yù)測運(yùn)動趨勢等函數(shù)。

所以可以看到，跟蹤過程需要將原始Object封裝成TrackedObject，創(chuàng)立跟蹤對象；最后跟蹤對象創(chuàng)立跟蹤過程ObjectTrack，可以通過ObjectTrack里面的函數(shù)來對ObjectTrack所標(biāo)記的TrackedObject進(jìn)行跟蹤。

預(yù)處理階段主要分兩個模塊：A.跟蹤建立(track setup)和B.預(yù)處理(preprocess)。跟蹤建立過程，主要是對上述得到的物體對象進(jìn)行跟蹤目標(biāo)的建立，這是Track第一次被調(diào)用的時候進(jìn)行的，后續(xù)只需要進(jìn)行跟蹤對象更新即可。建立過程相對比較簡單，主要包含：

物體對象坐標(biāo)系轉(zhuǎn)換(原先的lidar坐標(biāo)系-->lidar局部ENU坐標(biāo)系/有方向性)

對每個物體創(chuàng)建跟蹤對象，加入跟蹤列表

記錄現(xiàn)在被跟蹤的對象

從上面代碼來看，預(yù)處理階段兩模塊重復(fù)度很高，這里我們就介紹Initialize對象跟蹤建立函數(shù)。

這里我們注意到一個平移向量global_to_local_offset_，他是lidar坐標(biāo)系到世界坐標(biāo)系的變換矩陣velo2world_trans的平移成分，前面高精地圖ROI過濾器小節(jié)我們講過:local局部ENU坐標(biāo)系跟world世界坐標(biāo)系之間只有平移成分，沒有旋轉(zhuǎn)。所以這里取了轉(zhuǎn)變矩陣的平移成分，其實(shí)就是world世界坐標(biāo)系轉(zhuǎn)換到lidar局部ENU坐標(biāo)系的平移矩陣(變換矩陣)。P_local = P_world + global_to_local_offset_

從上面的TransformPoseGlobal2Local函數(shù)代碼我們可以得到一個沒有平移成分，只有旋轉(zhuǎn)成分的變換矩陣velo2world_pose，這個矩陣有什么作用？很簡單，這個矩陣就是lidar坐標(biāo)系到lidar局部ENU坐標(biāo)系的轉(zhuǎn)換矩陣。

也就是將Object包裝到TrackedObject中，那我們先來看一下TrackedObject類里面的成分：

從上面表格可以看到，TrackedObject封裝了Object，并且只增加了少量速度，加速度等額外信息。

ConstructTrackedObjects是由物體對象來創(chuàng)建跟蹤對象的代碼，這個過程相對來說比較簡單易懂，沒大的難點(diǎn)，下面就解釋一下具體的功能。

針對vector& objects中的每個對象，創(chuàng)建對應(yīng)的TrackedObject，并且計(jì)算他的shape feature，這個特征計(jì)算比較簡單，先計(jì)算物體xyz三個坐標(biāo)軸上的最大和最小值，分別將其劃分成10等份，對每個點(diǎn)xyz坐標(biāo)進(jìn)行bins投影與統(tǒng)計(jì)。最后的到的特征就是[x_bins,y_bins,z_bins]一共30維，歸一化(除點(diǎn)云數(shù)量)后得到最終的shape feature

TransformTrackedObject函數(shù)進(jìn)行跟蹤物體的方向、中心、原始點(diǎn)云、多邊形角點(diǎn)、重心等進(jìn)行坐標(biāo)系轉(zhuǎn)換。Lidar坐標(biāo)系變換到local ENU坐標(biāo)系

根據(jù)lidar的世界坐標(biāo)系坐標(biāo)查詢高精地圖HD Map計(jì)算車道線方向

同時函數(shù)CollectTrackedResults會將當(dāng)前正在跟蹤的對象(世界坐標(biāo)系坐標(biāo)形式)保存到向量中，該部分代碼比較簡單就不貼出來了。

在預(yù)處理階段，每個物體Object類經(jīng)過封裝以后，產(chǎn)生一個對應(yīng)的ObjectTrack過程類，里面封裝了對應(yīng)要跟蹤的物體(TrackedObject，由Object封裝而來)。這個階段的工作就是對跟蹤物體TrackedObject進(jìn)行卡爾曼濾波并預(yù)測其運(yùn)動方向。

首先，在這里我們簡單介紹一下卡爾曼濾波的一些基礎(chǔ)公式，方便下面理解。

一個系統(tǒng)擁有一個狀態(tài)方程和一個觀測方程。觀測方程是我們能宏觀看到的一些屬性，在這里比如說汽車重心xyz的位置和速度；而狀態(tài)方程是整個系統(tǒng)里面的一些狀態(tài)，包含能觀測到的屬性(如汽車重心xyz的位置和速度)，也可能包含其他一些看不見的屬性，這些屬性甚至我們都不能去定義它的物理意義。因此觀測方程的屬性是狀態(tài)方程的屬性的一部分現(xiàn)在有：

狀態(tài)方程: $X_t = A_{t,t-1}X_{t-1} + W_t$, 其中$W_t o N(0,Q) $

觀測方程: $Z_t = C_tX_t + V_t$, 其中$V_t o N(0,R) $

卡爾曼濾波分別兩個階段，分別是預(yù)測Predict與更新Update：

Predict預(yù)測階段

利用上時刻t-1最優(yōu)估計(jì)$X_{t-1}$預(yù)測當(dāng)前時刻狀態(tài)$X_{t,t-1} = A_{t,t-1}X_{t-1}$，這個$X_{t,t-1}$不是t時刻的最優(yōu)狀態(tài)，只是估計(jì)出來的狀態(tài)

利用上時刻t-1最優(yōu)協(xié)方差矩陣$P_{t-1}$預(yù)測當(dāng)前時刻協(xié)方差矩陣$P_{t,t-1} = A_{t,t-1}P_{t-1}{A_{t,t-1}}^T + Q$，這個$P_{t,t-1}$也不是t時刻最優(yōu)協(xié)方差

Update更新階段

利用$X_{t,t-1}$估計(jì)出t時刻最優(yōu)狀態(tài)$X_t = X_{t,t-1} + H_t[Z_t - C_tX_{t,t-1}]$, 其中$H_t = P_{t,t-1}{C_t}^T[C_tP_{t,t-1}{C_t}^T + R]^{-1}$

利用$P_{t,t-1}$估計(jì)出t時刻最優(yōu)協(xié)方差矩陣$P_t = [I - H_tC_t]P_{t,t-1}$

最終t從1開始遞歸計(jì)算k時刻的最優(yōu)狀態(tài)$X_k$與最優(yōu)協(xié)方差矩陣$P_t$

從代碼中我們可以看到，這個過程其實(shí)就是對object_tracks_列表中每個物體調(diào)用其Predict函數(shù)進(jìn)行濾波跟蹤(object_tracks_是上階段Object--TrackedObject--ObjectTrack的依次封裝)。接下去我們就對這個Predict函數(shù)進(jìn)行深層次的挖掘和分析，看看它實(shí)現(xiàn)了卡爾曼過濾器的哪個階段工作。

從上面兩個函數(shù)可以明顯看到這個階段就是卡爾曼濾波器的Predict階段。同時可以看到：

track_predict/predicted_state相當(dāng)于卡爾曼濾波其中的$X_{t,t-1}$,belief_anchor_point_和belief_velocity_相當(dāng)于$X_t$,ity_covariance_交替存儲$P_t$和$P_{t,t-1}$(Why?可以從上面的卡爾曼濾波器公式看到$P_t$在估測完$P_{t,t-1}$以后就沒用了，所以可以覆蓋存儲，節(jié)省部分空間)

狀態(tài)方程和觀測方程其實(shí)本質(zhì)上是一樣，也就是相同維度的。都是6維，分別表示重心的xyz坐標(biāo)和重心xyz的速度。同時在這個應(yīng)用中，短時間間隔內(nèi)。當(dāng)前時刻重心位置=上時刻重心位置 + 上時刻速度*時間差，所以可知卡爾曼濾波器中$A_{t,t-1}equiv1$, $Q = I*ts^2$

該過程工作:首先利用上時刻的最優(yōu)估計(jì)belief_anchor_point_和belief_velocity_(等同于$X_{t-1}$)估計(jì)出t時刻的狀態(tài)predicted_state(等同于$X_{t,t-1}$); 然后估計(jì)當(dāng)前時刻的協(xié)方差矩ity_covariance_($P_{t-1}$和$P_{t,t-1}$交替存儲)

這個階段主要的工作是匹配CNN分割+MinBox檢測到的物體和當(dāng)前ObjectTrack的跟蹤物體。主要的工作為：

A. Object和TrackedObject之間關(guān)聯(lián)矩陣association_mat計(jì)算

B. 子圖劃分，利用上述的關(guān)聯(lián)矩陣和設(shè)定的閾值(兩兩評分小于閾值則互相關(guān)聯(lián)，即節(jié)點(diǎn)之間鏈接)，將矩陣分割成一系列子圖

C. 匈牙利算法進(jìn)行二分圖匹配，得到cost最小的(Object,TrackedObject)連接對

重心位置坐標(biāo)距離差異評分

物體方向差異評分

標(biāo)定框尺寸差異評分

點(diǎn)云數(shù)量差異評分

外觀特征差異評分

最終以0.6，0.2，0.1，0.1，0.5的權(quán)重加權(quán)求和得到關(guān)聯(lián)評分。

各個子項(xiàng)的計(jì)算方式，這里以文字形式描述，假設(shè)：

Object重心坐標(biāo)為(x1,y1,z1)，方向?yàn)?dx1,dy1,dz1)，bbox尺寸為(l1,w1,h1), shape featrue為30維向量sf1，包含原始點(diǎn)云數(shù)量n1。

TrackedObject重心坐標(biāo)為(x2,y2,z2)，方向?yàn)?dx2,dy2,dz2)，bbox尺寸為(l2,w2,h2), shape featrue為30維向量sf2，包含原始點(diǎn)云數(shù)量n2。

則有：

重心位置坐標(biāo)距離差異評分location_distance計(jì)算

$location-distance = sqrt{{(x1 - x2)}^2 + {(y1 - y2)}^2}$

如果速度太大，則需要用方向向量去正則懲罰，具體可以參考代碼

物體方向差異評分direction_distance計(jì)算

方向差異其實(shí)就是計(jì)算兩個向量的夾角:

$cos heta = a·b/(|a|·|b|)$

夾角越大，差異越大，cos值越??；夾角越大，差異越大，cos值越大

最后使用1-cos計(jì)算評分，差異越小，評分越大。

標(biāo)定框尺寸差異評分bbox_size_distance計(jì)算

代碼中首先計(jì)算兩個量dot_val_00和dot_val_01：

這兩個量有什么意義？這里簡單解釋一下，從計(jì)算方式可以看到：

其實(shí)dot_val_00是兩個坐標(biāo)的點(diǎn)積，數(shù)學(xué)計(jì)算形式上是方向1投影到方向2向量上得到向量3，最后向量3模乘以方向2模長，這么做可以估算方向差異。因?yàn)?，?dāng)方向1和方向2兩個向量夾角靠近0或180度時，投影向量很長，dot_val_00這個點(diǎn)積的值會很大。dot_val_00越大說明兩個方向越接近。

同理dot_val_01上文我們提到過，差積的?？梢院饬績蓚€向量組成的四邊形面積大小，這么做也可以估算方向差異。因?yàn)椋?dāng)方向1和方向2兩個向量夾角靠近90度時，組成的四邊形面積最大，dot_val_01這個差積的模會很大。dot_val_00越大說明兩個方向越背離。

點(diǎn)云數(shù)量差異評分point_num_distance計(jì)算

$point-num_distance = |n1-n2|/max(n1,n2)$

外觀特征差異評分histogram_distance計(jì)算

$histogram-distance = sum_{m=0}^{30} |sf1[m]-sf2[m]|$

子圖劃分首先根據(jù)上步驟計(jì)算的association_mat矩陣，利用超參數(shù)s_match_distance_maximum_=4，關(guān)聯(lián)值小于閾值的判定為連接，否則不連接。最終得到的連接矩陣大小為(N+M)x(N+M)

主要的子圖劃分工作在ConnectedComponentAnalysis函數(shù)完成，具體的可以參考代碼，一個比較簡單地廣度優(yōu)先搜索。最后得到的components二維向量中，每一行為一個子圖的組成元素。

匹配的算法主要還是匈牙利算法的矩陣形式，跟wiki百科的基本描述一致，可以參考主頁匈牙利算法。

這個階段做的工作比較重要，對上述Hungarian Matcher得到的追蹤對。

計(jì)算真實(shí)的觀測變量，包括真實(shí)觀測到的車速度與加速度${Zv_t}$、$Za_t$

由上時刻最優(yōu)速度與加速度$Xv_{t-1}$、$Xa_{t-1}$ 估測當(dāng)前時刻的速度與加速度$Xv_{t,t-1}$、$Xa_{t,t-1}$

由估測速度與加速度$Xv_{t,t-1}$、$Xa_{t,t-1}$，更新得到t時刻最優(yōu)速度與加速度${Xv_t}$、$Xa_t$

另外這里還要一個說明，ObjectTrack類不僅封裝了TrackedObject類，同時也封裝了KalmanFilter類，KalmanFilter自己保存了上時刻最優(yōu)狀態(tài)，上時刻最優(yōu)協(xié)方差矩陣，當(dāng)前時刻最優(yōu)狀態(tài)等信息。TrackedObject、ObjectTrack里面也有這些狀態(tài)。

注意：KalmanFilter里面是濾波后的原始數(shù)據(jù)(沒有實(shí)際應(yīng)用的限制條件加入)；TrackedObject和ObjectTrack類同樣保存了一份狀態(tài)信息，這些狀態(tài)信息是從KalmanFilter中得到的原始信息，并且加入實(shí)際應(yīng)用限制濾波以后的狀態(tài)信息；ObjectTrack類里面的狀態(tài)是需要依賴TrackedObject類的，所以務(wù)必先更新TrackedObject再更新ObjectTrack的狀態(tài)。

總之三者狀態(tài)更新順序?yàn)椋?/p>

KalmanFilter -> TrackedObject -> ObjectTrack

從上述的代碼可以看到，更新過程有ObjectTrack::UpdateWithObject和ObjectTrack::UpdateWithoutObject兩個函數(shù)完成，這兩個函數(shù)間接調(diào)用kalman濾波器完成濾波更新，接下去我們簡單地分析ObjectTrack::UpdateWithObject函數(shù)的流程。

從代碼中也可以間接看出更新的過程A.1和A.2是更新KalmanFilter和TrackedObject狀態(tài)信息，B是更新ObjectTrack狀態(tài)，這里必須按順序來更新！

主要由KalmanFilter::UpdateWithObject函數(shù)完成，計(jì)算過成分下面幾步：

Step1. 計(jì)算更新評分ComputeUpdateQuality(new_object, old_object)

這個過程主要是計(jì)算更新力度，因?yàn)槊總€Object和對應(yīng)的跟蹤目標(biāo)TrackedObject之間有一個關(guān)聯(lián)系數(shù)association_score，這個系數(shù)衡量兩個目標(biāo)之間的相似度，所以這里需要增加對目標(biāo)的更新力度參數(shù)。

計(jì)算關(guān)聯(lián)力度:update_quality_according_association_score = 1 - association_score / s_association_score_maximum_。默認(rèn)s_association_score_maximum_= 1，關(guān)聯(lián)越大(相似度越大)，更新力度越大

計(jì)算點(diǎn)云變化力度:update_quality_according_point_num_change = 1 - |n1 - n2| / max(n1, n2)。點(diǎn)云變化越小，更新力度越大

最終取兩個值的較小值最為最終的更新力度。

Step2.計(jì)算當(dāng)前時刻的速度measured_velocity和加速度measured_acceleration(這兩個變量相當(dāng)于卡爾曼濾波中的觀測變量$Z_t$)

首先計(jì)算重心速度:measured_anchor_point_velocity = [NewObject_barycenter(x,y,z) - OldObject_barycenter(x,y,z)] / timediff。timediff是兩次計(jì)算的時間差，很簡單地計(jì)算方式

其次計(jì)算標(biāo)定框(中心)速度：measured_bbox_center_velocity = [NewObject_center(x,y,z) - OldObject_center(x,y,z)] / timediff。這里的中心區(qū)別于上面的重心，重心是所有點(diǎn)云的平均值；重心是MinBox的中心值。還有一個需要注意的是，如果求出來的中心速度方向和重心方向相反，這時候有干擾，中心速度暫時置為0。

然后計(jì)算標(biāo)定框角點(diǎn)速度:

A. 根據(jù)NewObject的點(diǎn)云計(jì)算bbox(這不是MinBox)，并求出中心center，然后根據(jù)反向求出4個點(diǎn)。如果NewObject方向是dir，那么首先對dir進(jìn)行歸一化得到dir_normal=dir/|dir|^2=(nx,ny,0)，然后求他的正交方向dir_ortho=(-ny,nx,0)，如果中心點(diǎn)坐標(biāo)center，那么左上角的坐標(biāo)就是: center+dir*size[0]*0.5+ortho_dir*size[1]*0.5。根據(jù)這個公式可以計(jì)算出其他三個點(diǎn)的坐標(biāo)。

B. 計(jì)算標(biāo)定框bbox四個角點(diǎn)的速度:bbox_corner_velocity = ((new_bbox_corner - old_bbox_corner) / time_diff)公式與上面的重心、中心計(jì)算方式一樣。

C. 計(jì)算4個角點(diǎn)的在主方向上的速度，去最小的點(diǎn)最為標(biāo)定框角點(diǎn)速度。只需要將B中的bbox_corner_velocity投影到主方向即可。

最后在重心速度、重心速度、bbox角速度中選擇速度增益最小的最后最終物體的增益。增益=當(dāng)前速度-上時刻最優(yōu)速度

加速度measured_acceleration計(jì)算比較簡單，采用最近3次的速度(v1,t1),(v2,t2),(v3,t3)，然后加速度a=(v3-v1)/(t2+t3)。注意(v2,t2)意思是某一時刻最優(yōu)估計(jì)速度為v2，且距離上次的時間差為t2，所以三次測量的時間差為t2+t3。速凍變化為v3-v1。

Step3. 估算最優(yōu)的速度與加速度(卡爾曼濾波Update步驟)

首先，計(jì)算卡爾曼增益$H_t = P_{t,t-1}{C_t}^T[C_tP_{t,t-1}{C_t}^T + R]^{-1}$，在apollo代碼中計(jì)算代碼如下：

從上面可知，代碼和我們給出的結(jié)果是一致的。

然后，由當(dāng)前時刻的估算速度$X_{t,t-1}$、觀測變量$Z_t$以及卡爾曼增益$H_t$，得到當(dāng)前時刻的最優(yōu)速度估計(jì)$X_t = X_{t,t-1} + H_t[Z_t - C_tX_{t,t-1}]$，在apollo代碼中計(jì)算了速度增益，也就是$X_t-X_{t,t-1}$：

然后對速度增益進(jìn)行平滑并且保存當(dāng)前t時刻最優(yōu)速度以及最優(yōu)加速度。

最后就是速度整流并且修正估計(jì)協(xié)方差矩陣$P_{t,t-1}$，得到當(dāng)前時刻最優(yōu)協(xié)方差矩陣$P_t=[I-H_tC_t]P_{t,t-1}$，在這個應(yīng)用中$C_tequiv1$。

加速度更新與上述速度更新方法一致。

Step4.緩存更新信息

將觀測變量measured_velocity和時間差time_diff緩存，同時使用觀測速度measured_velocity對實(shí)時協(xié)方差矩陣online_velocity_covariance_進(jìn)行更新。

設(shè)置TrackedObject的重心，速度(濾波器得到的t時刻最優(yōu)速度belief_velocity_)，加速度([最優(yōu)速度belief_velocity_- OldObject的最優(yōu)速度]/時間差)，更新跟蹤時長(++age)，目標(biāo)可見次數(shù)(++total_visible_count_)，跟蹤總時長(period_ += time_diff)，連續(xù)不可見時長置0(consecutive_invisible_count_=0)。

由原始KalmanFilter中的各個狀態(tài)信息，加入實(shí)際應(yīng)用中的限制進(jìn)行濾波得到ObjectTrack的狀態(tài)信息，這些信息才是真實(shí)被使用的。

對跟蹤物體的速度整流過程如下(ObjectTrack::SmoothTrackVelocity)：

如果物體的加速度增益查過一定閾值(s_acceleration_noise_maximum_, 默認(rèn)為5)，那么當(dāng)前速度保持上時刻的速度

否則，對小速度物體進(jìn)行修建。計(jì)算物體的速度，默認(rèn)s_speed_noise_maximum_ = 0.4

如果velocity_is_noise = speed < (s_speed_noise_maximum_ / 2),則判定為噪聲

如果velocity_is_small = speed < (s_speed_noise_maximum_ / 1)，則判定為小速度

計(jì)算物體兩個時刻角度的變化，fabs(velocity_angle_change) > M_PI / 4.0，如果cos值小于pi/4(45度)，說明物體沒有角度變化 最終判斷：if(velocity_is_noise || (velocity_is_small && is_velocity_angle_change)) 如果速度是噪聲，或者速度很小方向不變，那么認(rèn)定車是靜止的。 對于車是靜止的，真實(shí)速度和加速度都設(shè)置為0

這里需要注意：

// NEED TO NOTICE: claping small velocity may not reasonable when the true velocity of target object is really small. e.g. a moving out vehicle in a parking lot. Thus, instead of clapping all the small velocity, we clap those whose history trajectory or performance is close to a static one.

按照官方代碼提醒，其實(shí)這樣對小速度物體進(jìn)行修剪時不太合理，因?yàn)槟承┣闆r下物體速度確實(shí)很小，但是他確實(shí)是在運(yùn)動。E.g. 汽車倒車的時候，速度小，但是不能被忽略。所以最好的方法是根據(jù)歷史的軌跡(重心，anchor_point)來判斷物體在小速度的情況下是否是運(yùn)動的。

對跟蹤物體的方向整流過程如下(ObjectTrack::SmoothTrackOrientation)，如果物體運(yùn)動比較明顯velocity_is_obvious = current_speed > (s_speed_noise_maximum_ * 2)(大于0.4m/s)，那么當(dāng)前運(yùn)動方向?yàn)槲矬w速度的方向；否則設(shè)定為車道線方向。

就這樣，經(jīng)過三步驟，跟蹤配對的物體(Object-TrackedObject存在)完成了狀態(tài)信息的更新，主要包括當(dāng)前時刻最優(yōu)速度、方向、加速等信息。

如果跟蹤物體中沒有找到對應(yīng)的Object與之匹配，就需要使用UpdateUnassignedTracks函數(shù)來更新跟蹤物體的信息。從上面我們可以看到，匹配成功的可以用Object的屬性來計(jì)算觀測變量，間接估算出t時刻的最優(yōu)狀態(tài)。 但是未匹配的TrackedObject無法因?yàn)檎也坏絆bject，所以無法了解當(dāng)前時刻真實(shí)能測量到的位置、速度與加速度信息，因此只能依賴自身上時刻的最優(yōu)狀態(tài)來推算出當(dāng)前時候的狀態(tài)信息(注意，這個推算出來的不是最優(yōu)狀態(tài))。

對未找到Object的跟蹤物體，更新過程如下：

使用2.4.2節(jié)中的估算數(shù)據(jù)來預(yù)測當(dāng)前時刻的狀態(tài)；

其中predicted_shift是利用卡爾曼濾波Predict階段預(yù)測到的當(dāng)前時刻物體重心位置與速度狀態(tài)$Xp_{t,t-1}$和$Xv_{t,t-1}$，乘以時間差就可以得到這個時間差內(nèi)的位移，去更新中心，重心；

上時刻TrackedObject里面的原始點(diǎn)云和多邊形坐標(biāo)也加上這個位移，完成更新；

更新KalmanFilter里面的原始狀態(tài)信息，KalmanFilter::UpdateWithoutObject，KalmanFilter只更新重心坐標(biāo)，不需要更新速度和加速度(因?yàn)闊o法更新，缺少觀測數(shù)據(jù)Z，不能使用卡爾曼濾波器的Update過程去更新)；

更新TrackedObject狀態(tài)信息，更新跟蹤時長(++age)，跟蹤總時長(period_ += time_diff)，更新連續(xù)不可見時長置(++consecutive_invisible_count_=0)；

更新歷史緩存。

更新完匹配成功和不成功的跟蹤物體以后，下一步就是從跟蹤列表中刪掉丟失的跟蹤物體。遍歷整個跟蹤列表：

可見次數(shù)/跟蹤時長小于閾值(s_track_visible_ratio_minimum_，默認(rèn)0.6)，刪除；

連續(xù)不可見次數(shù)大于閾值(s_track_consecutive_invisible_maximum_，默認(rèn)1)，刪除。

如果Object沒有找到對應(yīng)的TrackedObject與之匹配，那么就創(chuàng)建新的跟蹤目標(biāo)，并且加入跟蹤隊(duì)列。

最終對HM物體跟蹤做一個總結(jié)與梳理，物體跟蹤主要是對上述CNN分割與MinBox邊框構(gòu)建產(chǎn)生的Object對一個跟蹤與匹配，主要流程是：

Step1.預(yù)處理，Object里面的中心，重心，點(diǎn)云，多邊形凸包從lidar坐標(biāo)系轉(zhuǎn)換成局部ENU坐標(biāo)系；

Step2.將坐標(biāo)轉(zhuǎn)換完成Object封裝成TrackedObject，方便后續(xù)加入跟蹤列表；

Step3.使用卡爾曼濾波Predict階段，對正處于跟蹤列表中的跟蹤物體進(jìn)行當(dāng)前時刻重心位置、速度的預(yù)測；

Step4.使用當(dāng)前檢測到的Object(封裝成了TrackedObject)，去和跟蹤列表中的物體進(jìn)行匹配：

計(jì)算Object與TrackedObject的關(guān)聯(lián)矩陣

重心位置坐標(biāo)距離差異評分

物體方向差異評分

標(biāo)定框尺寸差異評分

點(diǎn)云數(shù)量差異評分

外觀特征差異評分

根據(jù)關(guān)聯(lián)矩陣，配合閾值，劃分子圖

對于每個子圖使用匈牙利匹配算法(Hungarian Match)進(jìn)行匹配，得到、,

成功匹配(有Object計(jì)算觀測數(shù)據(jù))，更新KalmanFilter狀態(tài)(Update階段), 更新TrackedObject狀態(tài)，更新ObjectTrack狀態(tài)

沒有對應(yīng)的Object(無法得到觀測數(shù)據(jù)，無法使用卡爾曼濾波估算最優(yōu)速度)，更新部分KalmanFilter狀態(tài)(僅重心)，跟新TrackedObject狀態(tài)，更新ObjectTrack狀態(tài)

創(chuàng)建新的TrackedObject，加入跟蹤列表

刪除丟失的跟蹤目標(biāo)

可見次數(shù)/跟蹤時長過小

連續(xù)不可見次數(shù)過大

自Apollo平臺開放已來，我們收到了大量開發(fā)者的咨詢和反饋，越來越多開發(fā)者基于Apollo擦出了更多的火花，并愿意將自己的成果貢獻(xiàn)出來，這充分體現(xiàn)了Apollo『貢獻(xiàn)越多，獲得越多』的開源精神。為此我們開設(shè)了『開發(fā)者說』板塊，希望開發(fā)者們能夠踴躍投稿，更好地為廣大自動駕駛開發(fā)者營造一個共享交流的平臺！