diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index dbb74335f79d2..33cd501dbe7fd 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -150,6 +150,16 @@
+
+
+
+
+
+
+
+
+
+
diff --git a/perception/detection_by_tracker/CMakeLists.txt b/perception/detection_by_tracker/CMakeLists.txt
index 3813f1ba4707f..92f032cb33f1a 100644
--- a/perception/detection_by_tracker/CMakeLists.txt
+++ b/perception/detection_by_tracker/CMakeLists.txt
@@ -27,6 +27,7 @@ include_directories(
# Generate exe file
set(DETECTION_BY_TRACKER_SRC
src/detection_by_tracker_core.cpp
+ src/likelihood_field_tracker.cpp
src/utils.cpp
)
@@ -44,6 +45,20 @@ rclcpp_components_register_node(detection_by_tracker_node
EXECUTABLE detection_by_tracker
)
+ament_auto_add_library(likelihood_field_tracker_node SHARED
+ ${DETECTION_BY_TRACKER_SRC}
+)
+
+target_link_libraries(likelihood_field_tracker_node
+ Eigen3::Eigen
+ ${PCL_LIBRARIES}
+)
+
+rclcpp_components_register_node(likelihood_field_tracker_node
+ PLUGIN "LikelihoodFieldTracker"
+ EXECUTABLE likelihood_field_tracker
+)
+
ament_auto_package(
INSTALL_TO_SHARE
launch
diff --git a/perception/detection_by_tracker/include/detection_by_tracker/likelihood_field_tracker.hpp b/perception/detection_by_tracker/include/detection_by_tracker/likelihood_field_tracker.hpp
new file mode 100644
index 0000000000000..2e800df533195
--- /dev/null
+++ b/perception/detection_by_tracker/include/detection_by_tracker/likelihood_field_tracker.hpp
@@ -0,0 +1,232 @@
+// 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 DETECTION_BY_TRACKER__LIKELIHOOD_FIELD_TRACKER_CORE_HPP_
+#define DETECTION_BY_TRACKER__LIKELIHOOD_FIELD_TRACKER_CORE_HPP_
+
+#include "detection_by_tracker/debugger.hpp"
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+
+#include
+#include
+#include
+
+#ifdef ROS_DISTRO_GALACTIC
+#include
+#else
+#include
+#endif
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include "detection_by_tracker/detection_by_tracker_core.hpp"
+//using autoware::common::types::float64_t; // convert double to float64_t later
+
+
+/**
+ * @brief rectangle zone calc function
+ *
+ */
+class RectangleZone
+{
+//private:
+public:
+ double xmin_, xmax_, ymin_, ymax_;
+
+public:
+ RectangleZone();/// default constructor
+ RectangleZone(const double xmin, const double xmax, const double ymin, const double ymax);
+ void setZone(const double xmin, const double xmax, const double ymin, const double ymax);
+ bool contains(const Eigen::Vector2d point); /// if input coordinates is inside the rectangle zone, geometry_msgs::msg::Point
+};
+
+/**
+ * @brief Express Car Likelihood Field used in coarse fitting
+ *
+ * @details likelihood is calc as following equations:
+ * exp(-c/2/sigma/sigma)
+ * ,while c is cost parameter and sigma is measurement noise covariance
+ */
+class CoarseCarLikelihoodField
+{
+ private:
+ double measurement_covariance_ = 0.1;
+ /// cost value of {contour, outside}
+ std::vector costs_ = {-0.02, 0};
+ std::array contour_zones_;
+ const std::array, 4> indexes_ = {{{3,0},{0,1},{1,2},{2,3}}};
+
+
+ public:
+ explicit CoarseCarLikelihoodField(const double width, const double length,
+ const double outside_margin, const double inside_margin);
+ void setContourZones(const double width, const double length, const double outside_margin, const double inside_margin);
+ void setMeasurementCovariance(const double cov);
+ void setCostParameters(const std::vector & costs);
+ double calcLikelihoods(std::vector localized_measurements, std::uint8_t index_num);
+};
+
+
+/**
+ * @brief Express Car Likelihood Field used in fine fitting
+ *
+ */
+class FineCarLikelihoodField
+{
+ private:
+
+ double measurement_covariance_ = 0.1;
+
+ public:
+ RectangleZone car_contour_; /// Rectangle Area
+ /// cost value represent {penalty, contour, inside, outside}
+ std::vector costs_ = {0.02,-0.04, -0.01, 0};
+ const int indexes_[4][2] = {{3,0},{0,1},{1,2},{2,3}};
+ std::array penalty_zones_;
+ std::array contour_zones_;
+
+ explicit FineCarLikelihoodField(const double width, const double length,
+ const double outside_margin, const double inside_margin);
+ void setContourZones(const double width, const double length, const double outside_margin, const double inside_margin);
+ void setPenaltyZones(const double width, const double length, const double outside_margin, const double inside_margin);
+ void setMeasurementCovariance(const double cov);
+ void setCostParameters(const std::vector & costs);
+ double calcLikelihoods(std::vector localized_measurements, std::uint8_t index_num);
+};
+
+
+/**
+ * @brief Manage Each Vehicle Particle
+ *
+ */
+class VehicleParticle
+{
+private:
+ const double inside_margin_ = 0.25;
+ const double outside_margin_ = 1.0;
+ double measurement_noise_ = 0.1;
+
+
+
+public:
+ FineCarLikelihoodField fine_likelihood_; // maybe it's ok to use std::optional instead
+ CoarseCarLikelihoodField coarse_likelihood_;
+ std::array corner_points_;
+
+ Eigen::Vector2d center_;
+ double orientation_;
+ std::uint8_t corner_index_;
+ explicit VehicleParticle(const Eigen::Vector2d center, const double width, const double length, const double orientation);
+ void setCornerPoints(const Eigen::Vector2d center, const double width, const double length, const double orientation);
+ std::uint8_t getNearestCornerIndex(const Eigen::Vector2d & origin = Eigen::Vector2d(0.0,0.0)); /// ego origin is set 0,0 by default
+ void toLocalCoordinate(const std::vector & measurements, const Eigen::Vector2d & center, double orientation, std::vector& local_measurements);
+ double calcCoarseLikelihood(const std::vector & measurements);
+ double calcFineLikelihood(const std::vector & measurements);
+};
+
+
+class SingleLFTracker
+{
+ private:
+ std::vector vehicle_particle_;
+ VehicleParticle default_vehicle_;
+ std::uint32_t particle_num_;
+ Eigen::Vector2d position_;
+ double orientation_;
+ double width_;
+ double length_;
+ double default_likelihood_;
+ Eigen::Matrix3d covariance_;
+
+ public:
+ SingleLFTracker(const autoware_auto_perception_msgs::msg::TrackedObject & object);
+ void createRandomVehiclePositionParticle(const std::uint32_t particle_num);
+ void createGridVehiclePositionParticle();
+ //void createVehicleShapeParticle();
+ std::tuple calcMeanAndCovFromParticles(std::vector & likelihoods, std::vector vectors);
+ std::tuple calcBestParticles(std::vector & likelihoods, std::vector vectors);
+ void estimateState(const std::vector & scan);
+ autoware_auto_perception_msgs::msg::TrackedObject toTrackedObject(autoware_auto_perception_msgs::msg::TrackedObject &object);
+};
+
+
+
+class LikelihoodFieldTracker : public rclcpp::Node
+{
+public:
+ explicit LikelihoodFieldTracker(const rclcpp::NodeOptions & node_options);
+
+private:
+ rclcpp::Publisher::SharedPtr objects_pub_;
+ rclcpp::Subscription::SharedPtr trackers_sub_;
+ rclcpp::Subscription::SharedPtr scans_sub_;
+ rclcpp::Subscription::SharedPtr
+ initial_objects_sub_;
+
+ tf2_ros::Buffer tf_buffer_;
+ tf2_ros::TransformListener tf_listener_;
+
+ TrackerHandler tracker_handler_;
+ std::shared_ptr shape_estimator_;
+ std::shared_ptr cluster_;
+ std::shared_ptr debugger_;
+
+ bool ignore_unknown_tracker_;
+
+ void onObjects(
+ const sensor_msgs::msg::LaserScan::ConstSharedPtr input_msg);
+
+ // void divideUnderSegmentedObjects(
+ // const autoware_auto_perception_msgs::msg::DetectedObjects & tracked_objects,
+ // const tier4_perception_msgs::msg::DetectedObjectsWithFeature & in_objects,
+ // autoware_auto_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects,
+ // tier4_perception_msgs::msg::DetectedObjectsWithFeature & out_objects);
+
+ // float optimizeUnderSegmentedObject(
+ // const autoware_auto_perception_msgs::msg::DetectedObject & target_object,
+ // const sensor_msgs::msg::PointCloud2 & under_segmented_cluster,
+ // tier4_perception_msgs::msg::DetectedObjectWithFeature & output);
+
+ // void mergeOverSegmentedObjects(
+ // const autoware_auto_perception_msgs::msg::DetectedObjects & tracked_objects,
+ // const tier4_perception_msgs::msg::DetectedObjectsWithFeature & in_objects,
+ // autoware_auto_perception_msgs::msg::DetectedObjects & out_no_found_tracked_objects,
+ // tier4_perception_msgs::msg::DetectedObjectsWithFeature & out_objects);
+};
+
+#endif // DETECTION_BY_TRACKER__LIKELIHOOD_FIELD_TRACKER_CORE_HPP_
diff --git a/perception/detection_by_tracker/include/detection_by_tracker/nlohmann_json.hpp b/perception/detection_by_tracker/include/detection_by_tracker/nlohmann_json.hpp
new file mode 100644
index 0000000000000..805efdd760b70
--- /dev/null
+++ b/perception/detection_by_tracker/include/detection_by_tracker/nlohmann_json.hpp
@@ -0,0 +1,24640 @@
+// __ _____ _____ _____
+// __| | __| | | | JSON for Modern C++
+// | | |__ | | | | | | version 3.11.2
+// |_____|_____|_____|_|___| https://github.com/nlohmann/json
+//
+// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann
+// SPDX-License-Identifier: MIT
+
+/****************************************************************************\
+ * Note on documentation: The source files contain links to the online *
+ * documentation of the public API at https://json.nlohmann.me. This URL *
+ * contains the most recent documentation and should also be applicable to *
+ * previous versions; documentation for deprecated functions is not *
+ * removed, but marked deprecated. See "Generate documentation" section in *
+ * file docs/README.md. *
+\****************************************************************************/
+
+#ifndef INCLUDE_NLOHMANN_JSON_HPP_
+#define INCLUDE_NLOHMANN_JSON_HPP_
+
+#include // all_of, find, for_each
+#include // nullptr_t, ptrdiff_t, size_t
+#include // hash, less
+#include // initializer_list
+#ifndef JSON_NO_IO
+ #include // istream, ostream
+#endif // JSON_NO_IO
+#include // random_access_iterator_tag
+#include // unique_ptr
+#include // string, stoi, to_string
+#include // declval, forward, move, pair, swap
+#include // vector
+
+// #include
+// __ _____ _____ _____
+// __| | __| | | | JSON for Modern C++
+// | | |__ | | | | | | version 3.11.2
+// |_____|_____|_____|_|___| https://github.com/nlohmann/json
+//
+// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann
+// SPDX-License-Identifier: MIT
+
+
+
+#include
+
+// #include
+// __ _____ _____ _____
+// __| | __| | | | JSON for Modern C++
+// | | |__ | | | | | | version 3.11.2
+// |_____|_____|_____|_|___| https://github.com/nlohmann/json
+//
+// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann
+// SPDX-License-Identifier: MIT
+
+
+
+// This file contains all macro definitions affecting or depending on the ABI
+
+#ifndef JSON_SKIP_LIBRARY_VERSION_CHECK
+ #if defined(NLOHMANN_JSON_VERSION_MAJOR) && defined(NLOHMANN_JSON_VERSION_MINOR) && defined(NLOHMANN_JSON_VERSION_PATCH)
+ #if NLOHMANN_JSON_VERSION_MAJOR != 3 || NLOHMANN_JSON_VERSION_MINOR != 11 || NLOHMANN_JSON_VERSION_PATCH != 2
+ #warning "Already included a different version of the library!"
+ #endif
+ #endif
+#endif
+
+#define NLOHMANN_JSON_VERSION_MAJOR 3 // NOLINT(modernize-macro-to-enum)
+#define NLOHMANN_JSON_VERSION_MINOR 11 // NOLINT(modernize-macro-to-enum)
+#define NLOHMANN_JSON_VERSION_PATCH 2 // NOLINT(modernize-macro-to-enum)
+
+#ifndef JSON_DIAGNOSTICS
+ #define JSON_DIAGNOSTICS 0
+#endif
+
+#ifndef JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON
+ #define JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON 0
+#endif
+
+#if JSON_DIAGNOSTICS
+ #define NLOHMANN_JSON_ABI_TAG_DIAGNOSTICS _diag
+#else
+ #define NLOHMANN_JSON_ABI_TAG_DIAGNOSTICS
+#endif
+
+#if JSON_USE_LEGACY_DISCARDED_VALUE_COMPARISON
+ #define NLOHMANN_JSON_ABI_TAG_LEGACY_DISCARDED_VALUE_COMPARISON _ldvcmp
+#else
+ #define NLOHMANN_JSON_ABI_TAG_LEGACY_DISCARDED_VALUE_COMPARISON
+#endif
+
+#ifndef NLOHMANN_JSON_NAMESPACE_NO_VERSION
+ #define NLOHMANN_JSON_NAMESPACE_NO_VERSION 0
+#endif
+
+// Construct the namespace ABI tags component
+#define NLOHMANN_JSON_ABI_TAGS_CONCAT_EX(a, b) json_abi ## a ## b
+#define NLOHMANN_JSON_ABI_TAGS_CONCAT(a, b) \
+ NLOHMANN_JSON_ABI_TAGS_CONCAT_EX(a, b)
+
+#define NLOHMANN_JSON_ABI_TAGS \
+ NLOHMANN_JSON_ABI_TAGS_CONCAT( \
+ NLOHMANN_JSON_ABI_TAG_DIAGNOSTICS, \
+ NLOHMANN_JSON_ABI_TAG_LEGACY_DISCARDED_VALUE_COMPARISON)
+
+// Construct the namespace version component
+#define NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT_EX(major, minor, patch) \
+ _v ## major ## _ ## minor ## _ ## patch
+#define NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT(major, minor, patch) \
+ NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT_EX(major, minor, patch)
+
+#if NLOHMANN_JSON_NAMESPACE_NO_VERSION
+#define NLOHMANN_JSON_NAMESPACE_VERSION
+#else
+#define NLOHMANN_JSON_NAMESPACE_VERSION \
+ NLOHMANN_JSON_NAMESPACE_VERSION_CONCAT(NLOHMANN_JSON_VERSION_MAJOR, \
+ NLOHMANN_JSON_VERSION_MINOR, \
+ NLOHMANN_JSON_VERSION_PATCH)
+#endif
+
+// Combine namespace components
+#define NLOHMANN_JSON_NAMESPACE_CONCAT_EX(a, b) a ## b
+#define NLOHMANN_JSON_NAMESPACE_CONCAT(a, b) \
+ NLOHMANN_JSON_NAMESPACE_CONCAT_EX(a, b)
+
+#ifndef NLOHMANN_JSON_NAMESPACE
+#define NLOHMANN_JSON_NAMESPACE \
+ nlohmann::NLOHMANN_JSON_NAMESPACE_CONCAT( \
+ NLOHMANN_JSON_ABI_TAGS, \
+ NLOHMANN_JSON_NAMESPACE_VERSION)
+#endif
+
+#ifndef NLOHMANN_JSON_NAMESPACE_BEGIN
+#define NLOHMANN_JSON_NAMESPACE_BEGIN \
+ namespace nlohmann \
+ { \
+ inline namespace NLOHMANN_JSON_NAMESPACE_CONCAT( \
+ NLOHMANN_JSON_ABI_TAGS, \
+ NLOHMANN_JSON_NAMESPACE_VERSION) \
+ {
+#endif
+
+#ifndef NLOHMANN_JSON_NAMESPACE_END
+#define NLOHMANN_JSON_NAMESPACE_END \
+ } /* namespace (inline namespace) NOLINT(readability/namespace) */ \
+ } // namespace nlohmann
+#endif
+
+// #include
+// __ _____ _____ _____
+// __| | __| | | | JSON for Modern C++
+// | | |__ | | | | | | version 3.11.2
+// |_____|_____|_____|_|___| https://github.com/nlohmann/json
+//
+// SPDX-FileCopyrightText: 2013-2022 Niels Lohmann
+// SPDX-License-Identifier: MIT
+
+
+
+#include // transform
+#include // array
+#include // forward_list
+#include // inserter, front_inserter, end
+#include