Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(autoware_image_based_projection_fusion): redesign image based projection fusion node #10016

Draft
wants to merge 8 commits into
base: main
Choose a base branch
from
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,8 @@ endif()
# Build non-CUDA dependent nodes
ament_auto_add_library(${PROJECT_NAME} SHARED
src/camera_projection.cpp
src/fusion_collector.cpp
src/fusion_matching_strategy.cpp
src/fusion_node.cpp
src/debugger.cpp
src/utils/geometry.cpp
Expand Down
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
/**:
ros__parameters:
input_offset_ms: [61.67, 111.67, 45.0, 28.33, 78.33, 95.0]
timeout_ms: 70.0
match_threshold_ms: 50.0
rois_timestamp_offsets: [0.06167, 0.011167, 0.045, 0.02833, 0.07833, 0.095]
timeout_sec: 0.070
# match_threshold_ms: 50.0
image_buffer_size: 15
# projection setting for each ROI whether unrectify image
point_project_to_unrectified_image: [false, false, false, false, false, false]
Expand All @@ -22,3 +22,7 @@

# debug parameters
publish_processing_time_detail: false
matching_strategy:
type: advanced
det3d_noise_window: 0.01
rois_timestamp_noise_window: [0.005, 0.005, 0.005, 0.005, 0.005, 0.005]
Original file line number Diff line number Diff line change
@@ -0,0 +1,114 @@
// 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/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::shared_ptr<CameraProjection> camera_projector_ptr{nullptr};
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)
{
}
};

enum class CollectorStatus { Idle, Processing, Finished };

template <class Msg3D, class Msg2D, class ExportObj>
class FusionCollector
{
public:
FusionCollector(
std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> && ros2_parent_node,
std::size_t rois_number, const std::vector<Det2dStatus<Msg2D>> & det2d_status_list,
bool debug_mode);
bool process_msg3d(const typename Msg3D::ConstSharedPtr msg3d, double msg3d_timeout);
bool process_rois(
const std::size_t & rois_id, const typename Msg2D::ConstSharedPtr det2d_msg,
double det2d_timeout);
void fusion_callback();

[[nodiscard]] CollectorStatus get_status() 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 msg3d_exists();
void show_debug_message();
void reset();
void set_period(const std::chrono::nanoseconds period);

private:
std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> ros2_parent_node_;
rclcpp::TimerBase::SharedPtr timer_;
std::size_t rois_number_;
typename Msg3D::ConstSharedPtr msg3d_{nullptr};
std::vector<Det2dStatus<Msg2D>> det2d_status_list_;
std::unordered_map<std::size_t, typename Msg2D::ConstSharedPtr> id_to_rois_map_;
bool is_first_msg3d_{false};
bool debug_mode_;
std::mutex fusion_mutex_;
std::shared_ptr<FusionCollectorInfoBase> fusion_collector_info_;
CollectorStatus status_;
};

} // namespace autoware::image_projection_based_fusion
Original file line number Diff line number Diff line change
@@ -0,0 +1,145 @@
// 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 <set>
#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 Msg3dMatchingParams : public MatchingParamsBase
{
double msg3d_timestamp;

explicit Msg3dMatchingParams(double msg3d_timestamp = 0.0) : msg3d_timestamp(msg3d_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_msg3d_to_collector(
const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors,
const std::shared_ptr<Msg3dMatchingParams> & params) = 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,
const std::unordered_map<std::size_t, double> & id_to_offset_map);

[[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_msg3d_to_collector(
const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors,
const std::shared_ptr<Msg3dMatchingParams> & params) override;

void set_collector_info(
std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>> & collector,
const std::shared_ptr<MatchingParamsBase> & matching_params) override;

private:
std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> ros2_parent_node_;
std::unordered_map<std::size_t, double> id_to_offset_map_;
double threshold_;
};

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,
const std::unordered_map<std::size_t, double> & id_to_offset_map);

[[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_msg3d_to_collector(
const std::list<std::shared_ptr<FusionCollector<Msg3D, Msg2D, ExportObj>>> & fusion_collectors,
const std::shared_ptr<Msg3dMatchingParams> & params) 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 & msg3d_timestamp,
const std::optional<std::unordered_map<std::string, std::string>> & concatenated_status);

double extract_fractional(double timestamp);
void update_fractional_timestamp_set(double new_timestamp);
double compute_offset(double input_timestamp);

private:
std::shared_ptr<FusionNode<Msg3D, Msg2D, ExportObj>> ros2_parent_node_;
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::set<double> fractional_timestamp_set_; // Use set to store unique fractional timestamps
int success_status_counter_{0};
static constexpr int success_threshold{100};
bool database_created_{false};
};
} // namespace autoware::image_projection_based_fusion
Loading
Loading