diff --git a/modules/perception/data/hm_tracker_test/.gitattributes b/modules/perception/data/hm_tracker_test/.gitattributes deleted file mode 100644 index 96b3547586f01fcd6086d2dbf99eeadf4e20dad9..0000000000000000000000000000000000000000 --- a/modules/perception/data/hm_tracker_test/.gitattributes +++ /dev/null @@ -1 +0,0 @@ -*.pcd filter=lfs diff=lfs merge=lfs -text \ No newline at end of file diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_2.pcd b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_2.pcd deleted file mode 100644 index 67ab5277c7df52631dd8f2feaddf8f25aea4189d..0000000000000000000000000000000000000000 Binary files a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_2.pcd and /dev/null differ diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_2.pose b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_2.pose index 22b6dd1a29e06d51a673edb87924beb95bda5d8a..019dcfd0ab76788adb1eb4e16685a66bcca439e0 100644 --- a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_2.pose +++ b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_2.pose @@ -1 +1 @@ -11989 1160300588.419051 428033.330463 4435145.010161 40.746964 -0.002165 -0.010956 -0.639926 0.768355 +11989 588.419051 33.330463 45.010161 40.746964 -0.002165 -0.010956 -0.639926 0.768355 diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_3.pcd b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_3.pcd deleted file mode 100644 index 20d807c5994678b118eb203e0f1bdb00b8ffec12..0000000000000000000000000000000000000000 Binary files a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_3.pcd and /dev/null differ diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_3.pose b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_3.pose index 484df8bff18b654eddbfad9d50c855be1dc0d808..1a02c944274cad3a1f60e29af88b3f9b60f42bd7 100644 --- a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_3.pose +++ b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_3.pose @@ -1 +1 @@ -11990 1160300588.519102 428033.330382 4435145.010216 40.747129 -0.002152 -0.010966 -0.639940 0.768343 +11990 588.519102 33.330382 45.010216 40.747129 -0.002152 -0.010966 -0.639940 0.768343 diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_4.pcd b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_4.pcd deleted file mode 100644 index dc5b19f999d85804f6f58a29f600513771172161..0000000000000000000000000000000000000000 Binary files a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_4.pcd and /dev/null differ diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_4.pose b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_4.pose index 83a5402e48891236042afc744156e2edccf7ba69..5cae17767aaf3cebded90284a46adc3c5ca0362c 100644 --- a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_4.pose +++ b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_4.pose @@ -1 +1 @@ -11991 1160300588.619153 428033.330347 4435145.010282 40.747194 -0.002133 -0.010945 -0.639944 0.768341 +11991 588.619153 33.330347 45.010282 40.747194 -0.002133 -0.010945 -0.639944 0.768341 diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_5.pcd b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_5.pcd deleted file mode 100644 index 41d1d104f20c0d29015bd98e0a52aaae976faa20..0000000000000000000000000000000000000000 Binary files a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_5.pcd and /dev/null differ diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_5.pose b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_5.pose index 8791c84884e1d2386d0cfdc148dc4717256cecdf..1de3b81eaa81cff25218381fa27dd7feb137b3a6 100644 --- a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_5.pose +++ b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_5.pose @@ -1 +1 @@ -11992 1160300588.719204 428033.330361 4435145.010357 40.747331 -0.002134 -0.010937 -0.639935 0.768348 +11992 588.719204 33.330361 45.010357 40.747331 -0.002134 -0.010937 -0.639935 0.768348 diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_6.pcd b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_6.pcd deleted file mode 100644 index 8b8d3b21745964004fc7f9aeb484fc4d119b1484..0000000000000000000000000000000000000000 Binary files a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_6.pcd and /dev/null differ diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_6.pose b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_6.pose index 70d1e8cd4811efb88ce149e3da5a18b16d2b3dac..dc96baaf52a4c613d01f977d45c3e148755401df 100644 --- a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_6.pose +++ b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_6.pose @@ -1 +1 @@ -11993 1160300588.819255 428033.330328 4435145.010420 40.747546 -0.002154 -0.010940 -0.639938 0.768346 +11993 588.819255 33.330328 45.010420 40.747546 -0.002154 -0.010940 -0.639938 0.768346 diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_7.pcd b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_7.pcd deleted file mode 100644 index c68e5212d47a4af086961ca681debaff7f451705..0000000000000000000000000000000000000000 Binary files a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_7.pcd and /dev/null differ diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_7.pose b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_7.pose index 92e1a7a5cc82fc7bd9e7620a34e25199594fabdc..c7b91c4a306b95d63a5475985e2537f1a8e042e7 100644 --- a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_7.pose +++ b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_7.pose @@ -1 +1 @@ -11994 1160300588.919307 428033.330290 4435145.010480 40.747756 -0.002180 -0.010939 -0.639941 0.768343 +11994 588.919307 33.330290 45.010480 40.747756 -0.002180 -0.010939 -0.639941 0.768343 diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_8.pcd b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_8.pcd deleted file mode 100644 index 67fbe74afe9d3bdca1a1958c47f542f5fc6ea68d..0000000000000000000000000000000000000000 Binary files a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_8.pcd and /dev/null differ diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_8.pose b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_8.pose index a5b5702c7497311781c78a3b6cd110dd6b22be48..bfdd8e442dfff07f78a3a5e5a139fc8098afdd8f 100644 --- a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_8.pose +++ b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_8.pose @@ -1 +1 @@ -11995 1160300589.019364 428033.330300 4435145.010551 40.747892 -0.002173 -0.010936 -0.639935 0.768348 +11995 589.019364 33.330300 45.010551 40.747892 -0.002173 -0.010936 -0.639935 0.768348 diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_9.pcd b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_9.pcd deleted file mode 100644 index 48e1ab426b75081a023a87ef7527dc29fb9bc181..0000000000000000000000000000000000000000 Binary files a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_9.pcd and /dev/null differ diff --git a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_9.pose b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_9.pose index e983b77a16724a7f08a3c6142294619f4c38dc06..e06bc61ef7d7dae924ff13475a86a47ecfac3928 100644 --- a/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_9.pose +++ b/modules/perception/data/hm_tracker_test/QN68P2_12_1476265365_1476265665_9.pose @@ -1 +1 @@ -11996 1160300589.119415 428033.330241 4435145.010605 40.748109 -0.002156 -0.010970 -0.639944 0.768340 +11996 589.119415 33.330241 45.010605 40.748109 -0.002156 -0.010970 -0.639944 0.768340 diff --git a/modules/perception/obstacle/common/file_system_util_test.cc b/modules/perception/obstacle/common/file_system_util_test.cc index c9e572891264c5a7a09eb69376010f1847eb77e6..fa06c08cbedd8b2cada29c1404e675cae4a23225 100644 --- a/modules/perception/obstacle/common/file_system_util_test.cc +++ b/modules/perception/obstacle/common/file_system_util_test.cc @@ -24,15 +24,12 @@ namespace perception { TEST(FileSystemUtilTest, GetFileNamesInFolderById) { std::string data_path = "modules/perception/data/hm_tracker_test/"; - std::vector pcd_filenames; - GetFileNamesInFolderById(data_path, ".pcd", &pcd_filenames); std::vector seg_filenames; GetFileNamesInFolderById(data_path, ".seg", &seg_filenames); std::vector pose_filenames; GetFileNamesInFolderById(data_path, ".pose", &pose_filenames); - EXPECT_GT(pcd_filenames.size(), 0); - EXPECT_EQ(pcd_filenames.size(), seg_filenames.size()); - EXPECT_EQ(pcd_filenames.size(), pose_filenames.size()); + EXPECT_EQ(seg_filenames.size(), 8); + EXPECT_EQ(pose_filenames.size(), 8); } } // namespace perception diff --git a/modules/perception/obstacle/common/pose_util_test.cc b/modules/perception/obstacle/common/pose_util_test.cc index 034ab3829d0adf37bff12d31a420d670d93a3c79..8e2ba1489c10b5930d655850e85bc7c37d478e2e 100644 --- a/modules/perception/obstacle/common/pose_util_test.cc +++ b/modules/perception/obstacle/common/pose_util_test.cc @@ -39,9 +39,9 @@ TEST_F(PoseUtilTest, ReadPoseFile) { double time_stamp; EXPECT_TRUE(ReadPoseFile(test_file, &pose, &frame_id, &time_stamp)); EXPECT_EQ(frame_id, 11989); - EXPECT_EQ(time_stamp, 1160300588.419051); - EXPECT_EQ(pose(0, 3), 428033.330463); - EXPECT_EQ(pose(1, 3), 4435145.010161); + EXPECT_EQ(time_stamp, 588.419051); + EXPECT_EQ(pose(0, 3), 33.330463); + EXPECT_EQ(pose(1, 3), 45.010161); EXPECT_EQ(pose(2, 3), 40.746964); } 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 b4dfcf86e2bd29a456174ed6c6554da7ba1afa6b..247aabd2a46ae91ad40a7052e797277af4f37bff 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.cc +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.cc @@ -33,7 +33,7 @@ HmObjectTracker::HmObjectTracker(): matcher_method_(HUNGARIAN_MATCHER), filter_method_(KALMAN_FILTER), use_histogram_for_match_(false), histogram_bin_size_(10), - matcher_(NULL), + matcher_(nullptr), time_stamp_(0.0), valid_(false) { } @@ -41,7 +41,7 @@ HmObjectTracker::HmObjectTracker(): matcher_method_(HUNGARIAN_MATCHER), HmObjectTracker::~HmObjectTracker() { if (matcher_) { delete matcher_; - matcher_ = NULL; + matcher_ = nullptr; } } @@ -50,7 +50,7 @@ bool HmObjectTracker::Init() { using apollo::perception::ConfigManager; using apollo::perception::ModelConfig; - const ModelConfig* model_config = NULL; + const ModelConfig* model_config = nullptr; if (!ConfigManager::instance()->GetModelConfig(name(), &model_config)) { AERROR << "not found model config: " << name(); return false; @@ -371,14 +371,14 @@ bool HmObjectTracker::Track(const std::vector& objects, const TrackerOptions& options, std::vector* tracked_objects) { // A. track setup - if (tracked_objects == NULL) + if (tracked_objects == nullptr) return false; if (!valid_) { valid_ = true; return Initialize(objects, timestamp, options, tracked_objects); } Eigen::Matrix4d velo2world_pose = Eigen::Matrix4d::Identity(); - if (options.velodyne_trans != NULL) { + if (options.velodyne_trans != nullptr) { velo2world_pose = *(options.velodyne_trans); } else { AERROR << "Input velodyne_trans is null"; @@ -419,7 +419,7 @@ bool HmObjectTracker::Track(const std::vector& objects, UpdateUnassignedTracks(tracks_predict, unassigned_tracks, time_diff); DeleteLostTracks(); // E.3 create new tracks for objects without associated tracks - CreateNewTracks(transformed_objects, unassigned_objects, time_diff); + CreateNewTracks(transformed_objects, unassigned_objects); // F. collect tracked results CollectTrackedResults(tracked_objects); @@ -432,7 +432,7 @@ bool HmObjectTracker::Initialize(const std::vector& objects, std::vector* tracked_objects) { // A. track setup Eigen::Matrix4d velo2world_pose = Eigen::Matrix4d::Identity(); - if (options.velodyne_trans != NULL) { + if (options.velodyne_trans != nullptr) { velo2world_pose = *(options.velodyne_trans); } else { AERROR << "Input velodyne_trans is null"; @@ -454,11 +454,9 @@ bool HmObjectTracker::Initialize(const std::vector& objects, // C. create tracks std::vector unassigned_objects; unassigned_objects.resize(transformed_objects.size()); - for (size_t i = 0; i < transformed_objects.size(); i++) { - unassigned_objects[i] = i; - } + std::iota(unassigned_objects.begin(), unassigned_objects.end(), 0); double time_diff = 0.1; - CreateNewTracks(transformed_objects, unassigned_objects, time_diff); + CreateNewTracks(transformed_objects, unassigned_objects); time_stamp_ = timestamp; // D. collect tracked results @@ -588,8 +586,7 @@ void HmObjectTracker::UpdateUnassignedTracks( void HmObjectTracker::CreateNewTracks( const std::vector& new_objects, - const std::vector& unassigned_objects, - const double& time_diff) { + const std::vector& unassigned_objects) { // 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 4aae8d41cbe668b778f4eb76ed7f6ec39532fe7e..9bdbb2c7eef07c60536343f9eff2fe5d448c17f0 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.h +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.h @@ -176,12 +176,10 @@ class HmObjectTracker : public BaseTracker{ // @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 std::vector& unassigned_objects); // @brief delete lost tracks // @return nothing diff --git a/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker_test.cc b/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker_test.cc index 96f337be90ab7e1c80707ebbd8cb68e83719a661..555a523ddfb3f544d622f04a22b03c049fb4f85f 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker_test.cc +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker_test.cc @@ -16,9 +16,6 @@ #include "modules/perception/obstacle/lidar/tracker/hm_tracker/hm_tracker.h" -#include -#include -#include #include #include #include @@ -57,9 +54,9 @@ class HmObjectTrackerTest : public testing::Test { } void TearDown() { delete hm_tracker_; - hm_tracker_ = NULL; + hm_tracker_ = nullptr; delete object_builder_; - object_builder_ = NULL; + object_builder_ = nullptr; } protected: @@ -92,47 +89,21 @@ bool ConstructObjects(const std::string& filename, return true; } -bool LoadPclPcds(const std::string filename, - pcl_util::PointCloudPtr* cloud_out) { - pcl::PointCloud org_cloud; - if (pcl::io::loadPCDFile(filename, org_cloud) < 0) { - AERROR << "failed to load pcd file: " << filename; - return false; - } - (*cloud_out)->points.reserve(org_cloud.points.size()); - for (size_t i = 0; i < org_cloud.points.size(); ++i) { - pcl_util::Point pt; - pt.x = org_cloud.points[i].x; - pt.y = org_cloud.points[i].y; - pt.z = org_cloud.points[i].z; - pt.intensity = org_cloud.points[i].intensity; - if (isnan(org_cloud.points[i].x)) continue; - (*cloud_out)->push_back(pt); - } - return true; -} - TEST_F(HmObjectTrackerTest, Track) { // test initialization of hm tracker EXPECT_TRUE(hm_tracker_->Init()); // test tracking via hm tracker std::string data_path = "modules/perception/data/hm_tracker_test/"; - std::vector pcd_filenames; - GetFileNamesInFolderById(data_path, ".pcd", &pcd_filenames); std::vector seg_filenames; GetFileNamesInFolderById(data_path, ".seg", &seg_filenames); std::vector pose_filenames; GetFileNamesInFolderById(data_path, ".pose", &pose_filenames); int frame_id = -1; double time_stamp = 0.0; - EXPECT_GT(pcd_filenames.size(), 0); - EXPECT_EQ(pcd_filenames.size(), seg_filenames.size()); - EXPECT_EQ(pcd_filenames.size(), pose_filenames.size()); + EXPECT_GT(seg_filenames.size(), 0); + EXPECT_EQ(seg_filenames.size(), pose_filenames.size()); Eigen::Vector3d global_offset(0, 0, 0); for (size_t i = 0; i < seg_filenames.size(); ++i) { - // load frame clouds - pcl_util::PointCloudPtr cloud(new pcl_util::PointCloud); - EXPECT_TRUE(LoadPclPcds(data_path + pcd_filenames[i], &cloud)); // read pose Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(); if (!ReadPoseFile(data_path + pose_filenames[i], &pose, &frame_id, 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 eefedc777bede42f93517ca5af0e92605e4025ed..7ab63f0e98e0893322afae03ee4e78a10497b050 100644 --- a/modules/perception/obstacle/lidar/tracker/hm_tracker/object_track.cc +++ b/modules/perception/obstacle/lidar/tracker/hm_tracker/object_track.cc @@ -129,7 +129,7 @@ ObjectTrack::ObjectTrack(TrackedObjectPtr obj) { ObjectTrack::~ObjectTrack() { if (filter_) { delete filter_; - filter_ = NULL; + filter_ = nullptr; } } @@ -415,7 +415,7 @@ void ObjectTrackSet::Clear() { for (size_t i = 0; i < tracks_.size(); i++) { if (tracks_[i]) { delete (tracks_[i]); - tracks_[i] = NULL; + tracks_[i] = nullptr; } } tracks_.clear(); @@ -448,9 +448,9 @@ int ObjectTrackSet::RemoveLostTracks() { // remove lost tracks int no_removed = tracks_.size() - track_num; for (size_t i = track_num; i < tracks_.size(); ++i) { - if (tracks_[i] != NULL) { + if (tracks_[i] != nullptr) { delete (tracks_[i]); - tracks_[i] = NULL; + tracks_[i] = nullptr; } } tracks_.resize(track_num);