From 5a7d03e9d412ed56384504a5d99ea8ac0c6a12d0 Mon Sep 17 00:00:00 2001 From: kimifly06 <554255702@qq.com> Date: Tue, 5 Sep 2017 23:34:35 +0800 Subject: [PATCH] Clean hm tracker dev (#1496) * clean hm tracker IV * fix code style * clean hm tracker V * fixed a bug --- modules/perception/model/tracker.config | 52 ++- .../lidar/tracker/hm_tracker/base_filter.h | 11 +- .../lidar/tracker/hm_tracker/base_matcher.h | 14 +- .../tracker/hm_tracker/feature_descriptor.h | 3 +- .../lidar/tracker/hm_tracker/hm_tracker.cc | 335 +++++++++++------- .../lidar/tracker/hm_tracker/hm_tracker.h | 95 +++-- .../tracker/hm_tracker/hungarian_matcher.cc | 49 +-- .../tracker/hm_tracker/hungarian_matcher.h | 36 +- .../lidar/tracker/hm_tracker/kalman_filter.cc | 120 ++++--- .../lidar/tracker/hm_tracker/kalman_filter.h | 89 ++--- .../lidar/tracker/hm_tracker/object_track.cc | 211 +++++------ .../lidar/tracker/hm_tracker/object_track.h | 71 ++-- .../hm_tracker/track_object_distance.cc | 110 +++--- .../hm_tracker/track_object_distance.h | 133 +++---- .../tracker/hm_tracker/tracked_object.cc | 12 - .../lidar/tracker/hm_tracker/tracked_object.h | 11 +- 16 files changed, 736 insertions(+), 616 deletions(-) diff --git a/modules/perception/model/tracker.config b/modules/perception/model/tracker.config index 33140ce5f4..43c40d8225 100644 --- a/modules/perception/model/tracker.config +++ b/modules/perception/model/tracker.config @@ -12,53 +12,57 @@ model_configs { value: "kalman_filter" } integer_params { - name: "collect_consecutive_invisible_maximum" - value: 0 - } - integer_params { - name: "collect_age_minimum" - value: 0 + name: "track_cached_history_size_maximum" + value: 20 } integer_params { name: "track_consecutive_invisible_maximum" value: 1 } + float_params { + name: "track_visible_ratio_minimum" + value: 0.6 + } integer_params { - name: "track_cached_history_size_maximum" - value: 20 + name: "collect_age_minimum" + value: 0 } - float_params { - name: "speed_noise_maximum" - value: 0.4 + integer_params { + name: "collect_consecutive_invisible_maximum" + value: 0 } float_params { name: "acceleration_noise_maximum" value: 5 } + float_params { + name: "speed_noise_maximum" + value: 0.4 + } # matcher parameters float_params { - name: "max_match_distance" + name: "match_distance_maximum" value: 4.0 } float_params { - name: "weight_location_dist" + name: "location_distance_weight" value: 0.6 } float_params { - name: "weight_direction_dist" + name: "direction_distance_weight" value: 0.2 } float_params { - name: "weight_bbox_size_dist" + name: "bbox_size_distance_weight" value: 0.1 } float_params { - name: "weight_point_num_dist" + name: "point_num_distance_weight" value: 0.1 } float_params { - name: "weight_histogram_dist" + name: "histogram_distance_weight" value: 0.5 } integer_params { @@ -72,25 +76,19 @@ model_configs { value: true } float_params { - name: "centroid_measurement_noise" + name: "measurement_noise" value: 0.4 } float_params { - name: "centroid_initial_velocity_variance" + name: "initial_velocity_noise" value: 5 } float_params { - name: "propagation_variance_xy" + name: "xy_propagation_noise" value: 10 } float_params { - name: "propagation_variance_z" + name: "z_propagation_noise" value: 10 } - - # post-processing parameters - float_params { - name: "speed_threshold" - value: 1.0 # m/s - } } diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/base_filter.h b/modules/perception/obstacle/lidar/tracker/hm_tracker/base_filter.h index 407330a9eb..8c4f9a50a0 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/base_filter.h +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/base_filter.h @@ -18,6 +18,7 @@ #define MODULES_PERCEPTION_OBSTACLE_LIDAR_TRACKER_HM_TRACKER_BASE_FILTER_H_ #include + #include "Eigen/Core" #include "modules/perception/obstacle/base/object.h" #include "modules/perception/obstacle/lidar/tracker/hm_tracker/tracked_object.h" @@ -50,23 +51,23 @@ class BaseFilter { // @params[IN] time_diff: time interval for predicting // @return predicted states of filtering virtual Eigen::VectorXf Predict( - const double time_diff) = 0; + const double& time_diff) = 0; // @brief update filter with object - // @params[IN] new_object: new object for current updating - // @params[IN] old_object: old object for last updating + // @params[IN] new_object: recently detected object for current updating + // @params[IN] old_object: last detected object for last updating // @params[IN] time_diff: time interval from last updating // @return nothing virtual void UpdateWithObject( const TrackedObjectPtr& new_object, const TrackedObjectPtr& old_object, - const double time_diff) = 0; + const double& time_diff) = 0; // @brief update filter without object // @params[IN] time_diff: time interval from last updating // @return nothing virtual void UpdateWithoutObject( - const double time_diff) = 0; + const double& time_diff) = 0; // @brief get state of filter // @params[OUT] anchor_point: anchor point of current state diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/base_matcher.h b/modules/perception/obstacle/lidar/tracker/hm_tracker/base_matcher.h index de8b3c0346..1a556a8061 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/base_matcher.h +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/base_matcher.h @@ -40,12 +40,12 @@ class BaseMatcher { BaseMatcher() {} virtual ~BaseMatcher() {} - // @brief match new detected objects to tracks build previously - // @params[IN] objects: new detected objects - // @params[IN] tracks: tracks build previously - // @params[IN] tracks_predict: predicted states of tracks build previously - // @params[IN] time_diff: time interval from last matching - // @params[OUT] assignments: matched pair of objects & tracks + // @brief match detected objects to maintained tracks + // @params[IN] objects: detected objects + // @params[IN] tracks: maintained tracks + // @params[IN] tracks_predict: predicted states of maintained tracks + // @params[IN] time_diff: time interval from last match + // @params[OUT] assignments: matched pair of // @params[OUT] unassigned_tracks: unmatched tracks // @params[OUT] unassigned_objects: unmatched objects // @return nothing @@ -53,7 +53,7 @@ class BaseMatcher { std::vector* objects, const std::vector& tracks, const std::vector& tracks_predict, - const double time_diff, + const double& time_diff, std::vector* assignments, std::vector* unassigned_tracks, std::vector* unassigned_objects) = 0; diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/feature_descriptor.h b/modules/perception/obstacle/lidar/tracker/hm_tracker/feature_descriptor.h index 4519cbb9d2..7280fbcebb 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/feature_descriptor.h +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/feature_descriptor.h @@ -19,6 +19,7 @@ #include #include + #include "modules/perception/lib/pcl_util/pcl_types.h" namespace apollo { @@ -36,7 +37,7 @@ class FeatureDescriptor { ~FeatureDescriptor() {} // @brief compute histogram feature of given cloud - // @params[IN] bin_size: bin size of histogram feature computing + // @params[IN] bin_size: bin size of histogram // @params[OUT] feature: histogram feature of given cloud // @return nothing void ComputeHistogram( diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.cc b/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.cc index 0ace560cb9..7c7be188e5 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.cc +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.cc @@ -16,6 +16,7 @@ #include #include + #include "modules/common/log.h" #include "modules/perception/lib/config_manager/config_manager.h" #include "modules/perception/obstacle/common/geometry_util.h" @@ -57,45 +58,50 @@ bool HmObjectTracker::Init() { // A. Basic tracker setup std::string matcher_method_name = "hungarian_matcher"; + std::string filter_method_name = "kalman_filter"; + int track_cached_history_size_maximum = 20; + int track_consecutive_invisible_maximum = 1; + float track_visible_ratio_minimum = 0.6; + int collect_age_minimum = 0; + int collect_consecutive_invisible_maximum = 0; + float acceleration_noise_maximum = 5; + float speed_noise_maximum = 0.4; + // load match method if (!model_config->GetValue("matcher_method_name", &matcher_method_name)) { - AERROR << "matcher_method_name not found." << name(); + AERROR << "Failed to get matcher method name! " << name(); return false; } - if (matcher_method_name == "hungarian_matcher") { - matcher_method_ = HUNGARIAN_MATCHER; - matcher_ = new HungarianMatcher(); - } else { - AERROR << "Invalid matcher_method_name " << name(); + if (!SetMatcherMethod(matcher_method_name)) { + AERROR << "Failed to set matcher method! " << name(); return false; } - - std::string filter_method_name = "kalman_filter"; + if (matcher_method_ == HUNGARIAN_MATCHER) { + matcher_ = new HungarianMatcher(); + } + // load filter method if (!model_config->GetValue("filter_method_name", &filter_method_name)) { - AERROR << "filter_method_name not found." << name(); + AERROR << "Failed to get filter method name! " << name(); return false; } - if (filter_method_name == "kalman_filter") { - filter_method_ = KALMAN_FILTER; - ObjectTrack::SetFilterMethod(filter_method_); - } else { - AERROR << "Invalid filter_method_name " << name(); + if (!ObjectTrack::SetFilterMethod(filter_method_name)) { + AERROR << "Failed to set filter method! " << name(); return false; + } else { + filter_method_ = ObjectTrack::s_filter_method_; } - - if (!model_config->GetValue("collect_consecutive_invisible_maximum", - &collect_consecutive_invisible_maximum_)) { - AERROR << "Failed to get collect consecutive invisible maximum! " - << name(); + // load track cached history size maximum + if (!model_config->GetValue("track_cached_history_size_maximum", + &track_cached_history_size_maximum)) { + AERROR << "Failed to get track cached history size maximum! " << name(); return false; } - if (!model_config->GetValue("collect_age_minimum", - &collect_age_minimum_)) { - AERROR << "Failed to get collect age minimum! " << name(); + if (!ObjectTrack::SetTrackCachedHistorySizeMaximum( + track_cached_history_size_maximum)) { + AERROR << "Failed to set track cached history size maximum! " << name(); return false; } - - int track_consecutive_invisible_maximum = 1; + // load track consevutive invisible maximum if (!model_config->GetValue("track_consecutive_invisible_maximum", &track_consecutive_invisible_maximum)) { AERROR << "Failed to get track consecutive invisible maximum! " << name(); @@ -106,18 +112,40 @@ bool HmObjectTracker::Init() { AERROR << "Failed to set track consecutive invisible maximum! " << name(); return false; } - - float speed_noise_maximum = 0.4; - if (!model_config->GetValue("speed_noise_maximum", &speed_noise_maximum)) { - AERROR << "Failed to get speed noise maximum! " << name(); + // load track visible ratio minimum + if (!model_config->GetValue("track_visible_ratio_minimum", + &track_visible_ratio_minimum)) { + AERROR << "Failed to get track visible ratio minimum! " << name(); return false; } - if (!ObjectTrack::SetSpeedNoiseMaximum(speed_noise_maximum)) { - AERROR << "Failed to set speed noise maximum! " << name(); + if (!ObjectTrackSet::SetTrackVisibleRatioMinimum( + track_visible_ratio_minimum)) { + AERROR << "Failed to set track visible ratio minimum! " << name(); return false; } - - float acceleration_noise_maximum = 5; + // load collect age minimum + if (!model_config->GetValue("collect_age_minimum", &collect_age_minimum)) { + AERROR << "Failed to get collect age minimum! " << name(); + return false; + } + if (!SetCollectAgeMinimum(collect_age_minimum)) { + AERROR << "Failed to set collect age minimum! " << name(); + return false; + } + // load collect consecutive invisible maximum + if (!model_config->GetValue("collect_consecutive_invisible_maximum", + &collect_consecutive_invisible_maximum)) { + AERROR << "Failed to get collect consecutive invisible maximum! " + << name(); + return false; + } + if (!SetCollectConsecutiveInvisibleMaximum( + collect_consecutive_invisible_maximum)) { + AERROR << "Failed to set collect consecutive invisible maximum! " + << name(); + return false; + } + // load acceleration maximum if (!model_config->GetValue("acceleration_noise_maximum", &acceleration_noise_maximum)) { AERROR << "Failed to get acceleration noise maximum! " << name(); @@ -127,130 +155,198 @@ bool HmObjectTracker::Init() { AERROR << "Failed to set acceleration noise maximum! " << name(); return false; } - - int track_cached_history_size_maximum = 20; - if (!model_config->GetValue("track_cached_history_size_maximum", - &track_cached_history_size_maximum)) { - AERROR << "Failed to get cached history size maximum! " << name(); + // load speed noise maximum + if (!model_config->GetValue("speed_noise_maximum", &speed_noise_maximum)) { + AERROR << "Failed to get speed noise maximum! " << name(); return false; } - if (!ObjectTrack::SetTrackCachedHistorySizeMaximum( - track_cached_history_size_maximum)) { - AERROR << "Failed to set cached history size maximum! " << name(); + if (!ObjectTrack::SetSpeedNoiseMaximum(speed_noise_maximum)) { + AERROR << "Failed to set speed noise maximum! " << name(); return false; } // B. Matcher setup - float max_match_dist = 4.0; - float weight_location_dist = 0.6; - float weight_direction_dist = 0.2f; - float weight_bbox_size_dist = 0.1f; - float weight_point_num_dist = 0.1f; - float weight_histogram_dist = 0.5f; - - if (!model_config->GetValue("max_match_distance", &max_match_dist)) { - AERROR << "max_match_distance not found. " << name(); + float match_distance_maximum = 4.0; + float location_distance_weight = 0.6; + float direction_distance_weight = 0.2f; + float bbox_size_distance_weight = 0.1f; + float point_num_distance_weight = 0.1f; + float histogram_distance_weight = 0.5f; + int histogram_bin_size = 10; + // load match distance maximum + if (!model_config->GetValue("match_distance_maximum", + &match_distance_maximum)) { + AERROR << "Failed to get match distance maximum! " << name(); return false; } if (matcher_method_ == HUNGARIAN_MATCHER) { - if (!HungarianMatcher::SetMaxMatchDistance(max_match_dist)) { + if (!HungarianMatcher::SetMatchDistanceMaximum(match_distance_maximum)) { + AERROR << "Failed to set match distance maximum! " << name(); return false; } } - - if (!model_config->GetValue("weight_location_dist", - &weight_location_dist)) { - AERROR << "weight_location_dist not found." << name(); + // load location distance weight + if (!model_config->GetValue("location_distance_weight", + &location_distance_weight)) { + AERROR << "Failed to get location distance weight! " << name(); return false; } - if (!TrackObjectDistance::SetWeightLocationDist(weight_location_dist)) { + if (!TrackObjectDistance::SetLocationDistanceWeight( + location_distance_weight)) { + AERROR << "Failed to set location distance weight! " << name(); return false; } - - if (!model_config->GetValue("weight_direction_dist", - &weight_direction_dist)) { - AERROR << "weight_direction_dist not found." << name(); + // load direction distance weight + if (!model_config->GetValue("direction_distance_weight", + &direction_distance_weight)) { + AERROR << "Failed to get direction distance weight! " << name(); return false; } - if (!TrackObjectDistance::SetWeightDirectionDist(weight_direction_dist)) { + if (!TrackObjectDistance::SetDirectionDistanceWeight( + direction_distance_weight)) { + AERROR << "Failed to set direction distance weight! " << name(); return false; } - - if (!model_config->GetValue("weight_bbox_size_dist", - &weight_bbox_size_dist)) { - AERROR << "weight_bbox_size_dist not found." << name(); + // load bbox size distance weight + if (!model_config->GetValue("bbox_size_distance_weight", + &bbox_size_distance_weight)) { + AERROR << "Failed to get bbox size distance weight! " << name(); return false; } - if (!TrackObjectDistance::SetWeightBboxSizeDist(weight_bbox_size_dist)) { + if (!TrackObjectDistance::SetBboxSizeDistanceWeight( + bbox_size_distance_weight)) { + AERROR << "Failed to set bbox size distance weight! " << name(); return false; } - - if (!model_config->GetValue("weight_point_num_dist", - &weight_point_num_dist)) { - AERROR << "weight_point_num_dist not found." << name(); + // load point num distance weight + if (!model_config->GetValue("point_num_distance_weight", + &point_num_distance_weight)) { + AERROR << "Failed to get point num distance weight! " << name(); return false; } - if (!TrackObjectDistance::SetWeightPointNumDist(weight_point_num_dist)) { + if (!TrackObjectDistance::SetPointNumDistanceWeight( + point_num_distance_weight)) { + AERROR << "Failed to set point num distance weight! " << name(); return false; } - - if (!model_config->GetValue("weight_histogram_dist", - &weight_histogram_dist)) { - AERROR << "weight_histogram_dist not found." << name(); + // load histogram distance weight + if (!model_config->GetValue("histogram_distance_weight", + &histogram_distance_weight)) { + AERROR << "Failed to get histogram distance weight! " << name(); + return false; + } + if (!TrackObjectDistance::SetHistogramDistanceWeight( + histogram_distance_weight)) { + AERROR << "Failed to set histogram distance weight! " << name(); return false; } - if (!TrackObjectDistance::SetWeightHistogramDist(weight_histogram_dist)) { + use_histogram_for_match_ = + histogram_distance_weight > FLT_EPSILON ? true : false; + if (!model_config->GetValue("histogram_bin_size", &histogram_bin_size)) { + AERROR << "Failed to get histogram bin size! " << name(); return false; } - use_histogram_for_match_ = weight_histogram_dist > FLT_EPSILON ? true : false; - if (!model_config->GetValue("histogram_bin_size", &histogram_bin_size_)) { - AERROR << "histogram_bin_size not found." << name(); + if (!SetHistogramBinSize(histogram_bin_size)) { + AERROR << "Failed to set histogram bin size! " << name(); return false; } // C. Filter setup bool use_adaptive = false; if (!model_config->GetValue("use_adaptive", &use_adaptive)) { - AERROR << "use_adaptive not found." << name(); + AERROR << "Failed to get use adaptive! " << name(); return false; } if (filter_method_ == KALMAN_FILTER) { + double association_score_maximum = match_distance_maximum; + float measurement_noise = 0.4f; + float initial_velocity_noise = 5.0f; + float xy_propagation_noise = 10.0f; + float z_propagation_noise = 10.0f; KalmanFilter::SetUseAdaptive(use_adaptive); - double max_adaptive_score = max_match_dist; - KalmanFilter::SetMaxAdaptiveScore(max_adaptive_score); - float centroid_measurement_noise = 0.4f; - float centroid_initial_velocity_variance = 5.0f; - float propagation_variance_xy = 10.0f; - float propagation_variance_z = 10.0f; - if (!model_config->GetValue("centroid_measurement_noise", - ¢roid_measurement_noise)) { - AERROR << "centroid_measurement_noise not found." << name(); + if (!KalmanFilter::SetAssociationScoreMaximum( + association_score_maximum)) { + AERROR << "Failed to set association score maximum! " << name(); + return false; + } + if (!model_config->GetValue("measurement_noise", &measurement_noise)) { + AERROR << "Failed to get measurement noise! " << name(); return false; } - if (!model_config->GetValue("centroid_initial_velocity_variance", - ¢roid_initial_velocity_variance)) { - AERROR << "centroid_initial_velocity_variance not found." << name(); + if (!model_config->GetValue("initial_velocity_noise", + &initial_velocity_noise)) { + AERROR << "Failed to get initial velocity noise! " << name(); return false; } - if (!model_config->GetValue("propagation_variance_xy", - &propagation_variance_xy)) { - AERROR << "propagation_variance_xy not found." << name(); + if (!model_config->GetValue("xy_propagation_noise", + &xy_propagation_noise)) { + AERROR << "Failed to get xy propagation noise! " << name(); return false; } - if (!model_config->GetValue("propagation_variance_z", - &propagation_variance_z)) { - AERROR << "propagation_variance_z not found." << name(); + if (!model_config->GetValue("z_propagation_noise", + &z_propagation_noise)) { + AERROR << "Failed to get z propagation noise! " << name(); + return false; + } + if (!KalmanFilter::InitParams(measurement_noise, initial_velocity_noise, + xy_propagation_noise, + z_propagation_noise)) { + AERROR << "Failed to set params for kalman filter! " << name(); return false; } - KalmanFilter::InitParams(centroid_measurement_noise, - centroid_initial_velocity_variance, - propagation_variance_xy, - propagation_variance_z); } return true; } +bool HmObjectTracker::SetMatcherMethod( + const std::string& matcher_method_name) { + if (matcher_method_name == "hungarian_matcher") { + matcher_method_ = HUNGARIAN_MATCHER; + AINFO << "matcher method of " << name() << " is " << matcher_method_name; + return true; + } + AERROR << "invalid matcher method name of " << name(); + return false; +} + +bool HmObjectTracker::SetCollectConsecutiveInvisibleMaximum( + const int& collect_consecutive_invisible_maximum) { + if (collect_consecutive_invisible_maximum >= 0) { + collect_consecutive_invisible_maximum_ = + collect_consecutive_invisible_maximum; + AINFO << "collect consecutive invisible maximum of " << name() << " is " + << collect_consecutive_invisible_maximum_; + return true; + } + AERROR << "invalid collect consecutive invisible maximum of " << name(); + return false; +} + +bool HmObjectTracker::SetCollectAgeMinimum( + const int& collect_age_minimum) { + if (collect_age_minimum >= 0) { + collect_age_minimum_ = collect_age_minimum; + AINFO << "collect age minimum of " << name() << " is " + << collect_age_minimum_; + return true; + } + AERROR << "invalid collect age minimum of " << name(); + return false; +} + +bool HmObjectTracker::SetHistogramBinSize( + const int& histogram_bin_size) { + if (histogram_bin_size > 0) { + histogram_bin_size_ = histogram_bin_size; + AINFO << "histogram bin size of " << name() << " is " + << histogram_bin_size_; + return true; + } + AERROR << "invalid histogram bin size of " << name(); + return false; +} + const std::vector& HmObjectTracker::GetObjectTracks() const { return object_tracks_.GetTracks(); } @@ -259,12 +355,9 @@ bool HmObjectTracker::Track(const std::vector& objects, double timestamp, const TrackerOptions& options, std::vector* tracked_objects) { - // Track detected objects over consecutive frames - // A. track setup - if (tracked_objects == NULL) { + if (tracked_objects == NULL) return false; - } if (!valid_) { valid_ = true; return Initialize(objects, timestamp, options, tracked_objects); @@ -319,7 +412,7 @@ bool HmObjectTracker::Track(const std::vector& objects, } bool HmObjectTracker::Initialize(const std::vector& objects, - double timestamp, + const double& timestamp, const TrackerOptions& options, std::vector* tracked_objects) { // A. track setup @@ -359,8 +452,6 @@ bool HmObjectTracker::Initialize(const std::vector& objects, } void HmObjectTracker::TransformPoseGlobal2Local(Eigen::Matrix4d* pose) { - // Trasnform velodyne2world_pose to velodyne2local_pose to avoid huge value - // float computing in hm tracker (*pose)(0, 3) += global_to_local_offset_(0); (*pose)(1, 3) += global_to_local_offset_(1); (*pose)(2, 3) += global_to_local_offset_(2); @@ -371,8 +462,6 @@ void HmObjectTracker::ConstructTrackedObjects( std::vector* tracked_objects, const Eigen::Matrix4d& pose, const TrackerOptions& options) { - // Construct tracked objects. Help tracked objects with necessary - // transformation & feature extraction & lane direction query int num_objects = objects.size(); tracked_objects->clear(); tracked_objects->resize(num_objects); @@ -396,7 +485,7 @@ void HmObjectTracker::ConstructTrackedObjects( query_pt.z = anchor_point(2) - global_to_local_offset_(2); Eigen::Vector3d lane_dir; if (!options.hdmap_input->GetNearestLaneDirection(query_pt, &lane_dir)) { - AERROR << "Failed to initialize the lane direction of tracked object"; + AERROR << "Failed to initialize the lane direction of tracked object!"; // Set lane dir as host dir if query lane direction failed lane_dir = (pose * Eigen::Vector4d(1, 0, 0, 0)).head(3); } @@ -415,18 +504,18 @@ void HmObjectTracker::TransformTrackedObject(TrackedObjectPtr* obj, const Eigen::Matrix4d& pose) { // Transform tracked object with given pose TransformObject(&((*obj)->object_ptr), pose); - + // transform direction Eigen::Vector3f& dir = (*obj)->direction; dir = (pose * Eigen::Vector4d( - dir[0], dir[1], dir[2], 0)).head(3).cast(); - + dir(0), dir(1), dir(2), 0)).head(3).cast(); + // transform center Eigen::Vector3f& center = (*obj)->center; center = (pose * Eigen::Vector4d( - center[0], center[1], center[2], 1)).head(3).cast(); - + center(0), center(1), center(2), 1)).head(3).cast(); + // transform barycenter Eigen::Vector3f& barycenter = (*obj)->barycenter; barycenter = (pose * Eigen::Vector4d( - barycenter[0], barycenter[1], barycenter[2], 1)).head(3).cast(); + barycenter(0), barycenter(1), barycenter(2), 1)).head(3).cast(); } void HmObjectTracker::TransformObject(ObjectPtr* obj, @@ -434,18 +523,18 @@ void HmObjectTracker::TransformObject(ObjectPtr* obj, // Transform object with given pose Eigen::Vector3d& dir = (*obj)->direction; dir = (pose * Eigen::Vector4d(dir[0], dir[1], dir[2], 0)).head(3); - + // transform center Eigen::Vector3d& center = (*obj)->center; center = (pose * Eigen::Vector4d( center[0], center[1], center[2], 1)).head(3); - + // transform cloud & polygon TransformPointCloud(pose, (*obj)->cloud); TransformPointCloud(pose, &((*obj)->polygon)); } void HmObjectTracker::ComputeTracksPredict( std::vector* tracks_predict, - const double time_diff) { + const double& time_diff) { // Compute tracks' predicted states int no_track = object_tracks_.Size(); tracks_predict->resize(no_track); @@ -459,7 +548,7 @@ void HmObjectTracker::UpdateAssignedTracks( std::vector* tracks_predict, std::vector* new_objects, const std::vector& assignments, - const double time_diff) { + const double& time_diff) { // Update assigned tracks std::vector& tracks = object_tracks_.GetTracks(); for (size_t i = 0; i < assignments.size(); i++) { @@ -472,7 +561,7 @@ void HmObjectTracker::UpdateAssignedTracks( void HmObjectTracker::UpdateUnassignedTracks( const std::vector& tracks_predict, const std::vector& unassigned_tracks, - const double time_diff) { + const double& time_diff) { // Update tracks without matched objects std::vector& tracks = object_tracks_.GetTracks(); for (size_t i = 0; i < unassigned_tracks.size(); i++) { @@ -485,7 +574,7 @@ void HmObjectTracker::UpdateUnassignedTracks( void HmObjectTracker::CreateNewTracks( const std::vector& new_objects, const std::vector& unassigned_objects, - const double time_diff) { + const double& time_diff) { // Create new tracks for objects without matched tracks for (size_t i = 0; i < unassigned_objects.size(); i++) { int obj_id = unassigned_objects[i]; diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.h b/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.h index 01b97277dc..70e1319397 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.h +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.h @@ -20,6 +20,7 @@ #include #include #include + #include "modules/common/macro.h" #include "modules/perception/obstacle/base/object.h" #include "modules/perception/obstacle/lidar/interface/base_tracker.h" @@ -42,11 +43,36 @@ class HmObjectTracker : public BaseTracker{ // @return true if initialize successfully, otherwise return false bool Init(); + // @brief set matcher method + // @params[IN] matcher_method_name: name of mathcer method + // @return true if set successfully, otherwise return fasle + bool SetMatcherMethod( + const std::string& matcher_method_name); + + // @brief set collect consecutive invisible maximum + // @params[IN] collect_consecutive_invisible_maximum: collect consecutive + // invisible maximum + // @return true if set successfully, otherwise return fasle + bool SetCollectConsecutiveInvisibleMaximum( + const int& collect_consecutive_invisible_maximum); + + // @brief set collect age minimum + // @params[IN] collect_age_minimum: collect age minimum + // @return true if set successfully, otherwise return fasle + bool SetCollectAgeMinimum( + const int& collect_age_minimum); + + // @brief set histogram bin size + // @params[IN] histogram_bin_size: histogram bin size + // @return true if set successfully, otherwise return fasle + bool SetHistogramBinSize( + const int& histogram_bin_size); + // @brief track detected objects over consecutive frames - // @params[IN] objects: new detected objects for tracking - // @params[IN] timestamp: current timestamp for tracking - // @params[IN] options: tracker options contain information like pose - // @params[OUT] tracked_objects: tracked objects with track id + // @params[IN] objects: recently detected objects + // @params[IN] timestamp: timestamp of recently detected objects + // @params[IN] options: tracker options with necessary information + // @params[OUT] tracked_objects: tracked objects with tracking information // @return true if track successfully, otherwise return false bool Track( const std::vector& objects, @@ -63,30 +89,31 @@ class HmObjectTracker : public BaseTracker{ } protected: - // @brief initialize tracker after obtaining first frame's detections - // @params[IN] objects: new objects for tracking - // @params[IN] timestamp: current timestamp for tracking - // @params[IN] options: tracker options contain information like pose - // @params[IN] tracked_objects: tracked objects + // @brief initialize tracker after obtaining detection of first frame + // @params[IN] objects: recently detected objects + // @params[IN] timestamp: timestamp of recently detected objects + // @params[IN] options: tracker options with necessary information + // @params[OUT] tracked_objects: tracked objects with tracking information // @return true if initialize successfully, otherwise return false bool Initialize( const std::vector& objects, - double timestamp, + const double& timestamp, const TrackerOptions& options, std::vector* tracked_objects); // @brief transform v2world pose to v2local pose intend to avoid huge value // float computing - // @params[IN] pose: v2world pose + // @params[OUT] pose: v2world pose // @return nothing void TransformPoseGlobal2Local( Eigen::Matrix4d* pose); // @brief construct tracked objects via necessray transformation & feature // computing - // @params[IN] objects: new objects for constructions - // @params[OUT] tracked_objects: constructed objects for tracking + // @params[IN] objects: objects for construction + // @params[OUT] tracked_objects: constructed objects // @params[IN] pose: pose using for coordinate transformation + // @params[IN] options: tracker options with necessary information // @return nothing void ConstructTrackedObjects( const std::vector& objects, @@ -95,13 +122,13 @@ class HmObjectTracker : public BaseTracker{ const TrackerOptions& options); // @brief compute objects' shape feature - // @params[IN] object: object for computing shape feature + // @params[OUT] object: object for computing shape feature // @return nothing void ComputeShapeFeatures( TrackedObjectPtr* obj); // @brief transform tracked object with given pose - // @params[IN] obj: tracked object for transfromation + // @params[OUT] obj: tracked object for transfromation // @params[IN] pose: pose using for coordinate transformation // @return nothing void TransformTrackedObject( @@ -109,70 +136,62 @@ class HmObjectTracker : public BaseTracker{ const Eigen::Matrix4d& pose); // @brief transform object with given pose - // @params[IN] obj: object for transfromation + // @params[OUT] obj: object for transfromation // @params[IN] pose: pose using for coordinate transformation // @return nothing void TransformObject( ObjectPtr* obj, const Eigen::Matrix4d& pose); - // @brief compute tracks' predict states - // @params[OUT] tracks_predict: tracks' predict states + // @brief compute predicted states of maintained tracks + // @params[OUT] tracks_predict: predicted states of maintained tracks // @params[IN] time_diff: time interval for predicting // @return nothing void ComputeTracksPredict( std::vector* tracks_predict, - const double time_diff); + const double& time_diff); // @brief update assigned tracks - // @params[IN] tracks_predict: tracks' predict states - // @params[IN] new_objects: new objects for current updating - // @params[IN] assignments: assignment pair of new objects & tracks + // @params[IN] tracks_predict: predicted states of maintained tracks + // @params[IN] new_objects: recently detected objects + // @params[IN] assignments: assignment pair of // @params[IN] time_diff: time interval for updating // @return nothing void UpdateAssignedTracks( std::vector* tracks_predict, std::vector* new_objects, const std::vector& assignments, - const double time_diff); + const double& time_diff); // @brief update tracks without matched objects - // @params[IN] tracks_predict: tracks' predict states + // @params[IN] tracks_predict: predicted states of maintained tracks // @params[IN] unassigned_tracks: index of unassigned tracks // @params[IN] time_diff: time interval for updating // @return nothing void UpdateUnassignedTracks( const std::vector& tracks_predict, const std::vector& unassigned_tracks, - const double time_diff); + const double& time_diff); - // @brief create new tracks for objects without matched tracks - // @params[IN] new_objects: new objects for current updating + // @brief create new tracks for objects without matched track + // @params[IN] new_objects: recently detected objects // @params[IN] unassigned_objects: index of unassigned objects // @params[IN] time_diff: time interval for updating // @return nothing void CreateNewTracks( const std::vector& new_objects, const std::vector& unassigned_objects, - const double time_diff); + const double& time_diff); // @brief delete lost tracks // @return nothing void DeleteLostTracks(); - // @brief collect tracked results for reporting - // @params[OUT] tracked_objects: tracked objects include objects may be - // occluded temporaryly + // @brief collect tracked results + // @params[OUT] tracked_objects: tracked objects with tracking information // @return nothing void CollectTrackedResults(std::vector* tracked_objects); - // @brief compute track ids for rencet objects - // @params[OUT] tracked_objects: tracked objects include objects may be - // occluded temporaryly - // @return nothing - void ComputeTrackIdsForRecentObjects( - const std::vector& objects); - private: // algorithm setup MatcherType matcher_method_; diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/hungarian_matcher.cc b/modules/perception/obstacle/lidar/tracker/hm_tracker/hungarian_matcher.cc index 834b52fd67..171bbe7f57 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/hungarian_matcher.cc +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/hungarian_matcher.cc @@ -16,6 +16,7 @@ #include #include + #include "modules/common/log.h" #include "modules/perception/obstacle/common/graph_util.h" #include "modules/perception/obstacle/common/geometry_util.h" @@ -26,26 +27,27 @@ namespace apollo { namespace perception { -float HungarianMatcher::s_max_match_distance_ = 4.0f; +float HungarianMatcher::s_match_distance_maximum_ = 4.0f; -bool HungarianMatcher::SetMaxMatchDistance(float max_match_distance) { - if (max_match_distance >= 0) { - s_max_match_distance_ = max_match_distance; +bool HungarianMatcher::SetMatchDistanceMaximum( + const float& match_distance_maximum) { + if (match_distance_maximum >= 0) { + s_match_distance_maximum_ = match_distance_maximum; + AINFO << "match distance maximum of HungarianMatcher is " + << s_match_distance_maximum_; return true; - } else { - AERROR << "invalid max_match_distance for HungraianMatcher."; - return false; } + AERROR << "invalid match distance maximum of HungarianMatcher!"; + return false; } void HungarianMatcher::Match(std::vector* objects, const std::vector& tracks, const std::vector& tracks_predict, - const double time_diff, + const double& time_diff, std::vector* assignments, std::vector* unassigned_tracks, std::vector* unassigned_objects) { - // Match detected objects to tracks // A. computing association matrix Eigen::MatrixXf association_mat(tracks.size(), objects->size()); ComputeAssociateMatrix(tracks, tracks_predict, (*objects), time_diff, @@ -54,7 +56,7 @@ void HungarianMatcher::Match(std::vector* objects, // B. computing connected components std::vector > object_components; std::vector > track_components; - ComputeConnectedComponents(association_mat, s_max_match_distance_, + ComputeConnectedComponents(association_mat, s_match_distance_maximum_, &track_components, &object_components); AINFO << "HungarianMatcher: partition graph into " << track_components.size() << " sub-graphs."; @@ -86,9 +88,9 @@ void HungarianMatcher::Match(std::vector* objects, } } -void HungarianMatcher::MatchComponents(const Eigen::MatrixXf association_mat, - const std::vector track_component, - const std::vector object_component, +void HungarianMatcher::MatchComponents(const Eigen::MatrixXf& association_mat, + const std::vector& track_component, + const std::vector& object_component, std::vector* sub_assignments, std::vector* sub_unassigned_tracks, std::vector* sub_unassigned_objects) { @@ -111,7 +113,7 @@ void HungarianMatcher::MatchComponents(const Eigen::MatrixXf association_mat, if (track_component.size() == 1 && object_component.size() == 1) { int track_id = track_component[0]; int object_id = object_component[0]; - if (association_mat(track_id, object_id) <= s_max_match_distance_) { + if (association_mat(track_id, object_id) <= s_match_distance_maximum_) { sub_assignments->push_back(std::make_pair(track_id, object_id)); } else { sub_unassigned_objects->push_back(object_id); @@ -146,7 +148,7 @@ void HungarianMatcher::MatchComponents(const Eigen::MatrixXf association_mat, local_unassigned_tracks.assign(local_association_mat.rows(), -1); local_unassigned_objects.assign(local_association_mat.cols(), -1); AssignObjectsToTracks(local_association_mat, - s_max_match_distance_, &local_assignments, &local_unassigned_tracks, + s_match_distance_maximum_, &local_assignments, &local_unassigned_tracks, &local_unassigned_objects); for (size_t i = 0; i < local_assignments.size(); ++i) { int global_track_id = track_local2global[local_assignments[i].first]; @@ -168,7 +170,7 @@ void HungarianMatcher::ComputeAssociateMatrix( const std::vector& tracks, const std::vector& tracks_predict, const std::vector& new_objects, - const double time_diff, + const double& time_diff, Eigen::MatrixXf* association_mat) { // Compute matrix of association distance for (size_t i = 0; i < tracks.size(); ++i) { @@ -183,7 +185,7 @@ float HungarianMatcher::ComputeTrackObjectDistance( const ObjectTrackPtr& track, const Eigen::VectorXf& track_predict, const TrackedObjectPtr& new_object, - const double time_diff) const { + const double& time_diff) const { // Compute distance of given track & object return TrackObjectDistance::ComputeDistance( track, track_predict, new_object, time_diff); @@ -191,7 +193,7 @@ float HungarianMatcher::ComputeTrackObjectDistance( void HungarianMatcher::ComputeConnectedComponents( const Eigen::MatrixXf& association_mat, - const float connected_threshold, + const float& connected_threshold, std::vector >* track_components, std::vector >* object_components) { // Compute connected components within given threshold @@ -229,7 +231,7 @@ void HungarianMatcher::ComputeConnectedComponents( void HungarianMatcher::AssignObjectsToTracks( const Eigen::MatrixXf& association_mat, - const double max_distance, + const double& assign_distance_maximum, std::vector* assignments, std::vector* unassigned_tracks, std::vector* unassigned_objects) { @@ -250,7 +252,7 @@ void HungarianMatcher::AssignObjectsToTracks( cost[i + no_track].resize(no_object); for (int j = 0; j < no_object; ++j) { if (j == i) { - cost[i + no_track][j] = max_distance * 1.2f; + cost[i + no_track][j] = assign_distance_maximum * 1.2f; } else { cost[i + no_track][j] = 999999.0f; } @@ -268,9 +270,10 @@ void HungarianMatcher::AssignObjectsToTracks( objects_idx[i] < 0 || objects_idx[i] >= no_object) { continue; } - if (association_mat(tracks_idx[i], objects_idx[i]) < max_distance) { - (*assignments)[assignments_num++] = std::make_pair(tracks_idx[i], - objects_idx[i]); + if (association_mat(tracks_idx[i], objects_idx[i]) < + assign_distance_maximum) { + (*assignments)[assignments_num++] = + std::make_pair(tracks_idx[i], objects_idx[i]); tracks_used[tracks_idx[i]] = true; objects_used[objects_idx[i]] = true; } diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/hungarian_matcher.h b/modules/perception/obstacle/lidar/tracker/hm_tracker/hungarian_matcher.h index 16a195e65b..f49198af7e 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/hungarian_matcher.h +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/hungarian_matcher.h @@ -19,6 +19,7 @@ #include #include + #include "modules/perception/obstacle/lidar/tracker/hm_tracker/base_matcher.h" namespace apollo { @@ -29,10 +30,11 @@ class HungarianMatcher: public BaseMatcher{ HungarianMatcher() {} ~HungarianMatcher() {} - // @brief set max match distance for matcher object - // @params[IN] max_match_distance: threshold of match distance + // @brief set match distance maximum for matcher + // @params[IN] match_distance_maximum: match distance maximum // @return true if set successfuly, otherwise return false - static bool SetMaxMatchDistance(float max_match_distance); + static bool SetMatchDistanceMaximum( + const float& match_distance_maximum); // @brief match detected objects to tracks // @params[IN] objects: new detected objects for matching @@ -46,7 +48,7 @@ class HungarianMatcher: public BaseMatcher{ std::vector* objects, const std::vector& tracks, const std::vector& tracks_predict, - const double time_diff, + const double& time_diff, std::vector* assignments, std::vector* unassigned_tracks, std::vector* unassigned_objects); @@ -59,9 +61,9 @@ class HungarianMatcher: public BaseMatcher{ // @params[OUT] sub_unassigned_tracks: component tracks not matched // @params[OUT] sub_unasgined_objects: component objects not matched void MatchComponents( - const Eigen::MatrixXf association_mat, - const std::vector track_component, - const std::vector obj_component, + const Eigen::MatrixXf& association_mat, + const std::vector& track_component, + const std::vector& obj_component, std::vector* sub_assignments, std::vector* sub_unassigned_tracks, std::vector* sub_unassigned_objects); @@ -74,7 +76,7 @@ class HungarianMatcher: public BaseMatcher{ // @brief compute association matrix // @params[IN] tracks: maintained tracks for matching // @params[IN] tracks_predict: predicted states of maintained tracks - // @params[IN] new_objects: new detected objects for matching + // @params[IN] new_objects: recently detected objects // @params[IN] time_diff: time interval from last matching // @params[OUT] association_mat: matrix of association distance // @return nothing @@ -82,20 +84,20 @@ class HungarianMatcher: public BaseMatcher{ const std::vector& tracks, const std::vector& tracks_predict, const std::vector& new_objects, - const double time_diff, + const double& time_diff, Eigen::MatrixXf* association_mat); // @brief compute distance between track & object - // @params[IN] track: track for matching - // @params[IN] track_predict: predicted states of track for matching - // @params[IN] new_object: new detected object for matching + // @params[IN] track: maintained track + // @params[IN] track_predict: predicted states of given track + // @params[IN] new_object: recently detected object // @params[IN] time_diff: time interval from last matching // @return distance of given track & object float ComputeTrackObjectDistance( const ObjectTrackPtr& track, const Eigen::VectorXf& track_predict, const TrackedObjectPtr& new_object, - const double time_diff) const; + const double& time_diff) const; // @brief compute connected components within given threshold // @params[IN] association_mat: matrix of association distance @@ -105,27 +107,27 @@ class HungarianMatcher: public BaseMatcher{ // @return nothing void ComputeConnectedComponents( const Eigen::MatrixXf& association_mat, - const float connected_threshold, + const float& connected_threshold, std::vector >* track_components, std::vector >* obj_components); // @brief assign objects to tracks using components // @params[IN] association_mat: matrix of association distance - // @params[IN] max_distance: threshold of matching + // @params[IN] assign_distance_maximum: threshold distance of assignment // @params[OUT] assignments: assignment pair of matched object & track // @params[OUT] unassigned_tracks: tracks without matched object // @params[OUT] unassigned_objects: objects without matched track // @return nothing void AssignObjectsToTracks( const Eigen::MatrixXf& association_mat, - const double max_distance, + const double& assign_distance_maximum, std::vector* assignments, std::vector* unassigned_tracks, std::vector* unassigned_objects); private: // threshold of matching - static float s_max_match_distance_; + static float s_match_distance_maximum_; DISALLOW_COPY_AND_ASSIGN(HungarianMatcher); }; // class HmMatcher diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/kalman_filter.cc b/modules/perception/obstacle/lidar/tracker/hm_tracker/kalman_filter.cc index 4bd76ae366..6fdd5306a4 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/kalman_filter.cc +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/kalman_filter.cc @@ -15,6 +15,7 @@ *****************************************************************************/ #include + #include "modules/common/log.h" #include "modules/perception/obstacle/common/geometry_util.h" #include "modules/perception/obstacle/lidar/tracker/hm_tracker/kalman_filter.h" @@ -23,49 +24,73 @@ namespace apollo { namespace perception { bool KalmanFilter::s_use_adaptive_ = true; -double KalmanFilter::s_max_adaptive_score_ = 1.0; +double KalmanFilter::s_association_score_maximum_ = 1.0; +Eigen::Matrix3d KalmanFilter::s_propagation_noise_ = 10 * + Eigen::Matrix3d::Identity(); double KalmanFilter::s_measurement_noise_ = 0.4; -double KalmanFilter::s_init_velocity_variance_ = 5; -double KalmanFilter::s_propagation_variance_xy_ = 10; -double KalmanFilter::s_propagation_variance_z_ = 10; +double KalmanFilter::s_initial_velocity_noise_ = 5; -void KalmanFilter::SetUseAdaptive(const bool use_adaptive) { +void KalmanFilter::SetUseAdaptive(const bool& use_adaptive) { s_use_adaptive_ = use_adaptive; + AINFO << "use adaptive of KalmanFilter is " << s_use_adaptive_; } -void KalmanFilter::SetMaxAdaptiveScore( - const double max_adaptive_score) { - s_max_adaptive_score_ = max_adaptive_score; +bool KalmanFilter::SetAssociationScoreMaximum( + const double& association_score_maximum) { + if (association_score_maximum > 0) { + s_association_score_maximum_ = association_score_maximum; + AINFO << "association score maximum of KalmanFilter is " + << s_association_score_maximum_; + return true; + } + AERROR << "invalid association score maximum of KalmanFilter!"; + return false; } -void KalmanFilter::InitParams(const double measurement_noise, - const double init_velocity_variance, - const double propagation_variance_xy, - const double propagation_variance_z) { +bool KalmanFilter::InitParams( + const double& measurement_noise, + const double& initial_velocity_noise, + const double& xy_propagation_noise, + const double& z_propagation_noise) { + if (measurement_noise < 0) { + AERROR << "invalid measurement noise of KalmanFilter!"; + return false; + } + if (initial_velocity_noise < 0) { + AERROR << "invalid intial velocity noise of KalmanFilter!"; + return false; + } + if (xy_propagation_noise < 0) { + AERROR << "invalid xy propagation noise of KalmanFilter!"; + return false; + } + if (z_propagation_noise < 0) { + AERROR << "invalid z propagation noise of KalmanFilter!"; + return false; + } s_measurement_noise_ = measurement_noise; - s_init_velocity_variance_ = init_velocity_variance; - s_propagation_variance_xy_ = propagation_variance_xy; - s_propagation_variance_z_ = propagation_variance_z; + s_initial_velocity_noise_ = initial_velocity_noise; + s_propagation_noise_(0, 0) = xy_propagation_noise; + s_propagation_noise_(1, 1) = xy_propagation_noise; + s_propagation_noise_(2, 2) = z_propagation_noise; + AINFO << "measurment noise of KalmanFilter is " << s_measurement_noise_; + AINFO << "initial velocity noise of KalmanFilter is " + << s_initial_velocity_noise_; + AINFO << "propagation noise of KalmanFilter is\n" << s_propagation_noise_; + return true; } KalmanFilter::KalmanFilter() { name_ = "KalmanFilter"; age_ = 0; - covariance_velocity_ = s_init_velocity_variance_ * + velocity_covariance_ = s_initial_velocity_noise_ * Eigen::Matrix3d::Identity(); - covariance_propagation_uncertainty_ = Eigen::Matrix3d::Zero(); - covariance_propagation_uncertainty_(0, 0) = s_propagation_variance_xy_; - covariance_propagation_uncertainty_(1, 1) = s_propagation_variance_xy_; - covariance_propagation_uncertainty_(2, 2) = s_propagation_variance_z_; - + // states update_quality_ = 1.0; belief_velocity_ = Eigen::Vector3d::Zero(); belief_velocity_accelaration_ = Eigen::Vector3d::Zero(); } -KalmanFilter::~KalmanFilter() { -} - void KalmanFilter::Initialize(const Eigen::Vector3f& anchor_point, const Eigen::Vector3f& velocity) { update_quality_ = 1.0; @@ -74,7 +99,7 @@ void KalmanFilter::Initialize(const Eigen::Vector3f& anchor_point, belief_velocity_accelaration_ = Eigen::Vector3d::Zero(); } -Eigen::VectorXf KalmanFilter::Predict(const double time_diff) { +Eigen::VectorXf KalmanFilter::Predict(const double& time_diff) { // Compute predict states Eigen::VectorXf predicted_state; predicted_state.resize(6); @@ -87,7 +112,6 @@ Eigen::VectorXf KalmanFilter::Predict(const double time_diff) { predicted_state(3) = belief_velocity_(0); predicted_state(4) = belief_velocity_(1); predicted_state(5) = belief_velocity_(2); - // Compute predicted covariance Propagate(time_diff); return predicted_state; @@ -95,29 +119,29 @@ Eigen::VectorXf KalmanFilter::Predict(const double time_diff) { void KalmanFilter::UpdateWithObject(const TrackedObjectPtr& new_object, const TrackedObjectPtr& old_object, - const double time_diff) { + const double& time_diff) { if (time_diff <= DBL_EPSILON) { - AWARN << "Time diff is too limited to updating filter."; + AWARN << "Time diff is too limited to updating KalmanFilter!"; return; } - // Compute update quality if needed + // A. Compute update quality if needed if (s_use_adaptive_) { ComputeUpdateQuality(new_object, old_object); } else { update_quality_ = 1.0; } - // Compute measurements + // B. Compute measurements Eigen::Vector3f measured_anchor_point = new_object->anchor_point; Eigen::Vector3f measured_velocity = ComputeMeasuredVelocity(new_object, old_object, time_diff); - // Update model + // C. Update model UpdateModel(measured_anchor_point, measured_velocity, time_diff); age_ += 1; } -void KalmanFilter::UpdateWithoutObject(const double time_diff) { +void KalmanFilter::UpdateWithoutObject(const double& time_diff) { // Only update belief anchor point belief_anchor_point_ += belief_velocity_ * time_diff; age_ += 1; @@ -137,19 +161,19 @@ void KalmanFilter::GetState(Eigen::Vector3f* anchor_point, (*velocity_accelaration) = belief_velocity_accelaration_.cast(); } -void KalmanFilter::Propagate(const double time_diff) { +void KalmanFilter::Propagate(const double& time_diff) { // Only propagate tracked motion if (age_ <= 0) { return; } - covariance_velocity_ += covariance_propagation_uncertainty_ * (time_diff * - time_diff); + velocity_covariance_ += s_propagation_noise_ * + (time_diff * time_diff); } Eigen::VectorXf KalmanFilter::ComputeMeasuredVelocity( const TrackedObjectPtr& new_object, const TrackedObjectPtr& old_object, - const double time_diff) { + const double& time_diff) { // Compute 2D velocity measurment for filtering // Obtain robust measurment via observation redundency @@ -175,7 +199,7 @@ Eigen::VectorXf KalmanFilter::ComputeMeasuredVelocity( Eigen::VectorXf KalmanFilter::ComputeMeasuredAnchorPointVelocity( const TrackedObjectPtr& new_object, const TrackedObjectPtr& old_object, - const double time_diff) { + const double& time_diff) { // Compute 2D anchor point velocity measurment Eigen::Vector3f measured_anchor_point_velocity = new_object->anchor_point - old_object->anchor_point; @@ -187,7 +211,7 @@ Eigen::VectorXf KalmanFilter::ComputeMeasuredAnchorPointVelocity( Eigen::VectorXf KalmanFilter::ComputeMeasuredBboxCenterVelocity( const TrackedObjectPtr& new_object, const TrackedObjectPtr& old_object, - const double time_diff) { + const double& time_diff) { // Compute 2D bbox center velocity measurment Eigen::Vector3d old_dir = old_object->direction.cast(); Eigen::Vector3d old_size = old_object->size.cast(); @@ -213,7 +237,7 @@ Eigen::VectorXf KalmanFilter::ComputeMeasuredBboxCenterVelocity( Eigen::VectorXf KalmanFilter::ComputeMeasuredBboxCornerVelocity( const TrackedObjectPtr& new_object, const TrackedObjectPtr& old_object, - const double time_diff) { + const double& time_diff) { // Compute 2D bbox corner velocity measurment Eigen::Vector3f project_dir = new_object->anchor_point - old_object->anchor_point; @@ -318,14 +342,14 @@ Eigen::Vector3f KalmanFilter::SelectMeasuredVelocityAccordingMotionConsistency( return measured_velocity; } -void KalmanFilter::UpdateModel(const Eigen::VectorXf measured_anchor_point, - const Eigen::VectorXf measured_velocity, - const double time_diff) { +void KalmanFilter::UpdateModel(const Eigen::VectorXf& measured_anchor_point, + const Eigen::VectorXf& measured_velocity, + const double& time_diff) { // Compute kalman gain Eigen::Matrix3d mat_c = Eigen::Matrix3d::Identity(); Eigen::Matrix3d mat_q = s_measurement_noise_ * Eigen::Matrix3d::Identity(); - Eigen::Matrix3d mat_k = covariance_velocity_ * mat_c.transpose() * - (mat_c * covariance_velocity_ * mat_c.transpose() + mat_q).inverse(); + Eigen::Matrix3d mat_k = velocity_covariance_ * mat_c.transpose() * + (mat_c * velocity_covariance_ * mat_c.transpose() + mat_q).inverse(); // Compute posterior belief Eigen::Vector3d measured_anchor_point_d = @@ -353,8 +377,8 @@ void KalmanFilter::UpdateModel(const Eigen::VectorXf measured_anchor_point, } // Compute posterior covariance - covariance_velocity_ = (Eigen::Matrix3d::Identity() - mat_k * mat_c) * - covariance_velocity_; + velocity_covariance_ = (Eigen::Matrix3d::Identity() - mat_k * mat_c) * + velocity_covariance_; } void KalmanFilter::ComputeUpdateQuality(const TrackedObjectPtr& new_object, @@ -379,10 +403,10 @@ float KalmanFilter::ComputeUpdateQualityAccordingAssociationScore( // Compute update quality according association score float association_score = new_object->association_score; float update_quality = 1; - if (s_max_adaptive_score_ == 0) { + if (s_association_score_maximum_ == 0) { return update_quality; } - update_quality = 1 - (association_score / s_max_adaptive_score_); + update_quality = 1 - (association_score / s_association_score_maximum_); update_quality = update_quality > 1 ? 1 : update_quality; update_quality = update_quality < 0 ? 0 : update_quality; update_quality = update_quality * update_quality; diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/kalman_filter.h b/modules/perception/obstacle/lidar/tracker/hm_tracker/kalman_filter.h index b971a22f3b..455a706832 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/kalman_filter.h +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/kalman_filter.h @@ -18,6 +18,7 @@ #define MODULES_PERCEPTION_OBSTACLE_LIDAR_TRACKER_HM_TRACKER_KALMAN_FILTER_H_ #include + #include "modules/perception/obstacle/base/object.h" #include "modules/perception/obstacle/lidar/tracker/hm_tracker/base_filter.h" @@ -27,31 +28,31 @@ namespace perception { class KalmanFilter : public BaseFilter { public: KalmanFilter(); - ~KalmanFilter(); + ~KalmanFilter() {} // @brief set use adaptive for all the filter objects // @params[IN] use_adaptive: flag of whether use adaptive version or not // @return nothing static void SetUseAdaptive( - const bool use_adaptive); - - // @brief set max adaptive score for computing update qaulity - // @params[IN] max_adaptive_score: adaptive score upperbound - // @return nothing - static void SetMaxAdaptiveScore( - const double max_adaptive_score); - - // @brief init core parameters of all the filter objects - // @params[IN] centroid_measurement_noise: noise of centroid measurement - // @params[IN] init_velocity_variance: initial variance of velocity - // @params[IN] propagation_variance_xy: propagation uncertainty of xy - // @params[IN] propagation_variance_z: propagation uncertainty of z - // @return nothing - static void InitParams( - const double measurement_noise, - const double init_velocity_variance, - const double propagation_variance_xy, - const double propagation_variance_z); + const bool& use_adaptive); + + // @brief set association score maximum for computing update qaulity + // @params[IN] association_score_maximum: association score maximum + // @return true if set successfully, otherwise return false + static bool SetAssociationScoreMaximum( + const double& association_score_maximum); + + // @brief init initialize parameters for kalman filter + // @params[IN] measurement_noise: noise of measurement + // @params[IN] initial_velocity_noise: initial uncertainty of velocity + // @params[IN] xy_propagation_noise: propagation uncertainty of xy + // @params[IN] z_propagation_noise: propagation uncertainty of z + // @return true if set successfully, otherwise return false + static bool InitParams( + const double& measurement_noise, + const double& initial_velocity_noise, + const double& xy_propagation_noise, + const double& z_propagation_noise); // @brief initialize the state of filter // @params[IN] anchor_point: initial anchor point for filtering @@ -65,7 +66,7 @@ class KalmanFilter : public BaseFilter { // @params[IN] time_diff: time interval for predicting // @return predicted states of filtering Eigen::VectorXf Predict( - const double time_diff); + const double& time_diff); // @brief update filter with object // @params[IN] new_object: new object for current updating @@ -75,13 +76,13 @@ class KalmanFilter : public BaseFilter { void UpdateWithObject( const TrackedObjectPtr& new_object, const TrackedObjectPtr& old_object, - const double time_diff); + const double& time_diff); // @brief update filter without object // @params[IN] time_diff: time interval from last updating // @return nothing void UpdateWithoutObject( - const double time_diff); + const double& time_diff); // @brief get state of filter // @params[OUT] anchor_point: anchor point of current state @@ -106,7 +107,7 @@ class KalmanFilter : public BaseFilter { // @params[IN] time_diff: time interval from last updating // @return nothing void Propagate( - const double time_diff); + const double& time_diff); // @brief compute measured velocity // @params[IN] new_object: new object for current updating @@ -116,7 +117,7 @@ class KalmanFilter : public BaseFilter { Eigen::VectorXf ComputeMeasuredVelocity( const TrackedObjectPtr& new_object, const TrackedObjectPtr& old_object, - const double time_diff); + const double& time_diff); // @brief compute measured anchor point velocity // @params[IN] new_object: new object for current updating @@ -126,7 +127,7 @@ class KalmanFilter : public BaseFilter { Eigen::VectorXf ComputeMeasuredAnchorPointVelocity( const TrackedObjectPtr& new_object, const TrackedObjectPtr& old_object, - const double time_diff); + const double& time_diff); // @brief compute measured bbox center velocity // @params[IN] new_object: new object for current updating @@ -136,7 +137,7 @@ class KalmanFilter : public BaseFilter { Eigen::VectorXf ComputeMeasuredBboxCenterVelocity( const TrackedObjectPtr& new_object, const TrackedObjectPtr& old_object, - const double time_diff); + const double& time_diff); // @brief compute measured bbox corner velocity // @params[IN] new_object: new object for current updating @@ -146,7 +147,7 @@ class KalmanFilter : public BaseFilter { Eigen::VectorXf ComputeMeasuredBboxCornerVelocity( const TrackedObjectPtr& new_object, const TrackedObjectPtr& old_object, - const double time_diff); + const double& time_diff); // @brief select measured velocity among candidates // @params[IN] candidates: candidates of measured velocity @@ -167,9 +168,9 @@ class KalmanFilter : public BaseFilter { // @params[IN] time_diff: time interval from last updating // @return nothing void UpdateModel( - const Eigen::VectorXf measured_anchor_point, - const Eigen::VectorXf measured_velocity, - const double time_diff); + const Eigen::VectorXf& measured_anchor_point, + const Eigen::VectorXf& measured_velocity, + const double& time_diff); // @brief compute update quality for adaptive filtering // @params[IN] new_object: new object for current updating @@ -196,29 +197,29 @@ class KalmanFilter : public BaseFilter { // @return breakdown threshold float ComputeBreakdownThreshold(); + + protected: // adaptive - static bool s_use_adaptive_; - static double s_max_adaptive_score_; + static bool s_use_adaptive_; + static double s_association_score_maximum_; // parameters - static double s_measurement_noise_; - static double s_init_velocity_variance_; - static double s_propagation_variance_xy_; - static double s_propagation_variance_z_; + static Eigen::Matrix3d s_propagation_noise_; + static double s_measurement_noise_; + static double s_initial_velocity_noise_; // filter history - int age_; + int age_; // filter covariances - Eigen::Matrix3d covariance_velocity_; - Eigen::Matrix3d covariance_propagation_uncertainty_; + Eigen::Matrix3d velocity_covariance_; // filter states - double update_quality_; - Eigen::Vector3d belief_anchor_point_; - Eigen::Vector3d belief_velocity_; - Eigen::Vector3d belief_velocity_accelaration_; + Eigen::Vector3d belief_anchor_point_; + Eigen::Vector3d belief_velocity_; + Eigen::Vector3d belief_velocity_accelaration_; + double update_quality_; }; // class KalmanFilter } // namespace perception diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/object_track.cc b/modules/perception/obstacle/lidar/tracker/hm_tracker/object_track.cc index 269367dbe0..aee63f53b5 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/object_track.cc +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/object_track.cc @@ -17,6 +17,7 @@ #include #include #include + #include "modules/common/log.h" #include "modules/common/util/util.h" #include "modules/map/hdmap/hdmap.h" @@ -29,40 +30,42 @@ namespace perception { int ObjectTrack::s_track_idx_ = 0; FilterType ObjectTrack::s_filter_method_ = KALMAN_FILTER; +int ObjectTrackSet::s_track_consecutive_invisible_maximum_ = 1; +float ObjectTrackSet::s_track_visible_ratio_minimum_ = 0.6; int ObjectTrack::s_track_cached_history_size_maximum_ = 20; -double ObjectTrack::s_speed_noise_maximum_ = 0.4; double ObjectTrack::s_acceleration_noise_maximum_ = 5; -int ObjectTrackSet::s_track_consecutive_invisible_maximum_ = 1; +double ObjectTrack::s_speed_noise_maximum_ = 0.4; -void ObjectTrack::SetFilterMethod(const FilterType& filter_method) { - // Set filter method for all the track objects - if (filter_method == KALMAN_FILTER) { - s_filter_method_ = filter_method; - } else { - AINFO << "invalid filter_method!"; +bool ObjectTrack::SetFilterMethod(const std::string& filter_method_name) { + if (filter_method_name == "kalman_filter") { + s_filter_method_ = KALMAN_FILTER; + AINFO << "filter method of object track is " << filter_method_name; + return true; } - AINFO << "track filter algorithm is " << s_filter_method_; + AERROR << "invalid filter method name of object track!"; + return false; } bool ObjectTrack::SetTrackCachedHistorySizeMaximum( const int& track_cached_history_size_maximum) { if (track_cached_history_size_maximum > 0) { s_track_cached_history_size_maximum_ = track_cached_history_size_maximum; - AINFO << "track cached history size maximum is " + AINFO << "track cached history size maximum of object track is " << s_track_cached_history_size_maximum_; return true; } - AERROR << "invalid track cached history size maximum!"; + AERROR << "invalid track cached history size maximum of object track!"; return false; } bool ObjectTrack::SetSpeedNoiseMaximum(const double& speed_noise_maximum) { if (speed_noise_maximum > 0) { s_speed_noise_maximum_ = speed_noise_maximum; - AINFO << "speed noise maximum is " << s_speed_noise_maximum_; + AINFO << "speed noise maximum of object track is " + << s_speed_noise_maximum_; return true; } - AERROR << "invalid speed noise maximum!"; + AERROR << "invalid speed noise maximum of object track!"; return false; } @@ -70,10 +73,11 @@ bool ObjectTrack::SetAccelerationNoiseMaximum( const double& acceleration_noise_maximum) { if (acceleration_noise_maximum > 0) { s_acceleration_noise_maximum_ = acceleration_noise_maximum; - AINFO << "acceleration noise maximum is " << s_acceleration_noise_maximum_; + AINFO << "acceleration noise maximum of object track is " + << s_acceleration_noise_maximum_; return true; } - AERROR << "invalid acceleration noise maximum!"; + AERROR << "invalid acceleration noise maximum of object track!"; return false; } @@ -89,10 +93,9 @@ int ObjectTrack::GetNextTrackId() { } ObjectTrack::ObjectTrack(TrackedObjectPtr obj) { - // Setup filter + // Initialize filter Eigen::Vector3f initial_anchor_point = obj->anchor_point; Eigen::Vector3f initial_velocity = Eigen::Vector3f::Zero(); - if (s_filter_method_ == KALMAN_FILTER) { filter_ = new KalmanFilter(); } @@ -111,11 +114,11 @@ ObjectTrack::ObjectTrack(TrackedObjectPtr obj) { belief_anchor_point_ = initial_anchor_point; belief_velocity_ = initial_velocity; belief_velocity_accelaration_ = Eigen::Vector3f::Zero(); - // NEED TO NOTICE: All the states would be collected mainly based on states // of tracked object. Thus, update tracked object when you update the state // of track !!!!! obj->velocity = belief_velocity_; + // Initialize object direction with its lane direction obj->direction = obj->lane_direction; } @@ -127,8 +130,7 @@ ObjectTrack::~ObjectTrack() { } } -Eigen::VectorXf ObjectTrack::Predict(const double time_diff) { - // Predict the state of track +Eigen::VectorXf ObjectTrack::Predict(const double& time_diff) { // Get the predict of filter Eigen::VectorXf filter_predict = filter_->Predict(time_diff); // Get the predict of track @@ -143,27 +145,15 @@ Eigen::VectorXf ObjectTrack::Predict(const double time_diff) { } void ObjectTrack::UpdateWithObject(TrackedObjectPtr* new_object, - const double time_diff) { + const double& time_diff) { // Update track with object - if ((*new_object) == nullptr) { + if ((*new_object) == nullptr) UpdateWithoutObject(time_diff); - } - - /* 1. update object track */ - // 1.1 check measurement outliers - bool updating_new_object_is_outlier = false; - - // 1.2 update with object if observation is not outlier - if (!updating_new_object_is_outlier) { - filter_->UpdateWithObject((*new_object), current_object_, time_diff); - filter_->GetState(&belief_anchor_point_, &belief_velocity_); - } else { - /* Here, we only update belief anchor point with anchor point of new detected object. In - * the future, new method that handle outlier could be designed to handle more complicated - * strategies. */ - belief_anchor_point_ = (*new_object)->anchor_point; - } + // A. update object track + // A.1 update filter + filter_->UpdateWithObject((*new_object), current_object_, time_diff); + filter_->GetState(&belief_anchor_point_, &belief_velocity_); // NEED TO NOTICE: All the states would be collected mainly based on states // of tracked object. Thus, update tracked object when you update the state // of track !!!!! @@ -172,17 +162,12 @@ void ObjectTrack::UpdateWithObject(TrackedObjectPtr* new_object, belief_velocity_accelaration_ = ((*new_object)->velocity - current_object_->velocity) / time_diff; - - /* Currently, we only considered outliers' influence on motion estimation. Track level - * smoothness of orientation & class idx may also take into acount it in the future. */ - - // 1.4 update track info + // A.2 update track info age_++; total_visible_count_++; consecutive_invisible_count_ = 0; period_ += time_diff; - - // 1.5 update history + // A.3 update history int history_size = history_objects_.size(); if (history_size >= s_track_cached_history_size_maximum_) { history_objects_.pop_front(); @@ -190,44 +175,29 @@ void ObjectTrack::UpdateWithObject(TrackedObjectPtr* new_object, history_objects_.push_back(current_object_); current_object_ = *new_object; - /* Previously, velocity of object track is smoothed after the smoothing of orientation & type. - * However, compared to the output of original filter, smoothed velocity, which is a posteriori, - * is more reasonable to be used in the smoothing of orientation & type. */ - - /* Previously, track static hypothesis is checked before smoothing strategies. Now, it has been - * moved in the method of smooth track velocity. This is more reasonable, as track static - * hypothesis is decided by velocity more directly when velocity estimation improved. */ - - /* 2. smooth object track */ - // 2.1 smooth velocity + // B. smooth object track + // B.1 smooth velocity SmoothTrackVelocity((*new_object), time_diff); - - // 2.2 smooth orientation + // B.2 smooth orientation SmoothTrackOrientation(); - - // 2.3 smooth class idx - SmoothTrackClassIdx(); } -void ObjectTrack::UpdateWithoutObject(const double time_diff) { - // Update track's obstacle object +void ObjectTrack::UpdateWithoutObject(const double& time_diff) { + // A. update object of track TrackedObjectPtr new_obj(new TrackedObject()); new_obj->clone(*current_object_); - Eigen::Vector3f predicted_shift = belief_velocity_ * time_diff; new_obj->anchor_point = current_object_->anchor_point + predicted_shift; new_obj->barycenter = current_object_->barycenter + predicted_shift; new_obj->center = current_object_->center + predicted_shift; - // Update obstacle cloud + // B. update cloud & polygon pcl_util::PointCloudPtr pc = new_obj->object_ptr->cloud; for (size_t j = 0; j < pc->points.size(); ++j) { pc->points[j].x += predicted_shift[0]; pc->points[j].y += predicted_shift[1]; pc->points[j].z += predicted_shift[2]; } - - // Update obstacle polygon PolygonDType& polygon = new_obj->object_ptr->polygon; for (size_t j = 0; j < polygon.points.size(); ++j) { polygon.points[j].x += predicted_shift[0]; @@ -235,23 +205,22 @@ void ObjectTrack::UpdateWithoutObject(const double time_diff) { polygon.points[j].z += predicted_shift[2]; } - // Update filter + // C. update filter filter_->UpdateWithoutObject(time_diff); - // Update states of track + // D. update states of track belief_anchor_point_ = new_obj->anchor_point; - // NEED TO NOTICE: All the states would be collected mainly based on states // of tracked object included temporaryly occluded ones. Thus, update // tracked object when you update the state of track !!!! new_obj->velocity = belief_velocity_; - // Update track info + // E. update track info age_++; consecutive_invisible_count_++; period_ += time_diff; - // Update history + // F. update history int history_size = history_objects_.size(); if (history_size >= s_track_cached_history_size_maximum_) { history_objects_.pop_front(); @@ -261,25 +230,22 @@ void ObjectTrack::UpdateWithoutObject(const double time_diff) { } void ObjectTrack::UpdateWithoutObject(const Eigen::VectorXf& predict_state, - const double time_diff) { - // Update track's obstacle object + const double& time_diff) { + // A. update object of track TrackedObjectPtr new_obj(new TrackedObject()); new_obj->clone(*current_object_); - Eigen::Vector3f predicted_shift = predict_state.tail(3) * time_diff; new_obj->anchor_point = current_object_->anchor_point + predicted_shift; new_obj->barycenter = current_object_->barycenter + predicted_shift; new_obj->center = current_object_->center + predicted_shift; - // Update obstacle cloud + // B. update cloud & polygon pcl_util::PointCloudPtr pc = new_obj->object_ptr->cloud; for (size_t j = 0; j < pc->points.size(); ++j) { pc->points[j].x += predicted_shift[0]; pc->points[j].y += predicted_shift[1]; pc->points[j].z += predicted_shift[2]; } - - // Update obstacle polygon PolygonDType& polygon = new_obj->object_ptr->polygon; for (size_t j = 0; j < polygon.points.size(); ++j) { polygon.points[j].x += predicted_shift[0]; @@ -287,25 +253,22 @@ void ObjectTrack::UpdateWithoutObject(const Eigen::VectorXf& predict_state, polygon.points[j].z += predicted_shift[2]; } - /* No need to update filter*/ - - // Update filter + // C. update filter without object filter_->UpdateWithoutObject(time_diff); - // Update states of track + // D. update states of track belief_anchor_point_ = new_obj->anchor_point; - // NEED TO NOTICE: All the states would be collected mainly based on states // of tracked object included temporaryly occluded ones. Thus, update // tracked object when you update the state of track !!!! new_obj->velocity = belief_velocity_; - // Update track info + // E. update track info age_++; consecutive_invisible_count_++; period_ += time_diff; - // Update history + // F. update history int history_size = history_objects_.size(); if (history_size >= s_track_cached_history_size_maximum_) { history_objects_.pop_front(); @@ -315,9 +278,8 @@ void ObjectTrack::UpdateWithoutObject(const Eigen::VectorXf& predict_state, } void ObjectTrack::SmoothTrackVelocity(const TrackedObjectPtr& new_object, - const double time_diff) { - // Smooth velocity over track history - // 1. keep motion if accelaration of filter is greater than a threshold + const double& time_diff) { + // A. keep motion if accelaration of filter is greater than a threshold Eigen::Vector3f filter_anchor_point = Eigen::Vector3f::Zero(); Eigen::Vector3f filter_velocity = Eigen::Vector3f::Zero(); Eigen::Vector3f filter_velocity_accelaration = Eigen::Vector3f::Zero(); @@ -333,23 +295,19 @@ void ObjectTrack::SmoothTrackVelocity(const TrackedObjectPtr& new_object, } belief_velocity_ = last_velocity; belief_velocity_accelaration_ = Eigen::Vector3f::Zero(); - // NEED TO NOTICE: All the states would be collected mainly based on // states of tracked object included temporaryly occluded ones. Thus, // update tracked object when you update the state of track !!!! current_object_->velocity = belief_velocity_; - // keep static hypothesis return; } - // 2. static hypothesis check + // B. static hypothesis check & claping noise is_static_hypothesis_ = CheckTrackStaticHypothesis(new_object->object_ptr, time_diff); - if (is_static_hypothesis_) { belief_velocity_ = Eigen::Vector3f::Zero(); belief_velocity_accelaration_ = Eigen::Vector3f::Zero(); - // NEED TO NOTICE: All the states would be collected mainly based on // states of tracked object included temporaryly occluded ones. Thus, // update tracked object when you update the state of track !!!! @@ -358,7 +316,6 @@ void ObjectTrack::SmoothTrackVelocity(const TrackedObjectPtr& new_object, } void ObjectTrack::SmoothTrackOrientation() { - // Smooth orientation over track history Eigen::Vector3f current_dir = current_object_->direction; float current_speed = current_object_->velocity.head(2).norm(); bool velocity_is_obvious = current_speed > (s_speed_noise_maximum_ * 2); @@ -379,16 +336,13 @@ void ObjectTrack::SmoothTrackOrientation() { current_object_->size = new_size.cast(); } -void ObjectTrack::SmoothTrackClassIdx() { -} - bool ObjectTrack::CheckTrackStaticHypothesis(const ObjectPtr& new_object, - const double time_diff) { - // Check whether track is static - // Check whether track velocity is consistant + const double& time_diff) { + // A. check whether track velocity angle changed obviously bool is_velocity_angle_change = CheckTrackStaticHypothesisByVelocityAngleChange(new_object, time_diff); - // Evaluate velocity level + + // B. evaluate velocity level double speed = belief_velocity_.head(2).norm(); bool velocity_is_noise = speed < (s_speed_noise_maximum_ / 2); bool velocity_is_small = speed < (s_speed_noise_maximum_ / 1); @@ -407,18 +361,12 @@ bool ObjectTrack::CheckTrackStaticHypothesis(const ObjectPtr& new_object, bool ObjectTrack::CheckTrackStaticHypothesisByVelocityAngleChange( const ObjectPtr& new_object, - const double time_diff) { - // Sub strategy of checking whether track is static or not - let track be - // static if velocity angle change from previous and current frame is - // larger than M_PI / 3.0 + const double& time_diff) { Eigen::Vector3f previous_velocity = history_objects_[history_objects_.size() - 1]->velocity; Eigen::Vector3f current_velocity = current_object_->velocity; double velocity_angle_change = VectorTheta2dXy(previous_velocity, current_velocity); - // Previously, this threshold is set to PI/3. Now, as the smoothness of the - // filter is improved by Robust Adaptive Kalman Filter, we would use more - // strict threshold in the future. if (fabs(velocity_angle_change) > M_PI / 4.0) { return true; } @@ -426,8 +374,7 @@ bool ObjectTrack::CheckTrackStaticHypothesisByVelocityAngleChange( } /*class ObjectTrackSet*/ -ObjectTrackSet::ObjectTrackSet(): age_threshold_(5), - minimum_visible_ratio_(0.6f) { +ObjectTrackSet::ObjectTrackSet() { tracks_.reserve(1000); } @@ -440,9 +387,24 @@ bool ObjectTrackSet::SetTrackConsecutiveInvisibleMaximum( if (track_consecutive_invisible_maximum >= 0) { s_track_consecutive_invisible_maximum_ = track_consecutive_invisible_maximum; + AINFO << "track consecutive invisible maximum of object track set is " + << s_track_consecutive_invisible_maximum_; + return true; + } + AERROR << "invalid track consecutive invisible maximum of object track! "; + return false; +} + +bool ObjectTrackSet::SetTrackVisibleRatioMinimum( + const float& track_visible_ratio_minimum) { + if (track_visible_ratio_minimum >= 0 && + track_visible_ratio_minimum <= 1) { + s_track_visible_ratio_minimum_ = track_visible_ratio_minimum; + AINFO << "track visible ratio minimum of object track set is " + << s_track_visible_ratio_minimum_; return true; } - AERROR << "Failed to set track consecutive invisible maximum! "; + AERROR << "invalid track visible ratio minimum of object track! "; return false; } @@ -458,26 +420,21 @@ void ObjectTrackSet::Clear() { int ObjectTrackSet::RemoveLostTracks() { size_t track_num = 0; - for (size_t i = 0; i < tracks_.size(); i++) { - if (tracks_[i]->age_ < age_threshold_ && - tracks_[i]->consecutive_invisible_count_ > 1) { - continue; - } - - float visible_ratio = tracks_[i]->total_visible_count_ * 1.0f / + for (size_t i = 0; i < tracks_.size(); ++i) { + // A. remove tracks invisible ratio less than given minimum + float track_visible_ratio = tracks_[i]->total_visible_count_ * 1.0f / tracks_[i]->age_; - if (visible_ratio < minimum_visible_ratio_) { + if (track_visible_ratio < s_track_visible_ratio_minimum_) continue; - } - - if (tracks_[i]->consecutive_invisible_count_ - > s_track_consecutive_invisible_maximum_) { + // B. remove tracks consecutive invisible count greater than given maximum + int track_consecutive_invisible_count = + tracks_[i]->consecutive_invisible_count_; + if (track_consecutive_invisible_count > + s_track_consecutive_invisible_maximum_) continue; - } - + // C. update if (i == track_num) { track_num++; - continue; } else { ObjectTrackPtr tmp = tracks_[i]; tracks_[i] = tracks_[track_num]; @@ -485,9 +442,9 @@ int ObjectTrackSet::RemoveLostTracks() { track_num++; } } - + // remove lost tracks int no_removed = tracks_.size() - track_num; - for (size_t i = track_num; i < tracks_.size(); i++) { + for (size_t i = track_num; i < tracks_.size(); ++i) { if (tracks_[i] != NULL) { delete (tracks_[i]); tracks_[i] = NULL; diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/object_track.h b/modules/perception/obstacle/lidar/tracker/hm_tracker/object_track.h index 0cfee54642..3241ba5368 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/object_track.h +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/object_track.h @@ -39,19 +39,29 @@ class ObjectTrack { // @brief set filter method for all the object track objects // @params[IN] filter_method: method name of filtering algorithm - // @return nothing - static void SetFilterMethod( - const FilterType& filter_method); - + // @return true if set successfully, otherwise return false + static bool SetFilterMethod( + const std::string& filter_method_name); + + // @brief set track cached history size maximum + // @params[IN] track_cached_history_size_maximum: track cached history size + // maximum + // @return true if set successfully, otherwise return false static bool SetTrackCachedHistorySizeMaximum( const int& track_cached_history_size_maximum); - static bool SetSpeedNoiseMaximum( - const double& speed_noise_maximum); - + // @brief set acceleration noise maximum + // @params[IN] acceleration_noise_maximum: acceleration noise maximum + // @return true if set successfully, otherwise return false static bool SetAccelerationNoiseMaximum( const double& acceleration_noise_maximum); + // @brief set speed noise maximum + // @params[IN] speed noise maximum: speed noise maximum + // @return true if set successfully, otherwise return false + static bool SetSpeedNoiseMaximum( + const double& speed_noise_maximum); + // @brief get next avaiable track id // @return next avaiable track id static int GetNextTrackId(); @@ -60,21 +70,21 @@ class ObjectTrack { // @params[IN] time_diff: time interval for predicting // @return predicted states of track Eigen::VectorXf Predict( - const double time_diff); + const double& time_diff); // @brief update track with object - // @params[IN] new_object: new object for current updating + // @params[IN] new_object: recent detected object for current updating // @params[IN] time_diff: time interval from last updating // @return nothing void UpdateWithObject( TrackedObjectPtr* new_object, - const double time_diff); + const double& time_diff); // @brief update track without object // @params[IN] time_diff: time interval from last updating // @return nothing void UpdateWithoutObject( - const double time_diff); + const double& time_diff); // @brief update track without object with given predicted state // @params[IN] predict_state: given predicted state of track @@ -82,7 +92,7 @@ class ObjectTrack { // @return nothing void UpdateWithoutObject( const Eigen::VectorXf& predict_state, - const double time_diff); + const double& time_diff); protected: // @brief smooth velocity over track history @@ -91,23 +101,19 @@ class ObjectTrack { // @return nothing void SmoothTrackVelocity( const TrackedObjectPtr& new_object, - const double time_diff); + const double& time_diff); // @brief smooth orientation over track history // @return nothing void SmoothTrackOrientation(); - // @brief smooth track class idx over track history - // @return nothing - void SmoothTrackClassIdx(); - // @brief check whether track is static or not // @params[IN] new_object: new detected object just updated // @params[IN] time_diff: time interval between last two updating // @return true if track is static, otherwise return false bool CheckTrackStaticHypothesis( const ObjectPtr& new_object, - const double time_diff); + const double& time_diff); // @brief sub strategy of checking whether track is static or not via // considering the velocity angle change @@ -116,7 +122,7 @@ class ObjectTrack { // @return true if track is static, otherwise return false bool CheckTrackStaticHypothesisByVelocityAngleChange( const ObjectPtr& new_object, - const double time_diff); + const double& time_diff); private: ObjectTrack(); @@ -164,36 +170,57 @@ class ObjectTrackSet { ObjectTrackSet(); ~ObjectTrackSet(); + // @brief set track consecutive invisible maximum + // @params[IN] track_consecutive_invisible_maximum: track consecutive + // invisible maximum + // @return true if set successfully, otherwise return false static bool SetTrackConsecutiveInvisibleMaximum( const int& track_consecutive_invisible_maximum); + // @brief set track visible ratio minimum + // @params[IN] track_visible_ratio_minimum: track visible ratio minimum + // @return true if set successfully, otherwise return false + static bool SetTrackVisibleRatioMinimum( + const float& track_visible_ratio_minimum); + + // @brief get maintained tracks + // @return maintained tracks inline std::vector& GetTracks() { return tracks_; } + // @brief get maintained tracks + // @return maintained tracks inline const std::vector& GetTracks() const { return tracks_; } + // @brief get size of maintained tracks + // @return size of maintained tracks inline int Size() const { return tracks_.size(); } + // @brief add track to current set of maintained tracks + // @return nothing void AddTrack(const ObjectTrackPtr& track) { tracks_.push_back(track); } + // @brief remove lost tracks + // @return number of removed tracks int RemoveLostTracks(); + // @brief clear maintained tracks + // @return nothing void Clear(); public: - static int s_track_consecutive_invisible_maximum_; + static int s_track_consecutive_invisible_maximum_; + static float s_track_visible_ratio_minimum_; private: std::vector tracks_; - int age_threshold_; - double minimum_visible_ratio_; }; // class ObjectTrackSet } // namespace perception diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/track_object_distance.cc b/modules/perception/obstacle/lidar/tracker/hm_tracker/track_object_distance.cc index 7eb9a4088f..f8a0786081 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/track_object_distance.cc +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/track_object_distance.cc @@ -16,6 +16,7 @@ #include #include + #include "modules/common/log.h" #include "modules/perception/obstacle/common/geometry_util.h" #include "modules/perception/obstacle/lidar/tracker/hm_tracker/track_object_distance.h" @@ -23,71 +24,76 @@ namespace apollo { namespace perception { -double TrackObjectDistance::s_weight_location_dist_ = 0.6; -double TrackObjectDistance::s_weight_direction_dist_ = 0.2; -double TrackObjectDistance::s_weight_bbox_size_dist_ = 0.1; -double TrackObjectDistance::s_weight_point_num_dist_ = 0.1; -double TrackObjectDistance::s_weight_histogram_dist_ = 0.5; - -bool TrackObjectDistance::SetWeightLocationDist(float weight_location_dist) { - // Set weight of location dist for all the track object distance objects - if (weight_location_dist >= 0) { - s_weight_location_dist_ = weight_location_dist; +double TrackObjectDistance::s_location_distance_weight_ = 0.6; +double TrackObjectDistance::s_direction_distance_weight_ = 0.2; +double TrackObjectDistance::s_bbox_size_distance_weight_ = 0.1; +double TrackObjectDistance::s_point_num_distance_weight_ = 0.1; +double TrackObjectDistance::s_histogram_distance_weight_ = 0.5; + +bool TrackObjectDistance::SetLocationDistanceWeight( + const float& location_distance_weight) { + if (location_distance_weight >= 0) { + s_location_distance_weight_ = location_distance_weight; + AINFO << "location distance weight of TrackObjectDistance is " + << s_location_distance_weight_; return true; - } else { - AERROR << "invalid weight_location_dist. TrackObjectDistance"; - return false; } + AERROR << "invalid location distance weight of TrackeObjectDistance!"; + return false; } -bool TrackObjectDistance::SetWeightDirectionDist(float weight_direction_dist) { - // Set weight of direction dist for all the track object distance objects - if (weight_direction_dist >= 0) { - s_weight_direction_dist_ = weight_direction_dist; +bool TrackObjectDistance::SetDirectionDistanceWeight( + const float& direction_distance_weight) { + if (direction_distance_weight >= 0) { + s_direction_distance_weight_ = direction_distance_weight; + AINFO << "direction distance weight of TrackObjectDistance is " + << s_direction_distance_weight_; return true; - } else { - AERROR << "invalid weight_direction_dist. TrackObjectDistance"; - return false; } + AERROR << "invalid direction distance weight of TrackObjectDistance!"; + return false; } -bool TrackObjectDistance::SetWeightBboxSizeDist(float weight_bbox_size_dist) { - // Set weight of bbox size dist for all the track object distance objects - if (weight_bbox_size_dist >= 0) { - s_weight_bbox_size_dist_ = weight_bbox_size_dist; +bool TrackObjectDistance::SetBboxSizeDistanceWeight( + const float& bbox_size_distance_weight) { + if (bbox_size_distance_weight >= 0) { + s_bbox_size_distance_weight_ = bbox_size_distance_weight; + AINFO << "bbox size distance weight of TrackObjectDistance is " + << s_bbox_size_distance_weight_; return true; - } else { - AERROR << "invalid weight_bbox_size_dist. TrackedObjectDistance"; - return false; } + AERROR << "invalid bbox size distance weight of TrackObjectDistance!"; + return false; } -bool TrackObjectDistance::SetWeightPointNumDist(float weight_point_num_dist) { - // Set weight of point num dist for all the track object distance objects - if (weight_point_num_dist >= 0) { - s_weight_point_num_dist_ = weight_point_num_dist; +bool TrackObjectDistance::SetPointNumDistanceWeight( + const float& point_num_distance_weight) { + if (point_num_distance_weight >= 0) { + s_point_num_distance_weight_ = point_num_distance_weight; + AINFO << "point num distance weight of TrackObjectDistance is " + << s_point_num_distance_weight_; return true; - } else { - AERROR << "invalid weight_point_num_dist. TrackedObjectDistance"; - return false; } + AERROR << "invalid point num distance weight of TrackObjectDistance!"; + return false; } -bool TrackObjectDistance::SetWeightHistogramDist(float weight_histogram_dist) { - // Set weight of histogram dist for all the track objects distance objects - if (weight_histogram_dist >= 0) { - s_weight_histogram_dist_ = weight_histogram_dist; +bool TrackObjectDistance::SetHistogramDistanceWeight( + const float& histogram_distance_weight) { + if (histogram_distance_weight >= 0) { + s_histogram_distance_weight_ = histogram_distance_weight; + AINFO << "histogram distance weight of TrackObjectDistance is " + << s_histogram_distance_weight_; return true; - } else { - AERROR << "invalid weight_histogram_dist. TrackedObjectDistance"; - return false; } + AERROR << "invalid histogram distance weight of TrackObjectDistance!"; + return false; } float TrackObjectDistance::ComputeDistance(const ObjectTrackPtr& track, const Eigen::VectorXf& track_predict, const TrackedObjectPtr& new_object, - const double time_diff) { + const double& time_diff) { // Compute distance for given trakc & object float location_dist = ComputeLocationDistance(track, track_predict, new_object, time_diff); @@ -100,11 +106,11 @@ float TrackObjectDistance::ComputeDistance(const ObjectTrackPtr& track, float histogram_dist = ComputeHistogramDistance(track, track_predict, new_object, time_diff); - float result_dist = s_weight_location_dist_ * location_dist + - s_weight_direction_dist_ * direction_dist + - s_weight_bbox_size_dist_ * bbox_size_dist + - s_weight_point_num_dist_ * point_num_dist + - s_weight_histogram_dist_ * histogram_dist; + float result_dist = s_location_distance_weight_ * location_dist + + s_direction_distance_weight_ * direction_dist + + s_bbox_size_distance_weight_ * bbox_size_dist + + s_point_num_distance_weight_ * point_num_dist + + s_histogram_distance_weight_ * histogram_dist; return result_dist; } @@ -112,7 +118,7 @@ float TrackObjectDistance::ComputeDistance(const ObjectTrackPtr& track, float TrackObjectDistance::ComputeLocationDistance(const ObjectTrackPtr& track, const Eigen::VectorXf& track_predict, const TrackedObjectPtr& new_object, - const double time_diff) { + const double& time_diff) { // Compute locatin distance for given track & object // range from 0 to positive infinity const TrackedObjectPtr& last_object = track->current_object_; @@ -149,7 +155,7 @@ float TrackObjectDistance::ComputeLocationDistance(const ObjectTrackPtr& track, float TrackObjectDistance::ComputeDirectionDistance(const ObjectTrackPtr& track, const Eigen::VectorXf& track_predict, const TrackedObjectPtr& new_object, - const double time_diff) { + const double& time_diff) { // Compute direction distance for given track & object // range from 0 to 2 const TrackedObjectPtr& last_object = track->current_object_; @@ -176,7 +182,7 @@ float TrackObjectDistance::ComputeDirectionDistance(const ObjectTrackPtr& track, float TrackObjectDistance::ComputeBboxSizeDistance(const ObjectTrackPtr& track, const Eigen::VectorXf& track_predict, const TrackedObjectPtr& new_object, - const double time_diff) { + const double& time_diff) { // Compute bbox size distance for given track & object // range from 0 to 1 const TrackedObjectPtr& last_object = track->current_object_; @@ -212,7 +218,7 @@ float TrackObjectDistance::ComputeBboxSizeDistance(const ObjectTrackPtr& track, float TrackObjectDistance::ComputePointNumDistance(const ObjectTrackPtr& track, const Eigen::VectorXf& track_predict, const TrackedObjectPtr& new_object, - const double time_diff) { + const double& time_diff) { // Compute point num distance for given track & object // range from 0 and 1 const TrackedObjectPtr& last_object = track->current_object_; @@ -229,7 +235,7 @@ float TrackObjectDistance::ComputePointNumDistance(const ObjectTrackPtr& track, float TrackObjectDistance::ComputeHistogramDistance(const ObjectTrackPtr& track, const Eigen::VectorXf& track_predict, const TrackedObjectPtr& new_object, - const double time_diff) { + const double& time_diff) { // Compute histogram distance for given track & object // range from 0 to 3 const TrackedObjectPtr& last_object = track->current_object_; diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/track_object_distance.h b/modules/perception/obstacle/lidar/tracker/hm_tracker/track_object_distance.h index 208ebe9686..c3a6a2d607 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/track_object_distance.h +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/track_object_distance.h @@ -31,44 +31,50 @@ class TrackObjectDistance { public: // @brief set weight of location dist for all the track object distance // objects - // @params[IN] weight_location_dist: weight of location dist + // @params[IN] location_distance_weight: weight of location dist // @return true if set successfully, otherwise return false - static bool SetWeightLocationDist(float weight_location_dist); + static bool SetLocationDistanceWeight( + const float& location_distance_weight); // @brief set weight of direction dist for all the track object distance // objects - // @params[IN] weight_direction_dist: weight of direction dist + // @params[IN] direction_distance_weight: weight of direction dist // @return true if set successfully, otherwise return false - static bool SetWeightDirectionDist(float weight_direction_dist); + static bool SetDirectionDistanceWeight( + const float& direction_distance_weight); // @brief set weight of bbox size dist for all the track object distance // objects - // @params[IN] weight_bbox_size_dist: weight of bbox size dist + // @params[IN] bbox_size_distance_weight: weight of bbox size dist // @return true if set successfully, otherwise return false - static bool SetWeightBboxSizeDist(float weight_bbox_size_dist); + static bool SetBboxSizeDistanceWeight( + const float& bbox_size_distance_weight); // @brief set weight of point num dist for all the track object distance // objects - // @params[IN] weight_point_num_dist: weight of point num dist + // @params[IN] point_num_distance_weight: weight of point num dist // @return true if set successfully, otherwise return false - static bool SetWeightPointNumDist(float weight_point_num_dist); + static bool SetPointNumDistanceWeight( + const float& point_num_distance_weight); // @brief set weight of histogram dist for all the track object distance // objects // @params[IN] weight_histogram_dist: weight of histogram dist // @return true if set successfully, otherwise return false - static bool SetWeightHistogramDist(float weight_histogram_dist); + static bool SetHistogramDistanceWeight( + const float& histogram_distance_weight); // @brief compute distance for given track & object - // @params[IN] track: track for computing distance - // @params[IN] track_predict: predicted state of track for computing distance - // @params[IN] new_object: new detected object for computing distance + // @params[IN] track: track for distance computing + // @params[IN] track_predict: predicted state of given track + // @params[IN] new_object: recently detected object // @params[IN] time_diff: time interval from last matching // @return nothing - static float ComputeDistance(const ObjectTrackPtr& track, - const Eigen::VectorXf& track_predict, - const TrackedObjectPtr& new_object, - const double time_diff); + static float ComputeDistance( + const ObjectTrackPtr& track, + const Eigen::VectorXf& track_predict, + const TrackedObjectPtr& new_object, + const double& time_diff); std::string Name() const { return "TrackObjectDistance"; @@ -76,67 +82,72 @@ class TrackObjectDistance { private: // @brief compute location distance for given track & object - // @params[IN] track: track for computing distance - // @params[IN] track_predict: predicted state of track for computing distance - // @params[IN] new_object: new detected object for computing distance + // @params[IN] track: track for distance computing + // @params[IN] track_predict: predicted state of given track + // @params[IN] new_object: recently detected object // @params[IN] time_diff: time interval from last matching - // @return nothing - static float ComputeLocationDistance(const ObjectTrackPtr& track, - const Eigen::VectorXf& track_predict, - const TrackedObjectPtr& new_object, - const double time_diff); + // @return location distacne of given + static float ComputeLocationDistance( + const ObjectTrackPtr& track, + const Eigen::VectorXf& track_predict, + const TrackedObjectPtr& new_object, + const double& time_diff); // @brief compute direction distance for given track & object - // @params[IN] track: track for computing distance - // @params[IN] track_predict: predicted state of track for computing distance - // @params[IN] new_object: new detected object for computing distance + // @params[IN] track: track for distance computing + // @params[IN] track_predict: predicted state of given track + // @params[IN] new_object: recently detected object // @params[IN] time_diff: time interval from last matching - // @return nothing - static float ComputeDirectionDistance(const ObjectTrackPtr& track, - const Eigen::VectorXf& track_predict, - const TrackedObjectPtr& new_object, - const double time_diff); + // @return direction distance of given + static float ComputeDirectionDistance( + const ObjectTrackPtr& track, + const Eigen::VectorXf& track_predict, + const TrackedObjectPtr& new_object, + const double& time_diff); // @brief compute bbox size distance for given track & object - // @params[IN] track: track for computing distance - // @params[IN] track_predict: predicted state of track for computing distance - // @params[IN] new_object: new detected object for computing distance + // @params[IN] track: track for distance computing + // @params[IN] track_predict: predicted state of given track + // @params[IN] new_object: recently detected object // @params[IN] time_diff: time interval from last matching - // @return nothing - static float ComputeBboxSizeDistance(const ObjectTrackPtr& track, - const Eigen::VectorXf& track_predict, - const TrackedObjectPtr& new_object, - const double time_diff); + // @return bbox size distance of given + static float ComputeBboxSizeDistance( + const ObjectTrackPtr& track, + const Eigen::VectorXf& track_predict, + const TrackedObjectPtr& new_object, + const double& time_diff); // @brief compute point num distance for given track & object - // @params[IN] track: track for computing distance - // @params[IN] track_predict: predicted state of track for computing distance - // @params[IN] new_object: new detected object for computing distance + // @params[IN] track: track for distance computing + // @params[IN] track_predict: predicted state of given track + // @params[IN] new_object: recently detected object // @params[IN] time_diff: time interval from last matching - // @return nothing - static float ComputePointNumDistance(const ObjectTrackPtr& track, - const Eigen::VectorXf& track_predict, - const TrackedObjectPtr& new_object, - const double time_diff); + // @return point num distance of given + static float ComputePointNumDistance( + const ObjectTrackPtr& track, + const Eigen::VectorXf& track_predict, + const TrackedObjectPtr& new_object, + const double& time_diff); // @brief compute histogram distance for given track & object - // @params[IN] track: track for computing distance - // @params[IN] track_predict: predicted state of track for computing distance - // @params[IN] new_object: new detected object for computing distance + // @params[IN] track: track for distance computing + // @params[IN] track_predict: predicted state of given track + // @params[IN] new_object: recently detected object // @params[IN] time_diff: time interval from last matching - // @return nothing - static float ComputeHistogramDistance(const ObjectTrackPtr& track, - const Eigen::VectorXf& track_predict, - const TrackedObjectPtr& new_object, - const double time_diff); + // @return histogram distance of given + static float ComputeHistogramDistance( + const ObjectTrackPtr& track, + const Eigen::VectorXf& track_predict, + const TrackedObjectPtr& new_object, + const double& time_diff); protected: // distance weights - static double s_weight_location_dist_; - static double s_weight_direction_dist_; - static double s_weight_bbox_size_dist_; - static double s_weight_point_num_dist_; - static double s_weight_histogram_dist_; + static double s_location_distance_weight_; + static double s_direction_distance_weight_; + static double s_bbox_size_distance_weight_; + static double s_point_num_distance_weight_; + static double s_histogram_distance_weight_; private: DISALLOW_COPY_AND_ASSIGN(TrackObjectDistance); diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/tracked_object.cc b/modules/perception/obstacle/lidar/tracker/hm_tracker/tracked_object.cc index 9aa5aa8f87..8574b8627d 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/tracked_object.cc +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/tracked_object.cc @@ -48,12 +48,8 @@ TrackedObject::TrackedObject(const TrackedObject& rhs) { type = rhs.type; barycenter = rhs.barycenter; anchor_point = rhs.anchor_point; - distance_to_ref = rhs.distance_to_ref; - angle_to_ref = rhs.angle_to_ref; velocity = rhs.velocity; acceleration = rhs.acceleration; - angle = rhs.angle; - angular_velocity = rhs.angular_velocity; type = rhs.type; association_score = rhs.association_score; } @@ -67,12 +63,8 @@ TrackedObject& TrackedObject::operator = (const TrackedObject& rhs) { type = rhs.type; barycenter = rhs.barycenter; anchor_point = rhs.anchor_point; - distance_to_ref = rhs.distance_to_ref; - angle_to_ref = rhs.angle_to_ref; velocity = rhs.velocity; acceleration = rhs.acceleration; - angle = rhs.angle; - angular_velocity = rhs.angular_velocity; type = rhs.type; association_score = rhs.association_score; return (*this); @@ -89,12 +81,8 @@ void TrackedObject::clone(const TrackedObject& rhs) { type = rhs.type; barycenter = rhs.barycenter; anchor_point = rhs.anchor_point; - distance_to_ref = rhs.distance_to_ref; - angle_to_ref = rhs.angle_to_ref; velocity = rhs.velocity; acceleration = rhs.acceleration; - angle = rhs.angle; - angular_velocity = rhs.angular_velocity; type = rhs.type; association_score = rhs.association_score; } diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/tracked_object.h b/modules/perception/obstacle/lidar/tracker/hm_tracker/tracked_object.h index bb692183b6..77c8eb9266 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/tracked_object.h +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/tracked_object.h @@ -32,7 +32,6 @@ struct TrackedObject { * states of tracked * object. Thus, update tracked object's state when you update the state of * track !!! */ - TrackedObject(); explicit TrackedObject(ObjectPtr obj_ptr); TrackedObject(const TrackedObject& rhs); @@ -47,7 +46,7 @@ struct TrackedObject { Eigen::Vector3f barycenter; - // oriented + // bbox Eigen::Vector3f center; Eigen::Vector3f size; Eigen::Vector3f direction; @@ -57,19 +56,13 @@ struct TrackedObject { Eigen::Vector3f anchor_point; Eigen::Vector3f velocity; Eigen::Vector3f acceleration; - float angle = 0.0f; - float angular_velocity = 0.0f; // class type ObjectType type; // association distance - // range from 0 to max_match_distance + // range from 0 to association_score_maximum float association_score = 0.0f; - - // relative polar coordinate of barycenter point to the main car - float distance_to_ref = 0.0f; - float angle_to_ref = 0.0f; }; // struct TrackedObject typedef std::shared_ptr TrackedObjectPtr; -- GitLab