0: 设备选型
Camera选型： 如何选择视场角和焦距：
Radar选型
lidar选型
1: Radar毫米波雷达和Camera视觉融合
算法要求：
图像目标检测
Radar目标检测
radar和camera标定
融合效果
例如：
可用作交通事件分析
2: Lidar激光雷达和Camera视觉融合
算法要求：
图像目标检测
Lidar目标检测
Lidar和camera标定
激光测距到相机像素深度值：
标定投影原理：
https://blog.csdn.net/tMb8Z9Vdm66wH68VX1/article/details/120499743
zc 是三维点在相机坐标系中深度值。
效果展示：
左边图像目标检测和融合效果
右边Lidar目标检测
基于图像深度学习目标检测
基于激光雷达的深度学习目标检测
融合目标，重新画框显示可视化，赋予车辆速度信息
3: Lidar激光雷达和Radar毫米波雷达融合
算法要求：
图像目标检测
Radar目标检测
Radar和camera标定
融合的前提之跟踪算法
4: camera跟踪算法
匈牙利算法（又叫KM算法）就是用来解决分配问题的一种方法，它基于定理：
如果代价矩阵的某一行或某一列同时加上或减去某个数，则这个新的代价矩阵的最优分配仍然是原代价矩阵的最优分配。
算法步骤（假设矩阵为NxN方阵）：
1：对于矩阵的每一行，减去其中最小的元素
2：对于矩阵的每一列，减去其中最小的元素
3：用最少的水平线或垂直线覆盖矩阵中所有的0
4：如果线的数量等于N，则找到了最优分配，算法结束，否则进入步骤
5：找到没有被任何线覆盖的最小元素，每个没被线覆盖的行减去这个元素，每个被线覆盖的列加上这个元素，返回步骤3
图像的跟踪算法的理解
在跟踪之前，对所有目标已经完成检测，实现了特征建模过程。
- 第一帧进来时，以检测到的目标初始化并创建新的跟踪器，标注id。
- 后面帧进来时，先到卡尔曼滤波器中得到由前面帧box产生的状态预测和协方差预测。求跟踪器所有目标状态预测与本帧检测的box的IOU，通过匈牙利指派算法得到IOU最大的唯一匹配（数据关联部分），再去掉匹配值小于iou_threshold的匹配对。
https://blog.csdn.net/u011837761/article/details/52058703 - 用本帧中匹配到的目标检测box去更新卡尔曼跟踪器，计算卡尔曼增益、状态更新和协方差更新，并将状态更新值输出，作为本帧的跟踪box。对于本帧中没有匹配到的目标重新初始化跟踪器。
其中，卡尔曼跟踪器联合了历史跟踪记录，调节历史box与本帧box的残差，更好的匹配跟踪id。
message WeightParam {
optional float appearance = 1 [default = 0];
optional float motion = 2 [default = 0];
optional float shape = 3 [default = 0];
optional float tracklet = 4 [default = 0];
optional float overlap = 5 [default = 0];
}
message OmtParam {
optional int32 img_capability = 1 [default = 7];
optional int32 lost_age = 2 [default = 2];
optional int32 reserve_age = 3 [default = 3];
optional WeightParam weight_same_camera = 4;
optional WeightParam weight_diff_camera = 5;
optional float border = 9 [default = 30];
optional float target_thresh = 10 [default = 0.65];
optional bool correct_type = 11 [default = false];
optional TargetParam target_param = 12;
optional float min_init_height_ratio = 13 [default = 17];
optional float target_combine_iou_threshold = 14 [default = 0.5];
optional float fusion_target_thresh = 15 [default = 0.45];
optional float image_displacement = 16 [default = 50];
optional float abnormal_movement = 17 [default = 0.3];
optional double same_ts_eps = 18 [default = 0.05];
optional ReferenceParam reference = 19;
optional string type_change_cost = 20;
}
如果采用算法，可以综合以下，计算关联矩阵
float sa = ScoreAppearance(targets_[i], objects[j]);
float sm = ScoreMotion(targets_[i], objects[j]);
float ss = ScoreShape(targets_[i], objects[j]);
float so = ScoreOverlap(targets_[i], objects[j]);
if (sa == 0) {
hypo.score =
omt_param_.weight_diff_camera().motion() * sm
+ omt_param_.weight_diff_camera().shape() * ss
+ omt_param_.weight_diff_camera().overlap() * so;
} else {
hypo.score = (omt_param_.weight_same_camera().appearance() * sa +
omt_param_.weight_same_camera().motion() * sm +
omt_param_.weight_same_camera().shape() * ss +
omt_param_.weight_same_camera().overlap() * so);
}
5: Lidar跟踪算法
https://blog.csdn.net/lemonxiaoxiao/article/details/108645427
perception/lidar/lib/tracker/multi_lidar_fusion/mlf_track_object_matcher.cc
void MlfTrackObjectMatcher::Match(
const MlfTrackObjectMatcherOptions &options,
const std::vector<TrackedObjectPtr> &objects,
const std::vector<MlfTrackDataPtr> &tracks,
std::vector<std::pair<size_t, size_t>> *assignments,
std::vector<size_t> *unassigned_tracks,
std::vector<size_t> *unassigned_objects) {
assignments->clear();
unassigned_objects->clear();
unassigned_tracks->clear();
if (objects.empty() || tracks.empty()) {
unassigned_objects->resize(objects.size());
unassigned_tracks->resize(tracks.size());
std::iota(unassigned_objects->begin(), unassigned_objects->end(), 0);
std::iota(unassigned_tracks->begin(), unassigned_tracks->end(), 0);
return;
}
BipartiteGraphMatcherOptions matcher_options;
matcher_options.cost_thresh = max_match_distance_;
matcher_options.bound_value = bound_value_;
BaseBipartiteGraphMatcher *matcher =
objects[0]->is_background ? background_matcher_ : foreground_matcher_;
common::SecureMat<float> *association_mat = matcher->cost_matrix();
association_mat->Resize(tracks.size(), objects.size());
ComputeAssociateMatrix(tracks, objects, association_mat);
matcher->Match(matcher_options, assignments, unassigned_tracks,
unassigned_objects);
for (size_t i = 0; i < assignments->size(); ++i) {
objects[assignments->at(i).second]->association_score =
(*association_mat)(assignments->at(i).first,
assignments->at(i).second) /
max_match_distance_;
}
}
float MlfTrackObjectDistance::ComputeDistance(
const TrackedObjectConstPtr& object,
const MlfTrackDataConstPtr& track) const {
bool is_background = object->is_background;
const TrackedObjectConstPtr latest_object = track->GetLatestObject().second;
std::string key = latest_object->sensor_info.name + object->sensor_info.name;
const std::vector<float>* weights = nullptr;
if (is_background) {
auto iter = background_weight_table_.find(key);
if (iter == background_weight_table_.end()) {
weights = &kBackgroundDefaultWeight;
} else {
weights = &iter->second;
}
} else {
auto iter = foreground_weight_table_.find(key);
if (iter == foreground_weight_table_.end()) {
weights = &kForegroundDefaultWeight;
} else {
weights = &iter->second;
}
}
if (weights == nullptr || weights->size() < 7) {
AERROR << "Invalid weights";
return 1e+10f;
}
float distance = 0.f;
float delta = 1e-10f;
double current_time = object->object_ptr->latest_tracked_time;
track->PredictState(current_time);
double time_diff =
track->age_ ? current_time - track->latest_visible_time_ : 0;
if (weights->at(0) > delta) {
distance +=
weights->at(0) * LocationDistance(latest_object, track->predict_.state,
object, time_diff);
}
if (weights->at(1) > delta) {
distance +=
weights->at(1) * DirectionDistance(latest_object, track->predict_.state,
object, time_diff);
}
if (weights->at(2) > delta) {
distance +=
weights->at(2) * BboxSizeDistance(latest_object, track->predict_.state,
object, time_diff);
}
if (weights->at(3) > delta) {
distance +=
weights->at(3) * PointNumDistance(latest_object, track->predict_.state,
object, time_diff);
}
if (weights->at(4) > delta) {
distance +=
weights->at(4) * HistogramDistance(latest_object, track->predict_.state,
object, time_diff);
}
if (weights->at(5) > delta) {
distance += weights->at(5) * CentroidShiftDistance(latest_object,
track->predict_.state,
object, time_diff);
}
if (weights->at(6) > delta) {
distance += weights->at(6) *
BboxIouDistance(latest_object, track->predict_.state, object,
time_diff, background_object_match_threshold_);
}
// for foreground, calculate semantic map based distance
// if (!is_background) {
// distance += weights->at(7) * SemanticMapDistance(*track, object);
// }
return distance;
}
/******************************************************************************
* Copyright 2018 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include <algorithm>
#include <vector>
#include "cyber/common/log.h"
#include "modules/common/math/line_segment2d.h"
#include "modules/common/math/vec2d.h"
#include "modules/perception/common/geometry/basic.h"
#include "modules/perception/common/geometry/common.h"
#include "modules/perception/lidar/lib/tracker/association/distance_collection.h"
float LocationDistance(const TrackedObjectConstPtr& last_object,
const Eigen::VectorXf& track_predict,
const TrackedObjectConstPtr& new_object,
const double time_diff) {
// Compute locatin distance for last object and new object
// range from 0 to positive infinity
Eigen::Vector3f measured_anchor_point =
(new_object->anchor_point).cast<float>();
Eigen::Vector3f predicted_anchor_point = track_predict.head(3);
Eigen::Vector3f measure_predict_diff =
measured_anchor_point - predicted_anchor_point;
float location_dist = static_cast<float>(sqrt(
(measure_predict_diff.head(2).cwiseProduct(measure_predict_diff.head(2)))
.sum()));
/* NEED TO NOTICE: All the states would be collected mainly based on
* states of tracked objects. Thus, update tracked object when you
* update the state of track !!!!! */
Eigen::Vector2d ref_dir = last_object->output_velocity.head(2);
double speed = ref_dir.norm();
ref_dir /= speed;
/* Let location distance generated from a normal distribution with
* symmetric variance. Modify its variance when speed greater than
* a threshold. Penalize variance other than motion direction. */
if (speed > 2) {
Eigen::Vector2d ref_o_dir = Eigen::Vector2d(ref_dir[1], -ref_dir[0]);
double dx = ref_dir(0) * measure_predict_diff(0) +
ref_dir(1) * measure_predict_diff(1);
double dy = ref_o_dir(0) * measure_predict_diff(0) +
ref_o_dir(1) * measure_predict_diff(1);
location_dist = static_cast<float>(sqrt(dx * dx * 0.5 + dy * dy * 2));
}
return location_dist;
}
float DirectionDistance(const TrackedObjectConstPtr& last_object,
const Eigen::VectorXf& track_predict,
const TrackedObjectConstPtr& new_object,
const double time_diff) {
// Compute direction distance for last object and new object
// range from 0 to 2
Eigen::Vector3f old_anchor_point =
(last_object->belief_anchor_point).cast<float>();
Eigen::Vector3f new_anchor_point = (new_object->anchor_point).cast<float>();
Eigen::Vector3f anchor_point_shift_dir = new_anchor_point - old_anchor_point;
anchor_point_shift_dir[2] = 0.0;
Eigen::Vector3f track_motion_dir = track_predict.head(6).tail(3);
track_motion_dir[2] = 0.0;
double cos_theta = 0.994; // average cos
if (!track_motion_dir.head(2).isZero() &&
!anchor_point_shift_dir.head(2).isZero()) {
// cos_theta = vector_cos_theta_2d_xy(track_motion_dir,
// anchor_point_shift_dir);
cos_theta = common::CalculateCosTheta2DXY<float>(track_motion_dir,
anchor_point_shift_dir);
}
float direction_dist = static_cast<float>(-cos_theta) + 1.0f;
return direction_dist;
}
float BboxSizeDistance(const TrackedObjectConstPtr& last_object,
const Eigen::VectorXf& track_predict,
const TrackedObjectConstPtr& new_object,
const double time_diff) {
// Compute bbox size distance for last object and new object
// range from 0 to 1
Eigen::Vector3f old_bbox_dir = last_object->output_direction.cast<float>();
Eigen::Vector3f new_bbox_dir = new_object->direction.cast<float>();
Eigen::Vector3f old_bbox_size = last_object->output_size.cast<float>();
Eigen::Vector3f new_bbox_size = new_object->size.cast<float>();
float size_dist = 0.0f;
double dot_val_00 = fabs(old_bbox_dir(0) * new_bbox_dir(0) +
old_bbox_dir(1) * new_bbox_dir(1));
double dot_val_01 = fabs(old_bbox_dir(0) * new_bbox_dir(1) -
old_bbox_dir(1) * new_bbox_dir(0));
float temp_val_0 = 0.0f;
float temp_val_1 = 0.0f;
if (dot_val_00 > dot_val_01) {
temp_val_0 = static_cast<float>(fabs(old_bbox_size(0) - new_bbox_size(0))) /
std::max(old_bbox_size(0), new_bbox_size(0));
temp_val_1 = static_cast<float>(fabs(old_bbox_size(1) - new_bbox_size(1))) /
std::max(old_bbox_size(1), new_bbox_size(1));
size_dist = std::min(temp_val_0, temp_val_1);
} else {
temp_val_0 = static_cast<float>(fabs(old_bbox_size(0) - new_bbox_size(1))) /
std::max(old_bbox_size(0), new_bbox_size(1));
temp_val_1 = static_cast<float>(fabs(old_bbox_size(1) - new_bbox_size(0))) /
std::max(old_bbox_size(1), new_bbox_size(0));
size_dist = std::min(temp_val_0, temp_val_1);
}
return size_dist;
}
float PointNumDistance(const TrackedObjectConstPtr& last_object,
const Eigen::VectorXf& track_predict,
const TrackedObjectConstPtr& new_object,
const double time_diff) {
// Compute point num distance for last object and new object
// range from 0 and 1
int old_point_number = static_cast<int>(
(last_object->object_ptr->lidar_supplement).cloud_world.size());
int new_point_number = static_cast<int>(
(new_object->object_ptr->lidar_supplement).cloud_world.size());
float point_num_dist =
static_cast<float>(fabs(old_point_number - new_point_number)) * 1.0f /
static_cast<float>(std::max(old_point_number, new_point_number));
return point_num_dist;
}
float HistogramDistance(const TrackedObjectConstPtr& last_object,
const Eigen::VectorXf& track_predict,
const TrackedObjectConstPtr& new_object,
const double time_diff) {
// Compute histogram distance for last object and new object
// range from 0 to 3
const std::vector<float>& old_object_shape_features =
last_object->shape_features;
const std::vector<float>& new_object_shape_features =
new_object->shape_features;
if (old_object_shape_features.size() != new_object_shape_features.size()) {
AINFO << "sizes of compared features not matched. TrackObjectDistance";
return 100;
}
float histogram_dist = 0.0f;
for (size_t i = 0; i < old_object_shape_features.size(); ++i) {
histogram_dist +=
std::fabs(old_object_shape_features[i] - new_object_shape_features[i]);
}
return histogram_dist;
}
float CentroidShiftDistance(const TrackedObjectConstPtr& last_object,
const Eigen::VectorXf& track_predict,
const TrackedObjectConstPtr& new_object,
const double time_diff) {
float dist = static_cast<float>(
(last_object->barycenter.head(2) - new_object->barycenter.head(2))
.norm());
return dist;
}
float BboxIouDistance(const TrackedObjectConstPtr& last_object,
const Eigen::VectorXf& track_predict,
const TrackedObjectConstPtr& new_object,
const double time_diff, double match_threshold) {
// Step 1: unify bbox direction, change the one with less pts,
// for efficiency.
Eigen::Vector3f old_dir = last_object->output_direction.cast<float>();
Eigen::Vector3f old_size = last_object->output_size.cast<float>();
Eigen::Vector3d old_center = last_object->output_center;
Eigen::Vector3f new_dir = new_object->direction.cast<float>();
Eigen::Vector3f new_size = new_object->size.cast<float>();
Eigen::Vector3d new_center = new_object->center;
old_dir.normalize();
new_dir.normalize();
// handle randomness
old_size(0) = old_size(0) > 0.3f ? old_size(0) : 0.3f;
old_size(1) = old_size(1) > 0.3f ? old_size(1) : 0.3f;
new_size(0) = new_size(0) > 0.3f ? new_size(0) : 0.3f;
new_size(1) = new_size(1) > 0.3f ? new_size(1) : 0.3f;
int last_object_num_pts = static_cast<int>(
(last_object->object_ptr->lidar_supplement).cloud_world.size());
int cur_obj_num_pts = static_cast<int>(
(new_object->object_ptr->lidar_supplement).cloud_world.size());
bool change_cur_obj_bbox = last_object_num_pts > cur_obj_num_pts;
float minimum_edge_length = 0.01f;
base::PointDCloud& cloud =
(new_object->object_ptr->lidar_supplement).cloud_world;
if (change_cur_obj_bbox) {
new_dir = old_dir;
common::CalculateBBoxSizeCenter2DXY(cloud, new_dir, &new_size, &new_center,
minimum_edge_length);
} else {
old_dir = new_dir;
common::CalculateBBoxSizeCenter2DXY(cloud, old_dir, &old_size, &old_center,
minimum_edge_length);
}
// Step 2: compute 2d iou bbox to bbox
Eigen::Matrix2d trans_mat;
trans_mat(0, 0) = (old_dir.cast<double>())(0);
trans_mat(0, 1) = (old_dir.cast<double>())(1);
trans_mat(1, 0) = -(old_dir.cast<double>())(1);
trans_mat(1, 1) = (old_dir.cast<double>())(0);
Eigen::Vector2d old_center_transformed_2d =
static_cast<Eigen::Matrix<double, 2, 1, 0, 2, 1>>(trans_mat *
old_center.head(2));
Eigen::Vector2d new_center_transformed_2d =
static_cast<Eigen::Matrix<double, 2, 1, 0, 2, 1>>(trans_mat *
new_center.head(2));
old_center(0) = old_center_transformed_2d(0);
old_center(1) = old_center_transformed_2d(1);
new_center(0) = new_center_transformed_2d(0);
new_center(1) = new_center_transformed_2d(1);
Eigen::Vector3d old_size_tmp = old_size.cast<double>();
Eigen::Vector3d new_size_tmp = new_size.cast<double>();
double iou = common::CalculateIou2DXY<double>(old_center, old_size_tmp,
new_center, new_size_tmp);
// Step 4: compute dist
double dist = (1 - iou) * match_threshold;
return static_cast<float>(dist);
}
‘’‘
’‘’
6:Radar跟踪算法
bool HMMatcher::Match(const std::vector<RadarTrackPtr> &radar_tracks,
const base::Frame &radar_frame,
const TrackObjectMatcherOptions &options,
std::vector<TrackObjectPair> *assignments,
std::vector<size_t> *unassigned_tracks,
std::vector<size_t> *unassigned_objects) {
IDMatch(radar_tracks, radar_frame, assignments, unassigned_tracks,
unassigned_objects);
TrackObjectPropertyMatch(radar_tracks, radar_frame, assignments,
unassigned_tracks, unassigned_objects);
return true;
}
bool HMMatcher::RefinedTrack(const base::ObjectPtr &track_object,
double track_timestamp,
const base::ObjectPtr &radar_object,
double radar_timestamp) {
double dist = 0.5 * DistanceBetweenObs(track_object, track_timestamp,
radar_object, radar_timestamp) +
0.5 * DistanceBetweenObs(radar_object, radar_timestamp,
track_object, track_timestamp);
return dist < BaseMatcher::GetMaxMatchDistance();
}
void HMMatcher::TrackObjectPropertyMatch(
const std::vector<RadarTrackPtr> &radar_tracks,
const base::Frame &radar_frame, std::vector<TrackObjectPair> *assignments,
std::vector<size_t> *unassigned_tracks,
std::vector<size_t> *unassigned_objects) {
if (unassigned_tracks->empty() || unassigned_objects->empty()) {
return;
}
std::vector<std::vector<double>> association_mat(unassigned_tracks->size());
for (size_t i = 0; i < association_mat.size(); ++i) {
association_mat[i].resize(unassigned_objects->size(), 0);
}
ComputeAssociationMat(radar_tracks, radar_frame, *unassigned_tracks,
*unassigned_objects, &association_mat);
// from perception-common
common::SecureMat<double> *global_costs =
hungarian_matcher_.mutable_global_costs();
global_costs->Resize(unassigned_tracks->size(), unassigned_objects->size());
for (size_t i = 0; i < unassigned_tracks->size(); ++i) {
for (size_t j = 0; j < unassigned_objects->size(); ++j) {
(*global_costs)(i, j) = association_mat[i][j];
}
}
std::vector<TrackObjectPair> property_assignments;
std::vector<size_t> property_unassigned_tracks;
std::vector<size_t> property_unassigned_objects;
hungarian_matcher_.Match(
BaseMatcher::GetMaxMatchDistance(), BaseMatcher::GetBoundMatchDistance(),
common::GatedHungarianMatcher<double>::OptimizeFlag::OPTMIN,
&property_assignments, &property_unassigned_tracks,
&property_unassigned_objects);
for (size_t i = 0; i < property_assignments.size(); ++i) {
size_t gt_idx = unassigned_tracks->at(property_assignments[i].first);
size_t go_idx = unassigned_objects->at(property_assignments[i].second);
assignments->push_back(std::pair<size_t, size_t>(gt_idx, go_idx));
}
std::vector<size_t> temp_unassigned_tracks;
std::vector<size_t> temp_unassigned_objects;
for (size_t i = 0; i < property_unassigned_tracks.size(); ++i) {
size_t gt_idx = unassigned_tracks->at(property_unassigned_tracks[i]);
temp_unassigned_tracks.push_back(gt_idx);
}
for (size_t i = 0; i < property_unassigned_objects.size(); ++i) {
size_t go_idx = unassigned_objects->at(property_unassigned_objects[i]);
temp_unassigned_objects.push_back(go_idx);
}
*unassigned_tracks = temp_unassigned_tracks;
*unassigned_objects = temp_unassigned_objects;
}
void HMMatcher::ComputeAssociationMat(
const std::vector<RadarTrackPtr> &radar_tracks,
const base::Frame &radar_frame,
const std::vector<size_t> &unassigned_tracks,
const std::vector<size_t> &unassigned_objects,
std::vector<std::vector<double>> *association_mat) {
double frame_timestamp = radar_frame.timestamp;
for (size_t i = 0; i < unassigned_tracks.size(); ++i) {
for (size_t j = 0; j < unassigned_objects.size(); ++j) {
const base::ObjectPtr &track_object =
radar_tracks[unassigned_tracks[i]]->GetObs();
const base::ObjectPtr &frame_object =
radar_frame.objects[unassigned_objects[j]];
double track_timestamp =
radar_tracks[unassigned_tracks[i]]->GetTimestamp();
double distance_forward = DistanceBetweenObs(
track_object, track_timestamp, frame_object, frame_timestamp);
double distance_backward = DistanceBetweenObs(
frame_object, frame_timestamp, track_object, track_timestamp);
association_mat->at(i).at(j) =
0.5 * distance_forward + 0.5 * distance_backward;
}
}
}
double HMMatcher::DistanceBetweenObs(const base::ObjectPtr &obs1,
double timestamp1,
const base::ObjectPtr &obs2,
double timestamp2) {
double time_diff = timestamp2 - timestamp1;
return (obs2->center - obs1->center -
obs1->velocity.cast<double>() * time_diff)
.head(2)
.norm();
}
‘’‘
’‘’
‘’‘
7:Camera和Radar融合算法
’‘’
‘’‘
’‘’
‘’‘
8:Lidar和Camera融合入算法
#include <queue>
#include <set>
#include <vector>
#include <iostream>
using namespace std;
static const int MAX = 4;
static const double maxC = 10001.f;
vector< std::vector<double> > array_to_matrix(double* m, int rows, int cols);
void hungarian_print_Costmatrix(const vector<vector<double> >& Cost);
void hungarian_print_assignment(vector<int> &assignment);
double c[MAX][MAX];
double Fx[MAX];//Cost矩阵中每一行的最小值
double Fy[MAX];//Cost矩阵中每一列的最小值
int matchX[MAX];//初始化为-1|matchX[i]表示X中的第i个元素和Y[matchX[i]]产生配对关系
int matchY[MAX];//初始化为-1|matchY[i]表示Y中的第i个元素和X[matchY[i]]产生配对关系
int Trace[MAX];//初始化为-1
int m;//tracks.size();
int n;//detections.size();
int k;//k=max(m,n)
int start;
int finish;
double GetC(int i, int j);
void FindAugmentingPath();
void SubX_AddY();
void Enlarge();
void hungarian(vector<vector<double>>& DistMatrix, vector<int>& Assignment);
int main()
{
//double r[3 * 3] =
//{
// 100, 300, 300,
// 300, 100, 300,
// 300, 300, 100
//};
double r[4 * 4] =
{
100, 100, 300, 300,
300, 100, 100, 300,
100, 100, 300, 300,
300, 300, 100, 300
};
vector< vector<double> > Cost = array_to_matrix(r, 4, 4);
hungarian_print_Costmatrix(Cost);
vector<int> assignment;
//Hungarian APS;
//APS.Solve(Cost, assignment);
hungarian(Cost, assignment);
hungarian_print_assignment(assignment);
return 0;
}
void hungarian(vector<vector<double>>& DistMatrix, vector<int>& Assignment)
{
int i, j;
m = DistMatrix.size();
n = DistMatrix[0].size();
k = m > n ? m : n;//k=max(m,n)
//构建c矩阵，
for (i = 0; i < k; i++)
{
for (j = 0; j < k; j++)
{
if (i >= m || j >= n)
{
c[i][j] = maxC;//maxC = 10001.f|超过Cost矩阵范围的元素设为特别大
continue;
}
if (DistMatrix[i][j] == 0)
c[i][j] = maxC;//预测点与最新点的欧式距离为0的开销设为特别大
else
c[i][j] = DistMatrix[i][j];
}
}
for (i = 0; i < MAX; i++)
{
matchX[i] = -1;
matchY[i] = -1;
}
//寻找Cost矩阵每一行的最小值
for (i = 0; i < k; i++)
{
Fx[i] = maxC;
for (j = 0; j < k; j++)
{
if (c[i][j] < Fx[i])
{
Fx[i] = c[i][j];
}
}
}
//Cost矩阵的i行的每一个元素减去Fx[i]，然后寻找每一列的最小值
for (j = 0; j < k; j++)
{
Fy[j] = maxC;
for (i = 0; i < k; i++)
{
if (c[i][j] - Fx[i] < Fy[j])
{
Fy[j] = c[i][j] - Fx[i];
}
}
}
for (int x = 0; x < k; x++)
{
start = x;
finish = -1;
do
{
FindAugmentingPath();//利用0权重边寻找最大匹配
if (finish == -1)
SubX_AddY();//寻找最小点覆蓋集，调整边的权重
} while (finish == -1);
Enlarge();//在matchX、matchY中更新X和Y的配对关系
}
for (int x = 0; x < m; x++)
{
Assignment.push_back(matchX[x]);
}
for (int x = 0; x < m; x++)
{
for (int y = 0; y < n; y++)
{
DistMatrix[x][y] = c[x][y];
if (c[x][y] == maxC)
DistMatrix[x][y] = 0.f;
}
}
//计算损失函数值
//float cost = 0.f;
//for (int x = 0; x < m; x++)
//{
// int y = matchX[x];
// if (c[x][y] < maxC)
// {
// cost += c[x][y];
// }
//}
}
double GetC(int i, int j)
{
return c[i][j] - Fx[i] - Fy[j];
}
void FindAugmentingPath()
{
queue<int> q;
int i, j;
for (i = 0; i < MAX; i++)
{
Trace[i] = -1;
}
///memset(Trace, -1, sizeof(Trace));
//BFS宽度优先搜索|Running the algorithm BFS to find the opening of the road
q.push(start);
do
{
i = q.front();
q.pop();
for (j = 0; j < k; j++)
{
if (Trace[j] == -1 && GetC(i, j) == 0.0f)
{
Trace[j] = i;
if (matchY[j] == -1)
{
finish = j;
return;
}
q.push(matchY[j]);
}
}
} while (!q.empty());
}
void SubX_AddY()
{
int i, j, t;
double Delta;
set<int> VisitedX, VisitedY;
VisitedX.insert(start);
for (j = 0; j < k; j++)
{
if (Trace[j] != -1)
{
VisitedX.insert(matchY[j]);
VisitedY.insert(j);
}
}
Delta = maxC;
for (i = 0; i < k; i++)
{
if (VisitedX.find(i) != VisitedX.end())
{
for (j = 0; j < k; j++)
{
if ((VisitedY.find(j) == VisitedY.end()) && (GetC(i, j) < Delta))
Delta = GetC(i, j);
}
}
}
for (t = 0; t < k; t++)
{
if (VisitedX.find(t) != VisitedX.end())
Fx[t] = Fx[t] + Delta;
if (VisitedY.find(t) != VisitedY.end())
Fy[t] = Fy[t] - Delta;
}
}
void Enlarge()
{
int x, next;
do
{
x = Trace[finish];
next = matchX[x];
matchX[x] = finish;
matchY[finish] = x;
finish = next;
} while (finish != -1);
}
vector< std::vector<double> > array_to_matrix(double* m, int rows, int cols)
{
int i, j;
std::vector< std::vector<double> > r;
r.resize(rows, std::vector<double>(cols, 0));
for (i = 0; i < rows; i++)
{
for (j = 0; j < cols; j++)
r[i][j] = m[i*cols + j];
}
return r;
}
void hungarian_print_Costmatrix(const vector<vector<double> >& Cost)
{
int i, j;
cout << endl;
for (i = 0; i < Cost.size(); i++)
{
cout << "[";
for (j = 0; j < Cost[0].size(); j++)
{
fprintf(stderr, "%-5.0f ", Cost[i][j]);
}
cout << "]" << endl;
}
cout << endl;
}
void hungarian_print_assignment(vector<int> &assignment)
{
for (int i = 0; i < assignment.size(); i++)
{
cout << assignment[i] << " ";
}//0 1 2
cout << endl;
}
9: 雷视融合应用定速巡航+车道保持
2566
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
