Skip to content

A
apollo

Pinoxchio / apollo
与 Fork 源项目一致

从无法访问的项目Fork

通知 2
Star 0
Fork 0
A
apollo

体验新版 GitCode，发现更多精彩内容 >>

提交 5a7d03e9 编写于 作者: K kimifly06 提交者： weidezhang
浏览文件
操作

Clean hm tracker dev (#1496) 

* clean hm tracker IV

* fix code style

* clean hm tracker V

* fixed a bug
上级 dc1c4962
展开全部 隐藏空白更改
内联 并排
Showing with 736 addition and 616 deletion
modules/perception/model/tracker.config
浏览文件 @ 5a7d03e9
......@@ -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
}
}
modules/perception/obstacle/lidar/tracker/hm_tracker/base_filter.h
浏览文件 @ 5a7d03e9
......@@ -18,6 +18,7 @@
#define MODULES_PERCEPTION_OBSTACLE_LIDAR_TRACKER_HM_TRACKER_BASE_FILTER_H_
#include <string>
#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
......
modules/perception/obstacle/lidar/tracker/hm_tracker/base_matcher.h
浏览文件 @ 5a7d03e9
......@@ -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 <track, object>
// @params[OUT] unassigned_tracks: unmatched tracks
// @params[OUT] unassigned_objects: unmatched objects
// @return nothing
......@@ -53,7 +53,7 @@ class BaseMatcher {
std::vector<TrackedObjectPtr>* objects,
const std::vector<ObjectTrackPtr>& tracks,
const std::vector<Eigen::VectorXf>& tracks_predict,
const double time_diff,
const double& time_diff,
std::vector<TrackObjectPair>* assignments,
std::vector<int>* unassigned_tracks,
std::vector<int>* unassigned_objects) = 0;
......
modules/perception/obstacle/lidar/tracker/hm_tracker/feature_descriptor.h
浏览文件 @ 5a7d03e9
......@@ -19,6 +19,7 @@
#include <algorithm>
#include <vector>
#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(
......
modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.h
浏览文件 @ 5a7d03e9
......@@ -20,6 +20,7 @@
#include <string>
#include <utility>
#include <vector>
#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<ObjectPtr>& 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<ObjectPtr>& objects,
double timestamp,
const double& timestamp,
const TrackerOptions& options,
std::vector<ObjectPtr>* 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<ObjectPtr>& 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<Eigen::VectorXf>* 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 <track, object>
// @params[IN] time_diff: time interval for updating
// @return nothing
void UpdateAssignedTracks(
std::vector<Eigen::VectorXf>* tracks_predict,
std::vector<TrackedObjectPtr>* new_objects,
const std::vector<TrackObjectPair>& 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<Eigen::VectorXf>& tracks_predict,
const std::vector<int>& 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<TrackedObjectPtr>& new_objects,
const std::vector<int>& 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<ObjectPtr>* 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<TrackedObjectPtr>& objects);
private:
// algorithm setup
MatcherType matcher_method_;
......
modules/perception/obstacle/lidar/tracker/hm_tracker/hungarian_matcher.cc
浏览文件 @ 5a7d03e9
......@@ -16,6 +16,7 @@
#include <utility>
#include <vector>
#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<TrackedObjectPtr>* objects,
const std::vector<ObjectTrackPtr>& tracks,
const std::vector<Eigen::VectorXf>& tracks_predict,
const double time_diff,
const double& time_diff,
std::vector<TrackObjectPair>* assignments,
std::vector<int>* unassigned_tracks,
std::vector<int>* 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<TrackedObjectPtr>* objects,
// B. computing connected components
std::vector<std::vector<int> > object_components;
std::vector<std::vector<int> > 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<TrackedObjectPtr>* objects,
}
}
void HungarianMatcher::MatchComponents(const Eigen::MatrixXf association_mat,
const std::vector<int> track_component,
const std::vector<int> object_component,
void HungarianMatcher::MatchComponents(const Eigen::MatrixXf& association_mat,
const std::vector<int>& track_component,
const std::vector<int>& object_component,
std::vector<TrackObjectPair>* sub_assignments,
std::vector<int>* sub_unassigned_tracks,
std::vector<int>* 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<ObjectTrackPtr>& tracks,
const std::vector<Eigen::VectorXf>& tracks_predict,
const std::vector<TrackedObjectPtr>& 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<std::vector<int> >* track_components,
std::vector<std::vector<int> >* 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<TrackObjectPair>* assignments,
std::vector<int>* unassigned_tracks,
std::vector<int>* 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;
}
......
modules/perception/obstacle/lidar/tracker/hm_tracker/hungarian_matcher.h
浏览文件 @ 5a7d03e9
......@@ -19,6 +19,7 @@
#include <string>
#include <vector>
#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<TrackedObjectPtr>* objects,
const std::vector<ObjectTrackPtr>& tracks,
const std::vector<Eigen::VectorXf>& tracks_predict,
const double time_diff,
const double& time_diff,
std::vector<TrackObjectPair>* assignments,
std::vector<int>* unassigned_tracks,
std::vector<int>* 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<int> track_component,
const std::vector<int> obj_component,
const Eigen::MatrixXf& association_mat,
const std::vector<int>& track_component,
const std::vector<int>& obj_component,
std::vector<TrackObjectPair>* sub_assignments,
std::vector<int>* sub_unassigned_tracks,
std::vector<int>* 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<ObjectTrackPtr>& tracks,
const std::vector<Eigen::VectorXf>& tracks_predict,
const std::vector<TrackedObjectPtr>& 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<std::vector<int> >* track_components,
std::vector<std::vector<int> >* 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<TrackObjectPair>* assignments,
std::vector<int>* unassigned_tracks,
std::vector<int>* 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
......
modules/perception/obstacle/lidar/tracker/hm_tracker/kalman_filter.cc
浏览文件 @ 5a7d03e9
......@@ -15,6 +15,7 @@
*****************************************************************************/
#include <algorithm>
#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<float>();
}
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<double>();
Eigen::Vector3d old_size = old_object->size.cast<double>();
......@@ -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;
......
modules/perception/obstacle/lidar/tracker/hm_tracker/kalman_filter.h
浏览文件 @ 5a7d03e9
......@@ -18,6 +18,7 @@
#define MODULES_PERCEPTION_OBSTACLE_LIDAR_TRACKER_HM_TRACKER_KALMAN_FILTER_H_
#include <vector>
#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
......
modules/perception/obstacle/lidar/tracker/hm_tracker/object_track.cc
浏览文件 @ 5a7d03e9
......@@ -17,6 +17,7 @@
#include <algorithm>
#include <string>
#include <vector>
#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<float>();
}
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;
......
modules/perception/obstacle/lidar/tracker/hm_tracker/object_track.h
浏览文件 @ 5a7d03e9
......@@ -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<ObjectTrackPtr>& GetTracks() {
return tracks_;
}
// @brief get maintained tracks
// @return maintained tracks
inline const std::vector<ObjectTrackPtr>& 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<ObjectTrackPtr> tracks_;
int age_threshold_;
double minimum_visible_ratio_;
}; // class ObjectTrackSet
} // namespace perception
......
modules/perception/obstacle/lidar/tracker/hm_tracker/track_object_distance.cc
浏览文件 @ 5a7d03e9
......@@ -16,6 +16,7 @@
#include <algorithm>
#include <vector>
#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_;
......
modules/perception/obstacle/lidar/tracker/hm_tracker/track_object_distance.h
浏览文件 @ 5a7d03e9
......@@ -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 <track, object> 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 <track, object> 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 <track, object>
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 <track, object> 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 <track, object>
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 <track, object> 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 <track, object>
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 <track, object> 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 <track, object>
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 <track, object> 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 <track, object>
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);
......
modules/perception/obstacle/lidar/tracker/hm_tracker/tracked_object.cc
浏览文件 @ 5a7d03e9
......@@ -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;
}
......
modules/perception/obstacle/lidar/tracker/hm_tracker/tracked_object.h
浏览文件 @ 5a7d03e9
......@@ -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<TrackedObject> TrackedObjectPtr;
......
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或