71 {
72 if (new_objects.empty()) {
73 return false;
74 }
75 if (fused_objects->size() < 1) {
76 fused_objects->assign(new_objects.begin(), new_objects.end());
77 for (unsigned int j = 0; j < new_objects.size(); ++j) {
78 std::vector<base::Object> matched_objects;
79 matched_objects.push_back(new_objects[j]);
80 fusion_result->push_back(matched_objects);
81 }
82 return true;
83 }
84 int u_num = fused_objects->size();
85 int v_num = new_objects.size();
86 Eigen::MatrixXf association_mat(u_num, v_num);
87 ComputeAssociateMatrix(*fused_objects, new_objects, &association_mat);
88 std::vector<std::pair<int, int>> match_cps;
89 if (u_num > v_num) {
90 km_matcher_.
GetKMResult(association_mat.transpose(), &match_cps,
true);
91 } else {
92 km_matcher_.
GetKMResult(association_mat, &match_cps,
false);
93 }
94 for (auto it = match_cps.begin(); it != match_cps.end(); it++) {
95 if (it->second != -1) {
96 if (it->first == -1) {
97 fused_objects->push_back(new_objects[it->second]);
98 std::vector<base::Object> matched_objects;
99 matched_objects.push_back(fused_objects->back());
100 fusion_result->push_back(matched_objects);
101 } else {
102 (*fusion_result)[it->first].push_back(new_objects[it->second]);
103 }
104 }
105 }
106 return true;
107}
bool GetKMResult(const T &association_mat, std::vector< std::pair< int, int > > *match_cps, bool need_reverse=false)