Skip to content

Commit

Permalink
Ros2 v0.8.0 multi object tracker (autowarefoundation#259)
Browse files Browse the repository at this point in the history 
* 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>
  • Loading branch information
@wep21 @kmiya
@yukkysaito @kenji-miyake
5 people authored and wep21 committed Jan 30, 2021
1 parent a95e615 commit e079e34
Show file tree
Hide file tree
Showing 11 changed files with 164 additions and 282 deletions.
7 changes: 4 additions & 3 deletions perception/object_recognition/tracking/multi_object_tracker/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,8 +45,8 @@ set(MULTI_OBJECT_TRACKER_HEADERS
include/multi_object_tracker/multi_object_tracker_core.hpp
)

ament_auto_add_library(multi_object_tracker_node SHARED
${MULTI_OBJECT_TRACKER_SRC}
ament_auto_add_library(multi_object_tracker_node SHARED
${MULTI_OBJECT_TRACKER_SRC}
${MULTI_OBJECT_TRACKER_HEADERS}
)

Expand All @@ -64,4 +64,5 @@ endif()

ament_auto_package(INSTALL_TO_SHARE
launch
)
config
)
42 changes: 42 additions & 0 deletions ...ion/object_recognition/tracking/multi_object_tracker/config/data_assosiaction_matrix.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
/**:
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, 0, 0, #BICYCLE
0, 0, 0, 0, 1, 1, 0, 0, #MOTORBIKE
0, 0, 0, 0, 0, 0, 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, 4.5, 4.5, 4.5, 1.0, 1.0, 1.0, 1.0, #CAR
4.0, 4.0, 4.0, 4.0, 1.0, 1.0, 1.0, 1.0, #TRUCK
4.0, 4.0, 4.0, 4.0, 1.0, 1.0, 1.0, 1.0, #BUS
3.0, 1.0, 1.0, 1.0, 3.0, 3.0, 1.0, 1.0, #BICYCLE
3.0, 1.0, 1.0, 1.0, 3.0, 2.0, 1.0, 1.0, #MOTORBIKE
2.0, 1.0, 1.0, 1.0, 1.0, 1.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, 3.00, 10000.00, 10000.00, #BICYCLE
3.00, 10000.00, 10000.00, 10000.00, 2.50, 3.00, 10000.00, 10000.00, #MOTORBIKE
2.00, 10000.00, 10000.00, 10000.00, 10000.00, 10000.00, 2.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.001, 0.001, 0.000, 0.000, #BICYCLE
0.001, 0.000, 0.000, 0.000, 0.001, 0.001, 0.000, 0.000, #MOTORBIKE
0.001, 0.000, 0.000, 0.000, 0.000, 0.000, 0.001, 0.000, #PEDESTRIAN
0.500, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #ANIMAL
6 changes: 4 additions & 2 deletions ...g/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,14 +35,16 @@ class DataAssociation
private:
double getDistance(
const geometry_msgs::msg::Point & measurement, const geometry_msgs::msg::Point & tracker);
Eigen::MatrixXi can_assgin_matrix_;
Eigen::MatrixXi can_assign_matrix_;
Eigen::MatrixXd max_dist_matrix_;
Eigen::MatrixXd max_area_matrix_;
Eigen::MatrixXd min_area_matrix_;
const double score_threshold_;

public:
DataAssociation();
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);
void assign(
const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
std::unordered_map<int, int> & reverse_assignment);
Expand Down
3 changes: 2 additions & 1 deletion .../tracking/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,8 @@ class MultiObjectTracker : public rclcpp::Node

std::string world_frame_id_; // tracking frame
std::list<std::shared_ptr<Tracker>> list_tracker_;
DataAssociation data_association_;
std::unique_ptr<DataAssociation> data_association_;
bool enable_delay_compensation_;
};

#endif
2 changes: 2 additions & 0 deletions ...n/object_recognition/tracking/multi_object_tracker/launch/multi_object_tracker.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,13 @@
<arg name="output" default="objects"/>
<arg name="world_frame_id" default="map"/>
<arg name="publish_rate" default="10.0"/>
<arg name="data_association_matrix_path" default="$(find-pkg-share multi_object_tracker)/config/data_assosiaction_matrix.yaml" />

<node pkg="multi_object_tracker" exec="multi_object_tracker" name="multi_object_tracker" output="screen">
<remap from="input" to="$(var input)"/>
<remap from="output" to="$(var output)"/>
<param name="world_frame_id" value="$(var world_frame_id)" />
<param name="publish_rate" value="$(var publish_rate)" />
<param from="$(var data_association_matrix_path)" />
</node>
</launch>
269 changes: 28 additions & 241 deletions ...bject_recognition/tracking/multi_object_tracker/src/data_association/data_association.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,248 +21,35 @@
#include "successive_shortest_path/successive_shortest_path.hpp"
#include "multi_object_tracker/utils/utils.hpp"

DataAssociation::DataAssociation()
DataAssociation::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)
: score_threshold_(0.1)
{
can_assgin_matrix_ = Eigen::MatrixXi::Identity(20, 20);
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::UNKNOWN,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::CAR,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::TRUCK) =
1;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::BUS) = 1;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::CAR) =
1;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::BUS) =
1;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::BUS,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::CAR) = 1;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::TRUCK) =
1;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::BICYCLE,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::BICYCLE,
autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 1;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::MOTORBIKE,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::MOTORBIKE,
autoware_perception_msgs::msg::Semantic::BICYCLE) = 1;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::PEDESTRIAN,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0;
can_assgin_matrix_(
autoware_perception_msgs::msg::Semantic::ANIMAL,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0;
max_dist_matrix_ = Eigen::MatrixXd::Constant(20, 20, 1.0);
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::CAR,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 4.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::CAR) =
4.5;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::TRUCK) =
4.5;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::BUS) =
4.5;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 4.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::CAR) =
4.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK,
autoware_perception_msgs::msg::Semantic::TRUCK) = 4.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::BUS) =
4.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::BUS,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 4.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::CAR) =
4.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::TRUCK) =
4.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::BUS) =
4.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::BICYCLE,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 3.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::BICYCLE,
autoware_perception_msgs::msg::Semantic::BICYCLE) = 3.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::BICYCLE,
autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 3.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::MOTORBIKE,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 3.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::MOTORBIKE,
autoware_perception_msgs::msg::Semantic::BICYCLE) = 3.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::MOTORBIKE,
autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 2.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::PEDESTRIAN,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::PEDESTRIAN,
autoware_perception_msgs::msg::Semantic::PEDESTRIAN) = 2.0;
max_dist_matrix_(
autoware_perception_msgs::msg::Semantic::ANIMAL,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 1.0;
max_area_matrix_ = Eigen::MatrixXd::Constant(20, 20, /* large number */ 10000.0);
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::CAR,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.2 * 5.5;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::CAR) =
2.2 * 5.5;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::TRUCK) =
2.5 * 7.9;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::BUS) =
2.7 * 12.0;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.5 * 7.9;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::CAR) =
2.2 * 5.5;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK,
autoware_perception_msgs::msg::Semantic::TRUCK) = 2.5 * 7.9;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::BUS) =
2.7 * 12.0;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::BUS,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.7 * 12.0;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::CAR) =
2.2 * 5.5;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::TRUCK) =
2.5 * 7.9;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::BUS) =
2.7 * 12.0;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::BICYCLE,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.5;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::BICYCLE,
autoware_perception_msgs::msg::Semantic::BICYCLE) = 2.5;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::BICYCLE,
autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 3.0;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::MOTORBIKE,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 3.0;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::MOTORBIKE,
autoware_perception_msgs::msg::Semantic::BICYCLE) = 2.5;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::MOTORBIKE,
autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 3.0;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::PEDESTRIAN,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::PEDESTRIAN,
autoware_perception_msgs::msg::Semantic::PEDESTRIAN) = 2.0;
max_area_matrix_(
autoware_perception_msgs::msg::Semantic::ANIMAL,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0;
min_area_matrix_ = Eigen::MatrixXd::Constant(20, 20, /* small number */ 0.0);
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::CAR,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 1.2 * 3.0;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::CAR) =
1.2 * 3.0;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::TRUCK) =
1.5 * 4.0;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::CAR, autoware_perception_msgs::msg::Semantic::BUS) =
2.0 * 5.0;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 1.5 * 4.0;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::CAR) =
1.2 * 3.0;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK,
autoware_perception_msgs::msg::Semantic::TRUCK) = 1.5 * 4.0;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::TRUCK, autoware_perception_msgs::msg::Semantic::BUS) =
2.0 * 5.0;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::BUS,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 2.0 * 5.0;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::CAR) =
1.2 * 3.0;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::TRUCK) =
1.5 * 4.0;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::BUS, autoware_perception_msgs::msg::Semantic::BUS) =
2.0 * 5.0;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::BICYCLE,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0.001;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::BICYCLE,
autoware_perception_msgs::msg::Semantic::BICYCLE) = 0.001;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::BICYCLE,
autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 0.001;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::MOTORBIKE,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0.001;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::MOTORBIKE,
autoware_perception_msgs::msg::Semantic::BICYCLE) = 0.001;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::MOTORBIKE,
autoware_perception_msgs::msg::Semantic::MOTORBIKE) = 0.001;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::PEDESTRIAN,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0.001;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::PEDESTRIAN,
autoware_perception_msgs::msg::Semantic::PEDESTRIAN) = 0.001;
min_area_matrix_(
autoware_perception_msgs::msg::Semantic::ANIMAL,
autoware_perception_msgs::msg::Semantic::UNKNOWN) = 0.5;
{
const int assign_label_num = static_cast<int>(std::sqrt(can_assign_vector.size()));
Eigen::Map<Eigen::MatrixXi> can_assign_matrix_tmp(
can_assign_vector.data(), assign_label_num, assign_label_num);
can_assign_matrix_ = can_assign_matrix_tmp.transpose();
}
{
const int max_dist_label_num = static_cast<int>(std::sqrt(max_dist_vector.size()));
Eigen::Map<Eigen::MatrixXd> max_dist_matrix_tmp(
max_dist_vector.data(), max_dist_label_num, max_dist_label_num);
max_dist_matrix_ = max_dist_matrix_tmp.transpose();
}
{
const int max_area_label_num = static_cast<int>(std::sqrt(max_area_vector.size()));
Eigen::Map<Eigen::MatrixXd> max_area_matrix_tmp(
max_area_vector.data(), max_area_label_num, max_area_label_num);
max_area_matrix_ = max_area_matrix_tmp.transpose();
}
{
const int min_area_label_num = static_cast<int>(std::sqrt(min_area_vector.size()));
Eigen::Map<Eigen::MatrixXd> min_area_matrix_tmp(
min_area_vector.data(), min_area_label_num, min_area_label_num);
min_area_matrix_ = min_area_matrix_tmp.transpose();
}
}

void DataAssociation::assign(
Expand Down Expand Up @@ -311,7 +98,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
++measurement_idx)
{
double score = 0.0;
if (can_assgin_matrix_(
if (can_assign_matrix_(
(*tracker_itr)->getType(),
measurements.feature_objects.at(measurement_idx).object.semantic.type))
{
Expand Down
2 changes: 1 addition & 1 deletion ...object_tracker/src/data_association/successive_shortest_path/successive_shortest_path.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ namespace assignment_problem
{
struct ResidualEdge
{
// Desternation node
// Destination node
const int dst;
int capacity;
const double cost;
Expand Down
Loading

0 comments on commit e079e34

Please sign in to comment.