-
Notifications
You must be signed in to change notification settings - Fork 682
Commit
Signed-off-by: vividf <yihsiang.fang@tier4.jp>
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,106 @@ | ||
// Copyright 2024 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. | ||
|
||
#pragma once | ||
|
||
#include "autoware/image_projection_based_fusion/camera_projection.hpp" | ||
|
||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <cstddef> | ||
#include <memory> | ||
#include <mutex> | ||
#include <unordered_map> | ||
#include <vector> | ||
|
||
namespace autoware::image_projection_based_fusion | ||
{ | ||
using autoware::image_projection_based_fusion::CameraProjection; | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
class FusionNode; | ||
|
||
template <class Msg2D> | ||
struct Det2dStatus | ||
{ | ||
// camera index | ||
std::size_t id = 0; | ||
// camera projection | ||
std::unique_ptr<CameraProjection> camera_projector_ptr; | ||
bool project_to_unrectified_image = false; | ||
bool approximate_camera_projection = false; | ||
}; | ||
|
||
struct FusionCollectorInfoBase | ||
{ | ||
virtual ~FusionCollectorInfoBase() = default; | ||
}; | ||
|
||
struct NaiveCollectorInfo : public FusionCollectorInfoBase | ||
{ | ||
double timestamp; | ||
double threshold; | ||
|
||
explicit NaiveCollectorInfo(double timestamp = 0.0, double threshold = 0.0) | ||
: timestamp(timestamp), threshold(threshold) | ||
{ | ||
} | ||
}; | ||
|
||
struct AdvancedCollectorInfo : public FusionCollectorInfoBase | ||
{ | ||
double timestamp; | ||
double noise_window; | ||
|
||
explicit AdvancedCollectorInfo(double timestamp = 0.0, double noise_window = 0.0) | ||
: timestamp(timestamp), noise_window(noise_window) | ||
{ | ||
} | ||
}; | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
class FusionCollector | ||
{ | ||
public: | ||
FusionCollector( | ||
std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> && ros2_parent_node, double timeout_sec, | ||
bool debug_mode); | ||
bool process_msg_3d(const typename Msg3D::ConstSharedPtr msg_3d); | ||
bool process_rois(const std::size_t & roi_id, const typename Msg2D::ConstSharedPtr det2d_msg); | ||
void fusion_callback(); | ||
|
||
[[nodiscard]] bool fusion_finished() const; | ||
|
||
void set_info(std::shared_ptr<FusionCollectorInfoBase> collector_info); | ||
[[nodiscard]] std::shared_ptr<FusionCollectorInfoBase> get_info() const; | ||
// void show_debug_message(); | ||
bool ready_to_fuse(); | ||
bool rois_exists(const std::size_t & rois_id); | ||
bool det3d_exists(); | ||
|
||
private: | ||
std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> ros2_parent_node_; | ||
rclcpp::TimerBase::SharedPtr timer_; | ||
typename Msg3D::ConstSharedPtr det3d_msg_{nullptr}; | ||
std::vector<Det2dStatus<Msg2D>> det2d_list_; | ||
std::unordered_map<std::size_t, typename Msg2D::ConstSharedPtr> id_to_roi_map_; | ||
double timeout_sec_; | ||
double rois_number_; | ||
bool debug_mode_; | ||
bool fusion_finished_{false}; | ||
std::mutex fusion_mutex_; | ||
std::shared_ptr<FusionCollectorInfoBase> fusion_collector_info_; | ||
}; | ||
|
||
} // namespace autoware::image_projection_based_fusion |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,141 @@ | ||
// Copyright 2025 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. | ||
|
||
#pragma once | ||
|
||
#include "autoware/image_projection_based_fusion/fusion_collector.hpp" | ||
|
||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <cstddef> | ||
#include <list> | ||
#include <memory> | ||
#include <optional> | ||
#include <string> | ||
#include <unordered_map> | ||
|
||
namespace autoware::image_projection_based_fusion | ||
{ | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
class FusionNode; | ||
|
||
struct MatchingParamsBase | ||
{ | ||
virtual ~MatchingParamsBase() = default; | ||
}; | ||
|
||
struct Det3dMatchingParams : public MatchingParamsBase | ||
{ | ||
double det3d_timestamp; | ||
|
||
explicit Det3dMatchingParams(double det3d_timestamp = 0.0) : det3d_timestamp(det3d_timestamp) {} | ||
}; | ||
|
||
struct RoisMatchingParams : public MatchingParamsBase | ||
{ | ||
double rois_timestamp; | ||
std::size_t rois_id; | ||
|
||
explicit RoisMatchingParams(double rois_timestamp = 0.0, std::size_t rois_id = 0) | ||
: rois_timestamp(rois_timestamp), rois_id(rois_id) | ||
{ | ||
} | ||
}; | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
class FusionMatchingStrategy | ||
{ | ||
public: | ||
virtual ~FusionMatchingStrategy() = default; | ||
|
||
[[nodiscard]] virtual std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> | ||
match_rois_to_collector( | ||
const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors, | ||
const std::shared_ptr<RoisMatchingParams> & params) const = 0; | ||
|
||
[[nodiscard]] virtual std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> | ||
match_det3d_to_collector( | ||
const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors, | ||
const std::shared_ptr<Det3dMatchingParams> & params) const = 0; | ||
virtual void set_collector_info( | ||
std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> & collector, | ||
const std::shared_ptr<MatchingParamsBase> & matching_params) = 0; | ||
}; | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
class NaiveMatchingStrategy : public FusionMatchingStrategy<Msg3D, Msg2D, ExportObj> | ||
{ | ||
public: | ||
explicit NaiveMatchingStrategy( | ||
std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> && ros2_parent_node, | ||
std::size_t rois_number); | ||
|
||
[[nodiscard]] std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> | ||
match_rois_to_collector( | ||
const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors, | ||
const std::shared_ptr<RoisMatchingParams> & params) const override; | ||
|
||
[[nodiscard]] std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> | ||
match_det3d_to_collector( | ||
const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors, | ||
const std::shared_ptr<Det3dMatchingParams> & params) const override; | ||
|
||
void set_collector_info( | ||
std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> & collector, | ||
const std::shared_ptr<MatchingParamsBase> & matching_params) override; | ||
|
||
private: | ||
std::unordered_map<std::size_t, double> id_to_offset_map_; | ||
double threshold_; | ||
std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> ros2_parent_node_; | ||
}; | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
class AdvancedMatchingStrategy : public FusionMatchingStrategy<Msg3D, Msg2D, ExportObj> | ||
{ | ||
public: | ||
explicit AdvancedMatchingStrategy( | ||
std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> && ros2_parent_node, | ||
std::size_t rois_number); | ||
|
||
[[nodiscard]] std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> | ||
match_rois_to_collector( | ||
const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors, | ||
const std::shared_ptr<RoisMatchingParams> & params) const override; | ||
|
||
[[nodiscard]] std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> | ||
match_det3d_to_collector( | ||
const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors, | ||
const std::shared_ptr<Det3dMatchingParams> & params) const override; | ||
void set_collector_info( | ||
std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> & collector, | ||
const std::shared_ptr<MatchingParamsBase> & matching_params) override; | ||
|
||
double get_offset( | ||
const double & det3d_timestamp, | ||
const std::optional<std::unordered_map<std::string, std::string>> & concatenated_status) const; | ||
|
||
private: | ||
std::unordered_map<std::size_t, double> id_to_offset_map_; | ||
std::unordered_map<std::size_t, double> id_to_noise_window_map_; | ||
double det3d_noise_window_; | ||
std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> ros2_parent_node_; | ||
}; | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
std::shared_ptr<FusionMatchingStrategy<Msg3D, Msg2D, ExportObj>> parse_matching_strategy( | ||
rclcpp::Node & node); | ||
|
||
} // namespace autoware::image_projection_based_fusion |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,38 @@ | ||
// Copyright 2025 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. | ||
|
||
#pragma once | ||
|
||
#include <autoware_perception_msgs/msg/detected_objects.hpp> | ||
#include <sensor_msgs/msg/camera_info.hpp> | ||
#include <sensor_msgs/msg/image.hpp> | ||
#include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp> | ||
|
||
#include <pcl/point_cloud.h> | ||
#include <pcl/point_types.h> | ||
|
||
namespace autoware::image_projection_based_fusion | ||
{ | ||
using autoware_perception_msgs::msg::DetectedObject; | ||
using autoware_perception_msgs::msg::DetectedObjects; | ||
using sensor_msgs::msg::CameraInfo; | ||
using sensor_msgs::msg::Image; | ||
using PointCloudMsgType = sensor_msgs::msg::PointCloud2; | ||
using RoiMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; | ||
using ClusterMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; | ||
using ClusterObjType = tier4_perception_msgs::msg::DetectedObjectWithFeature; | ||
using tier4_perception_msgs::msg::DetectedObjectWithFeature; | ||
using PointCloud = pcl::PointCloud<pcl::PointXYZ>; | ||
using autoware_perception_msgs::msg::ObjectClassification; | ||
} // namespace autoware::image_projection_based_fusion |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,200 @@ | ||
// Copyright 2024 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. | ||
|
||
#include "autoware/image_projection_based_fusion/fusion_collector.hpp" | ||
|
||
#include "autoware/image_projection_based_fusion/fusion_node.hpp" | ||
#include "autoware/image_projection_based_fusion/fusion_types.hpp" | ||
|
||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <memory> | ||
#include <string> | ||
#include <unordered_map> | ||
#include <utility> | ||
|
||
namespace autoware::image_projection_based_fusion | ||
{ | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
FusionCollector<Msg3D, Msg2D, ExportObj>::FusionCollector( | ||
std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> && ros2_parent_node, double timeout_sec, | ||
bool debug_mode) | ||
: ros2_parent_node_(std::move(ros2_parent_node)), timeout_sec_(timeout_sec), debug_mode_(debug_mode) | ||
{ | ||
const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>( | ||
std::chrono::duration<double>(timeout_sec_)); | ||
|
||
timer_ = | ||
rclcpp::create_timer(ros2_parent_node_, ros2_parent_node_->get_clock(), period_ns, [this]() { | ||
std::lock_guard<std::mutex> fusion_lock(fusion_mutex_); | ||
if (fusion_finished_) return; | ||
fusion_callback(); | ||
}); | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
void FusionCollector<Msg3D, Msg2D, ExportObj>::set_info( | ||
std::shared_ptr<FusionCollectorInfoBase> fusion_collector_info) | ||
{ | ||
fusion_collector_info_ = std::move(fusion_collector_info); | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
std::shared_ptr<FusionCollectorInfoBase> FusionCollector<Msg3D, Msg2D, ExportObj>::get_info() const | ||
{ | ||
return fusion_collector_info_; | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
bool FusionCollector<Msg3D, Msg2D, ExportObj>::process_msg_3d( | ||
const typename Msg3D::ConstSharedPtr msg_3d) | ||
{ | ||
std::lock_guard<std::mutex> fusion_lock(fusion_mutex_); | ||
if (fusion_finished_) return false; | ||
|
||
if (det3d_msg_ != nullptr) { | ||
RCLCPP_WARN_STREAM_THROTTLE( | ||
ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), | ||
std::chrono::milliseconds(10000).count(), | ||
"Pointcloud already exists in the collector. Check the timestamp of the pointcloud."); | ||
} | ||
|
||
det3d_msg_ = msg_3d; | ||
if (ready_to_fuse()) { | ||
fusion_callback(); | ||
} | ||
|
||
return true; | ||
} | ||
Check warning on line 80 in perception/autoware_image_projection_based_fusion/src/fusion_collector.cpp
|
||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
bool FusionCollector<Msg3D, Msg2D, ExportObj>::process_rois( | ||
const std::size_t & roi_id, const typename Msg2D::ConstSharedPtr det2d_msg) | ||
{ | ||
std::lock_guard<std::mutex> fusion_lock(fusion_mutex_); | ||
if (fusion_finished_) return false; | ||
|
||
if (id_to_roi_map_.find(roi_id) != id_to_roi_map_.end()) { | ||
RCLCPP_WARN_STREAM_THROTTLE( | ||
ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), | ||
std::chrono::milliseconds(10000).count(), | ||
"ROIS '" << roi_id << "' already exists in the collector. Check the timestamp of the rois."); | ||
} | ||
id_to_roi_map_[roi_id] = det2d_msg; | ||
if (ready_to_fuse()) { | ||
fusion_callback(); | ||
} | ||
|
||
return true; | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
bool FusionCollector<Msg3D, Msg2D, ExportObj>::ready_to_fuse() | ||
{ | ||
return id_to_roi_map_.size() == rois_number_ && det3d_msg_ != nullptr; | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
bool FusionCollector<Msg3D, Msg2D, ExportObj>::fusion_finished() const | ||
{ | ||
return fusion_finished_; | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
void FusionCollector<Msg3D, Msg2D, ExportObj>::fusion_callback() | ||
{ | ||
// if (debug_mode_) { | ||
// show_debug_message(); | ||
// } | ||
|
||
// All pointcloud and rois are received or the timer has timed out, cancel the timer and fuse | ||
// them. | ||
timer_->cancel(); | ||
|
||
typename Msg3D::SharedPtr output_det3d_msg = std::make_shared<Msg3D>(*det3d_msg_); | ||
ros2_parent_node_->preprocess(*output_det3d_msg); | ||
|
||
for (const auto & [roi_id, roi] : id_to_roi_map_) { | ||
ros2_parent_node_->fuse_on_single_image( | ||
*det3d_msg_, det2d_list_[roi_id], *roi, *output_det3d_msg); | ||
} | ||
|
||
ros2_parent_node_->export_process(output_det3d_msg); | ||
fusion_finished_ = true; | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
bool FusionCollector<Msg3D, Msg2D, ExportObj>::rois_exists(const std::size_t & rois_id) | ||
{ | ||
return id_to_roi_map_.find(rois_id) != id_to_roi_map_.end(); | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
bool FusionCollector<Msg3D, Msg2D, ExportObj>::det3d_exists() | ||
{ | ||
return det3d_msg_ != nullptr; | ||
} | ||
|
||
// void CloudCollector::show_debug_message() | ||
// { | ||
// auto time_until_trigger = timer_->time_until_trigger(); | ||
// std::stringstream log_stream; | ||
// log_stream << std::fixed << std::setprecision(6); | ||
// log_stream << "Collector's fusion callback time: " | ||
// << ros2_parent_node_->get_clock()->now().seconds() << " seconds\n"; | ||
|
||
// if (auto advanced_info = std::dynamic_pointer_cast<AdvancedCollectorInfo>(collector_info_)) { | ||
// log_stream << "Advanced strategy:\n Collector's reference time min: " | ||
// << advanced_info->timestamp - advanced_info->noise_window | ||
// << " to max: " << advanced_info->timestamp + advanced_info->noise_window | ||
// << " seconds\n"; | ||
// } else if (auto naive_info = std::dynamic_pointer_cast<NaiveCollectorInfo>(collector_info_)) { | ||
// log_stream << "Naive strategy:\n Collector's timestamp: " << naive_info->timestamp | ||
// << " seconds\n"; | ||
// } | ||
|
||
// log_stream << "Time until trigger: " << (time_until_trigger.count() / 1e9) << " seconds\n"; | ||
|
||
// log_stream << "Pointclouds: ["; | ||
// std::string separator = ""; | ||
// for (const auto & [topic, cloud] : topic_to_cloud_map_) { | ||
// log_stream << separator; | ||
// log_stream << "[" << topic << ", " << rclcpp::Time(cloud->header.stamp).seconds() << "]"; | ||
// separator = ", "; | ||
// } | ||
|
||
// log_stream << "]\n"; | ||
|
||
// RCLCPP_INFO(ros2_parent_node_->get_logger(), "%s", log_stream.str().c_str()); | ||
// } | ||
|
||
// Explicit instantiation for the supported types | ||
|
||
// pointpainting fusion | ||
template class FusionCollector<PointCloudMsgType, RoiMsgType, DetectedObjects>; | ||
|
||
// roi cluster fusion | ||
template class FusionCollector<ClusterMsgType, RoiMsgType, ClusterMsgType>; | ||
|
||
// roi detected-object fusion | ||
template class FusionCollector<DetectedObjects, RoiMsgType, DetectedObjects>; | ||
|
||
// roi pointcloud fusion | ||
template class FusionCollector<PointCloudMsgType, RoiMsgType, ClusterMsgType>; | ||
|
||
// segment pointcloud fusion | ||
template class FusionCollector<PointCloudMsgType, Image, PointCloudMsgType>; | ||
|
||
} // namespace autoware::image_projection_based_fusion |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,287 @@ | ||
// Copyright 2025 TIER IV, Inc. | ||
Check warning on line 1 in perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp
|
||
// | ||
// 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. | ||
|
||
#include "autoware/image_projection_based_fusion/fusion_matching_strategy.hpp" | ||
|
||
#include "autoware/image_projection_based_fusion/fusion_collector.hpp" | ||
#include "autoware/image_projection_based_fusion/fusion_node.hpp" | ||
#include "autoware/image_projection_based_fusion/fusion_types.hpp" | ||
|
||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <cmath> | ||
#include <cstddef> | ||
#include <list> | ||
#include <memory> | ||
#include <string> | ||
#include <utility> | ||
#include <vector> | ||
|
||
namespace autoware::image_projection_based_fusion | ||
{ | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
NaiveMatchingStrategy<Msg3D, Msg2D, ExportObj>::NaiveMatchingStrategy( | ||
std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> && ros2_parent_node, std::size_t rois_number) | ||
: ros2_parent_node_(std::move(ros2_parent_node)) | ||
{ | ||
auto rois_timestamp_offsets = | ||
ros2_parent_node_->template declare_parameter<std::vector<double>>("rois_timestamp_offsets"); | ||
threshold_ = ros2_parent_node_->template declare_parameter<double>("matching_strategy.threshold"); | ||
|
||
if (rois_timestamp_offsets.size() != rois_number) { | ||
throw std::runtime_error("The number of rois does not match the number of timestamp offsets."); | ||
} | ||
|
||
for (size_t i = 0; i < rois_number; i++) { | ||
id_to_offset_map_[i] = rois_timestamp_offsets[i]; | ||
} | ||
|
||
RCLCPP_INFO( | ||
ros2_parent_node_->get_logger(), "Utilize advanced matching strategy for fusion nodes."); | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> | ||
NaiveMatchingStrategy<Msg3D, Msg2D, ExportObj>::match_rois_to_collector( | ||
const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors, | ||
const std::shared_ptr<RoisMatchingParams> & params) const | ||
{ | ||
std::optional<double> smallest_time_difference = std::nullopt; | ||
std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> closest_collector = nullptr; | ||
|
||
for (const auto & fusion_collector : fusion_collectors) { | ||
if (!fusion_collector->rois_exists(params->rois_id)) { | ||
auto info = fusion_collector->get_info(); | ||
if (auto naive_info = std::dynamic_pointer_cast<NaiveCollectorInfo>(info)) { | ||
double time_difference = std::abs(params->rois_timestamp - naive_info->timestamp); | ||
if ( | ||
!smallest_time_difference || | ||
(time_difference < smallest_time_difference && time_difference < naive_info->threshold)) { | ||
Check warning on line 71 in perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp
|
||
smallest_time_difference = time_difference; | ||
closest_collector = fusion_collector; | ||
} | ||
} | ||
} | ||
} | ||
|
||
if (closest_collector != nullptr) { | ||
return closest_collector; | ||
} | ||
return std::nullopt; | ||
} | ||
Check warning on line 83 in perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp
|
||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> | ||
NaiveMatchingStrategy<Msg3D, Msg2D, ExportObj>::match_det3d_to_collector( | ||
const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors, | ||
const std::shared_ptr<Det3dMatchingParams> & params) const | ||
{ | ||
std::optional<double> smallest_time_difference = std::nullopt; | ||
std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> closest_collector = nullptr; | ||
|
||
for (const auto & fusion_collector : fusion_collectors) { | ||
if (!fusion_collector->det3d_exists()) { | ||
auto info = fusion_collector->get_info(); | ||
if (auto naive_info = std::dynamic_pointer_cast<NaiveCollectorInfo>(info)) { | ||
double time_difference = std::abs(params->det3d_timestamp - naive_info->timestamp); | ||
if ( | ||
!smallest_time_difference || | ||
(time_difference < smallest_time_difference && time_difference < naive_info->threshold)) { | ||
Check warning on line 101 in perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp
|
||
smallest_time_difference = time_difference; | ||
closest_collector = fusion_collector; | ||
} | ||
} | ||
} | ||
} | ||
|
||
if (closest_collector != nullptr) { | ||
return closest_collector; | ||
} | ||
return std::nullopt; | ||
} | ||
Check warning on line 113 in perception/autoware_image_projection_based_fusion/src/fusion_matching_strategy.cpp
|
||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
void NaiveMatchingStrategy<Msg3D, Msg2D, ExportObj>::set_collector_info( | ||
std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> & collector, | ||
const std::shared_ptr<MatchingParamsBase> & matching_params) | ||
{ | ||
if ( | ||
auto det3d_matching_params = std::dynamic_pointer_cast<Det3dMatchingParams>(matching_params)) { | ||
auto info = | ||
std::make_shared<NaiveCollectorInfo>(det3d_matching_params->det3d_timestamp, threshold_); | ||
collector->set_info(info); | ||
} else if ( | ||
auto rois_matching_params = std::dynamic_pointer_cast<RoisMatchingParams>(matching_params)) { | ||
auto info = std::make_shared<NaiveCollectorInfo>( | ||
rois_matching_params->rois_timestamp - id_to_offset_map_.at(rois_matching_params->rois_id), | ||
threshold_); | ||
collector->set_info(info); | ||
} | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
AdvancedMatchingStrategy<Msg3D, Msg2D, ExportObj>::AdvancedMatchingStrategy( | ||
std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> && ros2_parent_node, std::size_t rois_number) | ||
: ros2_parent_node_(std::move(ros2_parent_node)) | ||
{ | ||
auto rois_timestamp_offsets = | ||
ros2_parent_node_->template declare_parameter<std::vector<double>>("rois_timestamp_offsets"); | ||
det3d_noise_window_ = | ||
ros2_parent_node_->template declare_parameter<double>("matching_strategy.det3d_noise_window"); | ||
auto rois_timestamp_noise_window = | ||
ros2_parent_node_->template declare_parameter<std::vector<double>>( | ||
"matching_strategy.rois_timestamp_noise_window"); | ||
|
||
if (rois_timestamp_offsets.size() != rois_number) { | ||
throw std::runtime_error("The number of rois does not match the number of timestamp offsets."); | ||
} | ||
if (rois_timestamp_noise_window.size() != rois_number) { | ||
throw std::runtime_error( | ||
"The number of rois does not match the number of timestamp noise window."); | ||
} | ||
|
||
for (size_t i = 0; i < rois_number; i++) { | ||
id_to_offset_map_[i] = rois_timestamp_offsets[i]; | ||
id_to_noise_window_map_[i] = rois_timestamp_noise_window[i]; | ||
} | ||
|
||
RCLCPP_INFO( | ||
ros2_parent_node_->get_logger(), "Utilize advanced matching strategy for fusion nodes."); | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> | ||
AdvancedMatchingStrategy<Msg3D, Msg2D, ExportObj>::match_rois_to_collector( | ||
const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors, | ||
const std::shared_ptr<RoisMatchingParams> & params) const | ||
{ | ||
for (const auto & fusion_collector : fusion_collectors) { | ||
auto info = fusion_collector->get_info(); | ||
if (auto advanced_info = std::dynamic_pointer_cast<AdvancedCollectorInfo>(info)) { | ||
auto reference_timestamp_min = advanced_info->timestamp - advanced_info->noise_window; | ||
auto reference_timestamp_max = advanced_info->timestamp + advanced_info->noise_window; | ||
double time = params->rois_timestamp - id_to_offset_map_.at(params->rois_id); | ||
if ( | ||
time < reference_timestamp_max + id_to_noise_window_map_.at(params->rois_id) && | ||
time > reference_timestamp_min - id_to_noise_window_map_.at(params->rois_id)) { | ||
return fusion_collector; | ||
} | ||
} | ||
} | ||
return std::nullopt; | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
std::optional<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> | ||
AdvancedMatchingStrategy<Msg3D, Msg2D, ExportObj>::match_det3d_to_collector( | ||
const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors, | ||
const std::shared_ptr<Det3dMatchingParams> & params) const | ||
{ | ||
auto concatenated_status = ros2_parent_node_->find_concatenation_status(params->det3d_timestamp); | ||
|
||
// TODO(vivid): double check this logic and fix the name | ||
double offset = get_offset(params->det3d_timestamp, concatenated_status); | ||
|
||
for (const auto & fusion_collector : fusion_collectors) { | ||
auto info = fusion_collector->get_info(); | ||
if (auto advanced_info = std::dynamic_pointer_cast<AdvancedCollectorInfo>(info)) { | ||
auto reference_timestamp_min = advanced_info->timestamp - advanced_info->noise_window; | ||
auto reference_timestamp_max = advanced_info->timestamp + advanced_info->noise_window; | ||
if ( | ||
params->det3d_timestamp - offset < reference_timestamp_max + det3d_noise_window_ && | ||
params->det3d_timestamp - offset > reference_timestamp_min - det3d_noise_window_) { | ||
return fusion_collector; | ||
} | ||
} | ||
} | ||
return std::nullopt; | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
void AdvancedMatchingStrategy<Msg3D, Msg2D, ExportObj>::set_collector_info( | ||
std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> & collector, | ||
const std::shared_ptr<MatchingParamsBase> & matching_params) | ||
{ | ||
if ( | ||
auto det3d_matching_params = std::dynamic_pointer_cast<Det3dMatchingParams>(matching_params)) { | ||
auto concatenated_status = | ||
ros2_parent_node_->find_concatenation_status(det3d_matching_params->det3d_timestamp); | ||
double offset = get_offset(det3d_matching_params->det3d_timestamp, concatenated_status); | ||
|
||
auto info = std::make_shared<AdvancedCollectorInfo>( | ||
det3d_matching_params->det3d_timestamp - offset, det3d_noise_window_); | ||
collector->set_info(info); | ||
} else if ( | ||
auto rois_matching_params = std::dynamic_pointer_cast<RoisMatchingParams>(matching_params)) { | ||
auto info = std::make_shared<AdvancedCollectorInfo>( | ||
rois_matching_params->rois_timestamp - id_to_offset_map_[rois_matching_params->rois_id], | ||
id_to_noise_window_map_[rois_matching_params->rois_id]); | ||
collector->set_info(info); | ||
} | ||
} | ||
|
||
template <class Msg3D, class Msg2D, class ExportObj> | ||
double AdvancedMatchingStrategy<Msg3D, Msg2D, ExportObj>::get_offset( | ||
const double & det3d_timestamp, | ||
const std::optional<std::unordered_map<std::string, std::string>> & concatenated_status) const | ||
{ | ||
double offset = 0.0; | ||
if (concatenated_status) { | ||
auto status_map = concatenated_status.value(); // Retrieve the inner map | ||
if ( | ||
status_map["cloud_concatenation_success"] == "False" && | ||
det3d_timestamp > std::stod(status_map["reference_timestamp_max"])) { | ||
// The defined earliest pointcloud is missed in the concatenation of pointcloud | ||
offset = det3d_timestamp - (std::stod(status_map["reference_timestamp_min"]) + | ||
(std::stod(status_map["reference_timestamp_max"]) - | ||
std::stod(status_map["reference_timestamp_min"])) / | ||
2); | ||
} | ||
} | ||
|
||
return offset; | ||
} | ||
|
||
// pointpainting fusion | ||
template class NaiveMatchingStrategy<PointCloudMsgType, RoiMsgType, DetectedObjects>; | ||
|
||
// roi cluster fusion | ||
template class NaiveMatchingStrategy<ClusterMsgType, RoiMsgType, ClusterMsgType>; | ||
|
||
// roi detected-object fusion | ||
template class NaiveMatchingStrategy<DetectedObjects, RoiMsgType, DetectedObjects>; | ||
|
||
// roi pointcloud fusion | ||
template class NaiveMatchingStrategy<PointCloudMsgType, RoiMsgType, ClusterMsgType>; | ||
|
||
// segment pointcloud fusion | ||
template class NaiveMatchingStrategy<PointCloudMsgType, Image, PointCloudMsgType>; | ||
|
||
// pointpainting fusion | ||
template class AdvancedMatchingStrategy<PointCloudMsgType, RoiMsgType, DetectedObjects>; | ||
|
||
// roi cluster fusion | ||
template class AdvancedMatchingStrategy<ClusterMsgType, RoiMsgType, ClusterMsgType>; | ||
|
||
// roi detected-object fusion | ||
template class AdvancedMatchingStrategy<DetectedObjects, RoiMsgType, DetectedObjects>; | ||
|
||
// roi pointcloud fusion | ||
template class AdvancedMatchingStrategy<PointCloudMsgType, RoiMsgType, ClusterMsgType>; | ||
|
||
// segment pointcloud fusion | ||
template class AdvancedMatchingStrategy<PointCloudMsgType, Image, PointCloudMsgType>; | ||
|
||
} // namespace autoware::image_projection_based_fusion |
Large diffs are not rendered by default.