Skip to content

Commit

Permalink
feat: add multi_object_tracker package (autowarefoundation#74)
Browse files Browse the repository at this point in the history
* release v0.4.0

* remove ROS1 packages temporarily

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Revert "remove ROS1 packages temporarily"

This reverts commit 4acc04bb2a3b2ef21e687a8d3ba88008fa32846b.

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add COLCON_IGNORE to ros1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Rename launch files to launch.xml (autowarefoundation#28)

* ROS2 Porting: multi_object_tracker (autowarefoundation#24)

* First pass at node (exclude tracker library)
 - Rename files to match existing packages
   - node -> _core
   - main -> _node

 - Add publishers and subscribers
 - Add TF and logging
 - issue with lookupTransform from tf2::BufferCore doesn't allow for duration specification in foxy
bfe938b

* Add the base implementation of the helper class and files
 - Depends on the unique_id package which has not been release rosdep yet
 - Point to the correct packages
 - Add vehicle_tracker class
 - Missing uuid implementation for the object id creation
 - Convert bicycle_tracker class
 - Convert pedestrian_tracker to ROS2
 - Add successive_shortest_path library
   - Changed header file extension to match existing files
 - Convert data_association to ROS2
   - Return type change to resolve -Wreturn-type warning

* Add data associator and tracker method calls back into the MultiObjectTracker implementation

* Clean up
 - Align headers
 - Fix typo
 - Add back the transform with no duration;  see issue
 - Remove comments

* Clean up package.xml and CMakelists

* Clean up header order

* Remove main file

* Fix issues after rebase
 - Add wait for transform to add duration
 - Add UUID generation
 - Add -Werror

* Address PR comment:
 - Reintroduce getUUID method in tracker code

* Fix the waitForTransform implementation
 - Use the synchronous approach to getting the transform using wait_for()

* Address PR comments:
 - Add further comments in CMakelist explaining Eigen library deps
 - Use durable transient_local QoS for subscriber
 - Remove second explicit string type decl
 - Convert buffer and listener to plain objects, move initialization to initialization list
 - Make generation of random bits more idiomatic
 - Use indepedent_bit_engine instead of uniform distribution

* Address PR comments:
 - Fix eigen cmake dep

* Fix Eigen3 deps
 - find cmake module

* fix duration unit for RCLCPP_*_THROTTLE (autowarefoundation#75)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143)

* Use quotes for includes where appropriate (autowarefoundation#144)

* Use quotes for includes where appropriate

* Fix lint tests

* Make tests pass hopefully

* Run uncrustify on the entire Pilot.Auto codebase (autowarefoundation#151)

* Run uncrustify on the entire Pilot.Auto codebase

* Exclude open PRs

* adding linters to multi_object_tracker (autowarefoundation#177)

* [multi_object_tracker] fix bug in calculating timer period (autowarefoundation#198)

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Replace waitForTransform with lookupTransform in multi_object_tracker (autowarefoundation#213)

* Ros2 v0.8.0 multi object tracker (autowarefoundation#259)

* restore file name for v0.8.0 update

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix typos in perception (autowarefoundation#862)

* Feature/camera lidar perception (autowarefoundation#937)

* add object splitter

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* add object merger

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* change pkg name

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* cosmetic change

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* add comment

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* remove litter

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* bug fix : debug code

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* enable vehicle to unknown track

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* bug fix

* add object position in clustering

* 🚮

* change param

* fix name

* bug fix

* add install

* add delay compensation param (autowarefoundation#1035)

* fix tracking bug and change tracking param (autowarefoundation#1036)

* fix bug

* cut stop noise velocity

* update param

* support unknown labeled object tracking (autowarefoundation#1017)

* add data association matrix param

* fix typo

* apply clang-format-6.0

* set pedestrian model when label is unknown

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* Add missing install of config directory (autowarefoundation#1045)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Revert "restore file name for v0.8.0 update"

This reverts commit 5fdf5b179f2395f01672c976920da1f8de8cc33a.

* Fix typo

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix parameter type

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix missing arg

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* fix min_area (autowarefoundation#347)

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* Rename ROS-related .yaml to .param.yaml (autowarefoundation#352)

* Rename ROS-related .yaml to .param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove prefix 'default_' of yaml files

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename vehicle_info.yaml to vehicle_info.param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Rename diagnostic_aggregator's param files

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix overlooked parameters

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Feature/ekf tracker (autowarefoundation#1158) (autowarefoundation#381)

* Feature/ekf tracker (autowarefoundation#1158)

* change to ekf tracker

* visualize covariance

* cosmetic change

* cosmetic change

* change param

* add multi model ekf tracker (autowarefoundation#1165)

* add multi model ekf tracker

* cosmetic change

* cosmetic change

* add iou filter

* change correct license

* change correct license

* cosmetic change

* bug fix

* cosmetic change

* check clockwise

* change param

* bug fix

* cosmetic change

* add comment

* add enum

* cosmetic change

* cosmetic change

* apply format

* apply format

* fix config name

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* [multi_object_tracker] apply ament_uncrustify

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* [multi_object_tracker] fix lint errors

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix include brackets

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* fix duration (autowarefoundation#445)

* fix duration

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* change to from_seconds

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* fix other duration

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* replace -1 with 0

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* apply ament_lint_common

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* uncrustify

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* add space

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* add another space

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* Fix typo in perception module (autowarefoundation#440)

* add use_sim-time option (autowarefoundation#454)

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix calculation of corners (autowarefoundation#1271) (autowarefoundation#1273)

* fix calculation of corners

* reduce calc cost

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* Porting small fix (autowarefoundation#1288)

* Delete unused code (autowarefoundation#1183)

* Fix control topic name of closest_velocity_checker.py (autowarefoundation#1174)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add comments for livox tag (autowarefoundation#1188)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Clear return value (autowarefoundation#1193)

* Change tracker model of unknown object (autowarefoundation#1204)

* treat polygon points as relative (autowarefoundation#1205)

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* hotfix: reference velocity in consideration of vehicle gear (autowarefoundation#1213)

* fix reference velocity for vehicle gear

* add initialization

* revert

* add comment

* change max area param (autowarefoundation#1218)

* Fix an identical code for different branches (autowarefoundation#1230)

* Update livox_tag_filter.launch.xml

* Fixup

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: shin <8327162+0x126@users.noreply.github.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix bug (change noise in measurement update) (autowarefoundation#1313)

* fix bug (change noise in measurement update)

* Uncrustify

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* Feature/porting mussp with vendor (autowarefoundation#1323)

* change to mussp (autowarefoundation#1180)

* change to mussp

* add license

* add license

* change to solver plugin

* fix bug

* cosmetic change

* modified to follow ROS2 coding style

* Use mussp vendor

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix dependencies in package.xml

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Add mussp_vendor into build_depends.repos

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix typo

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix typo

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Use fork instead of vendor package

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Cleanup CMakeLists.txt

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Remove comment

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Sort package dependencies in alphabetical order

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Keisuke Shima <keisuke.shima@tier4.jp>

* Porting unknown tracker (autowarefoundation#1292)

* add unknown tracker (autowarefoundation#1211)

* add unknown tracker

* bug fix

* fix typo

* fix for test

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* Fix/tracking ros2 (autowarefoundation#1478)

* cosmetic change and fix multi model tracker update bug (autowarefoundation#1335)

* Fix lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix typo

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>

* Add missing eigen macro (autowarefoundation#1336) (autowarefoundation#1477)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Fix parameter file for mot (autowarefoundation#1489)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Add pre-commit (autowarefoundation#1560)

* add pre-commit

* add pre-commit-config

* add additional settings for private repository

* use default pre-commit-config

* update pre-commit setting

* Ignore whitespace for line breaks in markdown

* Update .github/workflows/pre-commit.yml

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>

* exclude svg

* remove pretty-format-json

* add double-quote-string-fixer

* consider COLCON_IGNORE file when seaching modified package

* format file

* pre-commit fixes

* Update pre-commit.yml

* Update .pre-commit-config.yaml

Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: pre-commit <pre-commit@example.com>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>

* Add markdownlint and prettier (autowarefoundation#1661)

* Add markdownlint and prettier

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore .param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* suppress warnings for declare parameters (autowarefoundation#1724)

* fix for lanelet2_extension

* fix for traffic light ssd fine detector

* fix for topic_state_monitor

* fix for dummy diag publisher

* fix for remote cmd converter

* fix for vehicle_info_util

* fix for multi object tracker

* fix for freespace planner

* fix for autoware_error_monitor

* add Werror for multi object tracker

* fix for multi object tracker

* add Werror for liraffic light ssd fine detector

* add Werror for topic state monitor

* add Werror

* add Werror

* add Werror

* add Werror

* fix style

* Fix -Wunused-parameter (autowarefoundation#1836)

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix mistake

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix spell

* Fix lint issues

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore flake8 warnings

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>

* Fix clang warnings (autowarefoundation#1859)

* Fix -Wreturn-std-move

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix -Wunused-private-field

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore -Wnonportable-include-path for mussp

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix -Wunused-const-variable

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix "can not be used when making a shared object"

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Feature/porting modify tracker life cycle (autowarefoundation#1675)

* Modify tracker life cycle (autowarefoundation#1462)

* add z lpf

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* modify life cycle

Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>

* refactor

* apply format

* fix typo

* add read me

* fix typo

* change readme

* modify readme

* fix typo

* cosmetic change

* cosmetic change

* refactor and change removal conditions

* cosmetic change

* add note

* bug fix

* change specific alive pattern

* format code

Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Apply lint

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* pre-commit fixes

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* fix bug

Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>

* Detection by tracker (autowarefoundation#1910)

* initial commit

* backup

* apply format

* cosmetic change

* implement divided under segmenterd clusters

* cosmetic change

* bug fix

* bug fix

* bug fix

* modify launch

* add debug and bug fix

* bug fix

* bug fix

* add no found tracked object

* modify parameters and cmake

* bug fix

* remove debug info

* add readme

* modify clustering launch

* run pre-commit

* cosmetic change

* cosmetic change

* cosmetic change

* apply markdownlint

* modify launch

* modify for cpplint

* modify qos

* change int to size_T

* bug fix

* change perception qos

* Update perception/object_recognition/detection/detection_by_tracker/package.xml

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* cosmetic change

* cosmetic change

* fix launch

* Update perception/object_recognition/detection/detection_by_tracker/src/utils.cpp

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* modify header include order

* change include order

* Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* change to std::optional

* cosmetic change

* Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* bug fix

* modify readme

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>

* Change formatter to clang-format and black (autowarefoundation#2332)

* Revert "Temporarily comment out pre-commit hooks"

This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3.

* Replace ament_lint_common with autoware_lint_common

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply Black

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add COLCON_IGNORE (autowarefoundation#500)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Port multi object tracker for autoware_auto_msgs (autowarefoundation#526)

* backup

* backup

* backup

* add todo author

* apply pre-commit

* remove COLCON_IGNORE

* fix: fix README.md

* ci(pre-commit): autofix

* Apply suggestions from code review

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
Co-authored-by: Jilada Eccleston <jilada.eccleston@gmail.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: nik-tier4 <71747268+nik-tier4@users.noreply.github.com>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: Kazuki Miyahara <kmiya@outlook.com>
Co-authored-by: Yukihiro Saito <yukky.saito@gmail.com>
Co-authored-by: Taichi Higashide <taichi.higashide@tier4.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Yusuke Muramatsu <yukke42@users.noreply.github.com>
Co-authored-by: Keisuke Shima <keisuke.shima@tier4.jp>
Co-authored-by: shin <8327162+0x126@users.noreply.github.com>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: pre-commit <pre-commit@example.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
23 people committed Dec 5, 2021
1 parent 5d32d55 commit 331da4a
Show file tree
Hide file tree
Showing 37 changed files with 4,699 additions and 0 deletions.
70 changes: 70 additions & 0 deletions perception/multi_object_tracker/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,70 @@
cmake_minimum_required(VERSION 3.5)
project(multi_object_tracker)

### Compile options
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic -Werror)
endif()

# Ignore -Wnonportable-include-path in Clang for mussp
if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wno-nonportable-include-path)
endif()

### Find Packages
find_package(ament_cmake_auto REQUIRED)

### Find Eigen Dependencies
find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3 REQUIRED)

### Find dependencies listed in the package.xml
ament_auto_find_build_dependencies()

# Generate exe file
set(MULTI_OBJECT_TRACKER_SRC
src/multi_object_tracker_core.cpp
src/utils/utils.cpp
src/tracker/model/tracker_base.cpp
src/tracker/model/big_vehicle_tracker.cpp
src/tracker/model/normal_vehicle_tracker.cpp
src/tracker/model/multiple_vehicle_tracker.cpp
src/tracker/model/bicycle_tracker.cpp
src/tracker/model/pedestrian_tracker.cpp
src/tracker/model/pedestrian_and_bicycle_tracker.cpp
src/tracker/model/unknown_tracker.cpp
src/data_association/data_association.cpp
)

ament_auto_add_library(mu_successive_shortest_path SHARED
src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
)

ament_auto_add_library(multi_object_tracker_node SHARED
${MULTI_OBJECT_TRACKER_SRC}
)

target_link_libraries(multi_object_tracker_node
mu_successive_shortest_path
Eigen3::Eigen
)

rclcpp_components_register_node(multi_object_tracker_node
PLUGIN "MultiObjectTracker"
EXECUTABLE multi_object_tracker
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package(INSTALL_TO_SHARE
launch
config
)
131 changes: 131 additions & 0 deletions perception/multi_object_tracker/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,131 @@
# multi_object_tracker

## Purpose

The results of the detection are processed by a time series. The main purpose is to give ID and estimate velocity.

## Inner-workings / Algorithms

This multi object tracker consists of data association and EKF.

![multi_object_tracker_overview](image/multi_object_tracker_overview.svg)

### Data association

The data association performs maximum score matching, called min cost max flow problem.
In this package, mussp[1] is used as solver.
In addition, when associating observations to tracers, data association have gates such as the area of the object from the BEV, Mahalanobis distance, and maximum distance, depending on the class label.

### EKF Tracker

Models for pedestrians, bicycles (motorcycles), cars and unknown are available.
The pedestrian or bicycle tracker is running at the same time as the respective EKF model in order to enable the transition between pedestrian and bicycle tracking.
For big vehicles such as trucks and buses, we have separate models for passenger cars and large vehicles because they are difficult to distinguish from passenger cars and are not stable. Therefore, separate models are prepared for passenger cars and big vehicles, and these models are run at the same time as the respective EKF models to ensure stability.

<!-- Write how this package works. Flowcharts and figures are great. Add sub-sections as you like.
Example:
### Flowcharts
...(PlantUML or something)
### State Transitions
...(PlantUML or something)
### How to filter target obstacles
...
### How to optimize trajectory
...
-->

## Inputs / Outputs

### Input

| Name | Type | Description |
| --------- | ----------------------------------------------------- | ----------- |
| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | obstacles |

### Output

| Name | Type | Description |
| ---------- | ---------------------------------------------------- | ------------------ |
| `~/output` | `autoware_auto_perception_msgs::msg::TrackedObjects` | modified obstacles |

## Parameters

<!-- Write parameters of this package.
Example:
### Node Parameters
| Name | Type | Description |
| ---------------------- | ---- | ------------------------------- |
| `output_debug_markers` | bool | whether to output debug markers |
-->

### Core Parameters

| Name | Type | Description |
| --------------------------- | ------ | -------------------------------------------------------------- |
| `can_assign_matrix` | double | Assignment table for data association |
| `max_dist_matrix` | double | Maximum distance table for data association |
| `max_area_matrix` | double | Maximum area table for data association |
| `min_area_matrix` | double | Minimum area table for data association |
| `max_rad_matrix` | double | Maximum angle table for data association |
| `world_frame_id` | double | tracking frame |
| `enable_delay_compensation` | bool | Estimate obstacles at current time considering detection delay |
| `publish_rate` | double | if enable_delay_compensation is true, how many hertz to output |

## Assumptions / Known limits

<!-- Write assumptions and limitations of your implementation.
Example:
This algorithm assumes obstacles are not moving, so if they rapidly move after the vehicle started to avoid them, it might collide with them.
Also, this algorithm doesn't care about blind spots. In general, since too close obstacles aren't visible due to the sensing performance limit, please take enough margin to obstacles.
-->

## (Optional) Error detection and handling

<!-- Write how to detect errors and how to recover from them.
Example:
This package can handle up to 20 obstacles. If more obstacles found, this node will give up and raise diagnostic errors.
-->

## (Optional) Performance characterization

### Evaluation of muSSP

According to our evaluation, muSSP is faster than normal [SSP](src/data_association/successive_shortest_path) when the matrix size is more than 100.

Execution time for varying matrix size at 95% sparsity. In real data, the sparsity was often around 95%.
![mussp_evaluation1](image/mussp_evaluation1.png)

Execution time for varying the sparsity with matrix size 100.
![mussp_evaluation2](image/mussp_evaluation2.png)

## (Optional) References/External links

This package makes use of external code.

| Name | License | Original Repository |
| -------------------------------------------------------------- | ------------------------------------------------------------------- | ------------------------------------ |
| [muSSP](src/data_association/mu_successive_shortest_path/impl) | [Apache 2.0](src/data_association/mu_successive_shortest_path/impl) | <https://github.com/yu-lab-vt/muSSP> |

[1] C. Wang, Y. Wang, Y. Wang, C.-t. Wu, and G. Yu, “muSSP: Efficient
Min-cost Flow Algorithm for Multi-object Tracking,” NeurIPS, 2019

## (Optional) Future extensions / Unimplemented parts

<!-- Write future extensions of this package.
Example:
Currently, this package can't handle the chattering obstacles well. We plan to add some probabilistic filters in the perception layer to improve it.
Also, there are some parameters that should be global(e.g. vehicle size, max steering, etc.). These will be refactored and defined as global parameters so that we can share the same parameters between different nodes.
-->
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
/**:
ros__parameters:
can_assign_matrix:
#UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL
[0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN
1, 1, 1, 1, 0, 0, 0, 0, #CAR
1, 1, 1, 1, 0, 0, 0, 0, #TRUCK
1, 1, 1, 1, 0, 0, 0, 0, #BUS
0, 0, 0, 0, 1, 1, 1, 0, #BICYCLE
0, 0, 0, 0, 1, 1, 1, 0, #MOTORBIKE
0, 0, 0, 0, 1, 1, 1, 0, #PEDESTRIAN
0, 0, 0, 0, 0, 0, 0, 1] #ANIMAL
max_dist_matrix:
#UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL
[1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN
4.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, #CAR
4.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, #TRUCK
4.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, #BUS
3.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, 1.0, #BICYCLE
3.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, 1.0, #MOTORBIKE
2.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, 1.0, #PEDESTRIAN
1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] #ANIMAL
max_area_matrix:
#UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL
[10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, #UNKNOWN
12.10, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #CAR
19.75, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #TRUCK
32.40, 12.10, 19.75, 32.40, 10000.00, 10000.00, 10000.00, 10000.00, #BUS
2.50, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, 10000.00, #BICYCLE
3.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, 10000.00, #MOTORBIKE
2.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00, 10000.00, #PEDESTRIAN
2.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00] #ANIMAL
min_area_matrix:
#UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL
[ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN
3.600, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #CAR
6.000, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #TRUCK
10.000, 3.600, 6.000, 10.000, 0.000, 0.000, 0.000, 0.000, #BUS
0.001, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, 0.000, #BICYCLE
0.001, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, 0.000, #MOTORBIKE
0.001, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, 0.000, #PEDESTRIAN
0.500, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #ANIMAL
max_rad_matrix: # If value is greater than pi, it will be ignored.
#UNKNOWN, CAR, TRUCK, BUS, BICYCLE, MOTORBIKE, PEDESTRIAN, ANIMAL
[3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN
3.150, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, 3.150, #CAR
3.150, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, 3.150, #TRUCK
3.150, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, 3.150, #BUS
3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE
3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE
3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #PEDESTRIAN
3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #ANIMAL
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Original file line number Diff line number Diff line change
@@ -0,0 +1,62 @@
// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
//
// Author: v1.0 Yukihiro Saito
//

#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_

#include <list>
#include <memory>
#include <unordered_map>
#include <vector>

#define EIGEN_MPL2_ONLY
#include "multi_object_tracker/data_association/solver/gnn_solver.hpp"
#include "multi_object_tracker/tracker/tracker.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>

class DataAssociation
{
private:
Eigen::MatrixXi can_assign_matrix_;
Eigen::MatrixXd max_dist_matrix_;
Eigen::MatrixXd max_area_matrix_;
Eigen::MatrixXd min_area_matrix_;
Eigen::MatrixXd max_rad_matrix_;
const double score_threshold_;
std::unique_ptr<gnn_solver::GnnSolverInterface> gnn_solver_ptr_;

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
DataAssociation(
std::vector<int> can_assign_vector, std::vector<double> max_dist_vector,
std::vector<double> max_area_vector, std::vector<double> min_area_vector,
std::vector<double> max_rad_vector);
void assign(
const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
std::unordered_map<int, int> & reverse_assignment);
Eigen::MatrixXd calcScoreMatrix(
const autoware_auto_perception_msgs::msg::DetectedObjects & measurements,
const std::list<std::shared_ptr<Tracker>> & trackers);
virtual ~DataAssociation() {}
};

#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
// Copyright 2021 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_

#include "multi_object_tracker/data_association/solver/gnn_solver_interface.hpp"
#include "multi_object_tracker/data_association/solver/mu_successive_shortest_path.hpp"
#include "multi_object_tracker/data_association/solver/successive_shortest_path.hpp"

#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
// Copyright 2021 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
#define MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_

#include <unordered_map>
#include <vector>

namespace gnn_solver
{
class GnnSolverInterface
{
public:
GnnSolverInterface() = default;
virtual ~GnnSolverInterface() = default;

virtual void maximizeLinearAssignment(
const std::vector<std::vector<double>> & cost, std::unordered_map<int, int> * direct_assignment,
std::unordered_map<int, int> * reverse_assignment) = 0;
};
} // namespace gnn_solver

#endif // MULTI_OBJECT_TRACKER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_
Loading

0 comments on commit 331da4a

Please sign in to comment.