int N=tracks.size(); // the number of tracks int M=detections.size(); // the number of points detected // Matrix distance from track N-th to point detected M-th vector< vector<double> > Cost(N,vector<double>(M)); vector<int> assignment; // matrix used to determine N-th track will be join with point detected M-th based on Hungarian algorithm // matrix distance double dist; for(int i=0;i<tracks.size();i++) { for(int j=0;j<detections.size();j++) { Point2d diff=(tracks[i]->prediction-detections[j]); //euclid distance dist=sqrtf(diff.x*diff.x+diff.y*diff.y); Cost[i][j]=dist; } }

这段代码实现了一个轨迹跟踪算法中的匈牙利算法部分，用于将已有轨迹和新检测到的点匹配。首先，得到了已有轨迹的数量N和新检测到的点的数量M。然后，创建了一个二维的Cost矩阵，用于存储从第i个轨迹到第j个点的距离。接着，遍历所有轨迹和所有检测到的点，计算它们之间的欧几里得距离，将其存储在Cost矩阵中。最后，通过匈牙利算法，将每一个轨迹与最近的点进行匹配，并将匹配结果存储在assignment向量中。