diff --git a/.bazelrc b/.bazelrc index 31463cb8..fea8522f 100644 --- a/.bazelrc +++ b/.bazelrc @@ -17,7 +17,7 @@ common --enable_bzlmod common --distdir=.cache/distdir # Prefer https://bcr.bazel.build/ -#build --registry=file:///apollo/data/bazel-central-registry +# build --registry=file:///apollo/data/bazel-central-registry build --registry=https://bcr.wheelos.cn/ build --registry=https://bcr.bazel.build/ diff --git a/modules/drivers/lidar/compensator/compensator.cc b/modules/drivers/lidar/compensator/compensator.cc index 6b39eb5b..0db77b32 100644 --- a/modules/drivers/lidar/compensator/compensator.cc +++ b/modules/drivers/lidar/compensator/compensator.cc @@ -32,7 +32,7 @@ bool Compensator::QueryPoseAffineFromTF2(const uint64_t& timestamp, void* pose, Eigen::Affine3d* tmp_pose = static_cast(pose); std::string err_string; if (!transform_query_.LookupTransformToAffine( - config_.world_frame_id(), child_frame_id, query_time, tmp_pose, + config_.map_frame_id(), child_frame_id, query_time, tmp_pose, config_.transform_query_timeout(), &err_string)) { AERROR << "Can not find transform. " << timestamp << " frame_id:" << child_frame_id << " Error info: " << err_string; diff --git a/modules/drivers/lidar/compensator/conf/lidar_compensator_config.pb.txt b/modules/drivers/lidar/compensator/conf/lidar_compensator_config.pb.txt index f6fe2cec..3563a532 100644 --- a/modules/drivers/lidar/compensator/conf/lidar_compensator_config.pb.txt +++ b/modules/drivers/lidar/compensator/conf/lidar_compensator_config.pb.txt @@ -1,3 +1,3 @@ output_channel: "/apollo/sensor/lidar/compensator/PointCloud2" transform_query_timeout: 0.02 -world_frame_id: "map" +map_frame_id: "map" diff --git a/modules/drivers/lidar/compensator/proto/lidar_compensator_config.proto b/modules/drivers/lidar/compensator/proto/lidar_compensator_config.proto index fc43cabc..66bcfcfc 100644 --- a/modules/drivers/lidar/compensator/proto/lidar_compensator_config.proto +++ b/modules/drivers/lidar/compensator/proto/lidar_compensator_config.proto @@ -5,7 +5,7 @@ package apollo.drivers.lidar; message LidarCompensatorConfig { optional string output_channel = 1; optional float transform_query_timeout = 2 [default = 0.02]; - optional string world_frame_id = 3 [default = "map"]; + optional string map_frame_id = 3 [default = "map"]; optional string target_frame_id = 4; optional uint32 point_cloud_size = 5; optional uint64 reserve_point_cloud_size = 6 [default = 500000]; diff --git a/modules/drivers/lidar/livox/component/livox_component.cpp b/modules/drivers/lidar/livox/component/livox_component.cpp index 5d2d1ce3..8303bb23 100644 --- a/modules/drivers/lidar/livox/component/livox_component.cpp +++ b/modules/drivers/lidar/livox/component/livox_component.cpp @@ -16,6 +16,7 @@ #include "modules/drivers/lidar/livox/component/livox_component.h" +#include #include #include @@ -25,16 +26,64 @@ namespace lidar { static LivoxLidarComponent* g_livox_component = nullptr; -uint64_t GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, - uint8_t size) { +uint64_t LivoxLidarComponent::GetEthPacketTimestamp(uint8_t handle, + uint8_t timestamp_type, + uint8_t* time_stamp, + uint8_t size) { LdsStamp time; memcpy(time.stamp_bytes, time_stamp, size); + uint64_t packet_timestamp = time.stamp; + uint64_t system_time_ns = cyber::Time::Now().ToNanosecond(); + + // Default is use_lidar_clock=true, check if explicitly disabled + bool use_lidar_clock = config_.has_use_lidar_clock() ? + config_.use_lidar_clock() : true; + + // If use_lidar_clock is false, always use system time + // Only warn once since this is the user's explicit configuration + if (!use_lidar_clock) { + if (!warned_not_use_lidar_clock_) { + AWARN << "[Livox Timestamp] use_lidar_clock is DISABLED. " + << "Using system time (data arrival time) instead of lidar clock. " + << "This means measurement_time will NOT reflect laser scan time!"; + warned_not_use_lidar_clock_ = true; + } + return system_time_ns; + } + + // use_lidar_clock=true: Try to use packet timestamp (laser scan time) + double max_diff_s = config_.has_max_timestamp_diff_s() ? + config_.max_timestamp_diff_s() : 10.0; + uint64_t max_diff_ns = static_cast(max_diff_s * 1e9); + + // apply timestamp offset + if (config_.timestamp_offset() != 0) { + packet_timestamp += static_cast(config_.timestamp_offset() * 1e9); + } + // Calculate absolute difference + uint64_t diff_ns; + if (packet_timestamp > system_time_ns) { + diff_ns = packet_timestamp - system_time_ns; + } else { + diff_ns = system_time_ns - packet_timestamp; + } - if (timestamp_type == kTimestampTypeGptpOrPtp || - timestamp_type == kTimestampTypeGps) { - return time.stamp; + // If diff is too large, packet time is likely not synchronized + if (diff_ns > max_diff_ns) { + double packet_time_s = packet_timestamp / 1e9; + double system_time_s = system_time_ns / 1e9; + double diff_s = static_cast(diff_ns) / 1e9; + AWARN_EVERY(10000) << std::fixed << std::setprecision(3) + << "[Livox Ts] Handle=" << static_cast(handle) + << " type=" << static_cast(timestamp_type) + << " pkt_ts=" << packet_time_s + << " sys_ts=" << system_time_s + << " diff=" << diff_s << "s > " << max_diff_s << "s, using sys time"; + return system_time_ns; } - return cyber::Time::Now().ToNanosecond(); + + // Use packet timestamp (lidar clock / laser scan time) + return packet_timestamp; } void LivoxLidarComponent::BinaryDataProcess(const unsigned char* data, @@ -98,14 +147,14 @@ void LivoxLidarComponent::PointCloudCallback(uint8_t handle, return; } - AINFO << boost::format( - "point cloud handle: %d, data_num: %d, data_type: %d\n") % - static_cast(handle) % data_num % - static_cast(data->data_type); + ADEBUG << boost::format( + "point cloud handle: %d, data_num: %d, data_type: %d") % + static_cast(handle) % data_num % + static_cast(data->data_type); size_t byte_size = g_livox_component->GetEthPacketByteSize(data, data_num); - uint64_t pkt_timestamp = GetEthPacketTimestamp( - data->timestamp_type, data->timestamp, sizeof(data->timestamp)); + uint64_t pkt_timestamp = g_livox_component->GetEthPacketTimestamp( + handle, data->timestamp_type, data->timestamp, sizeof(data->timestamp)); g_livox_component->BinaryDataProcess(data->data, data->data_type, data_num, pkt_timestamp); diff --git a/modules/drivers/lidar/livox/component/livox_component.h b/modules/drivers/lidar/livox/component/livox_component.h index 8bf793cd..1c10ed6f 100644 --- a/modules/drivers/lidar/livox/component/livox_component.h +++ b/modules/drivers/lidar/livox/component/livox_component.h @@ -49,9 +49,6 @@ typedef union { int64_t stamp; } LdsStamp; -uint64_t GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, - uint8_t size); - class LivoxLidarComponent final : public LidarComponentBase { public: void BinaryDataProcess(const unsigned char* data, int data_type, @@ -68,6 +65,11 @@ class LivoxLidarComponent final : public LidarComponentBase { static void OnDeviceStateUpdate(const DeviceInfo* device, DeviceEvent event); static void OnLidarStartSamplingCb(livox_status status, uint8_t handle, uint8_t response, void* client_data); + + private: + uint64_t GetEthPacketTimestamp(uint8_t handle, uint8_t timestamp_type, + uint8_t* time_stamp, uint8_t size); + livox::Config config_; std::deque integral_queue_; uint64_t last_pointcloud_pub_timestamp_{0}; @@ -75,6 +77,7 @@ class LivoxLidarComponent final : public LidarComponentBase { double integral_time_ = {0.4}; uint8_t lidar_handle_ = {0}; bool sdk_initialized_ = {false}; + bool warned_not_use_lidar_clock_ = {false}; }; CYBER_REGISTER_COMPONENT(LivoxLidarComponent) } // namespace lidar diff --git a/modules/drivers/lidar/livox/proto/livox.proto b/modules/drivers/lidar/livox/proto/livox.proto index ff290a8a..8497fc8c 100644 --- a/modules/drivers/lidar/livox/proto/livox.proto +++ b/modules/drivers/lidar/livox/proto/livox.proto @@ -9,6 +9,30 @@ message Config { optional string broadcast_code = 2; optional double integral_time = 3 [default = 0.1]; optional bool enable_sdk_console_log = 4 [default = false]; + // Use lidar clock (packet timestamp) instead of system time + // When true, use lidar hardware timestamp (laser scan time) + // When false, use system time (data arrival time) + // Default: true (use lidar clock for accurate laser scan time) + optional bool use_lidar_clock = 5 [default = true]; + + // Max allowed diff between packet timestamp and system time (seconds) + // If the diff exceeds this value and use_lidar_clock is true, + // fall back to system time and warn about time sync issue + // This helps detect when lidar hardware time is not synchronized + // + // Livox timestamp sync modes (timestamp_type in packet): + // 0 = NoSync - No sync signal mode (may have time drift) + // 1 = PTP - 1588v2.0 PTP sync mode + // 2 = Reserved - Reserved for future use + // 3 = PPS+GPS - PPS (Pulse Per Second) + GPS sync mode + // 4 = PPS - PPS only sync mode + // 5 = Unknown - Unknown sync mode + // + // Default: 10.0 seconds + optional double max_timestamp_diff_s = 6 [default = 10.0]; + + // timestamp offset in seconds, applyed to point cloud timestamp + optional double timestamp_offset = 7 [default = 0]; } message LivoxScan { diff --git a/modules/drivers/lidar/processor/BUILD b/modules/drivers/lidar/processor/BUILD new file mode 100644 index 00000000..a4677448 --- /dev/null +++ b/modules/drivers/lidar/processor/BUILD @@ -0,0 +1,156 @@ +load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library", "cc_test") +load("@rules_cuda//cuda:defs.bzl", "cuda_library") +load("//tools:cpplint.bzl", "cpplint") + +package(default_visibility=["//visibility:public"]) + +config_setting( + name = "lidar_gpu_backend_enabled", + values = {"define": "lidar_gpu_backend=true"}, +) + +config_setting( + name = "lidar_mode_force_cpu", + values = {"define": "lidar_policy_mode=cpu"}, +) + +config_setting( + name = "lidar_mode_force_gpu", + values = {"define": "lidar_policy_mode=gpu"}, +) + +cc_binary( + name="liblidar_unified_component.so", + linkshared=True, + linkstatic=True, + deps=[":lidar_unified_component_lib"], +) + +cuda_library( + name = "lidar_policy_cuda_kernels_lib", + srcs = ["policy/lidar_policy_cuda_kernels.cu"], + hdrs = ["policy/lidar_policy_cuda_kernels.h"], + deps = [ + "@cuda//:cuda_runtime", + ], +) + +cc_library( + name="lidar_policy_lib", + srcs=[ + "policy/lidar_policy_common.cc", + "policy/lidar_policy_cpu_deskew.cc", + "policy/lidar_policy_cpu_filter.cc", + "policy/lidar_policy_cpu_fusion.cc", + "policy/lidar_policy_factory.cc", + "policy/lidar_policy_gpu_deskew.cc", + "policy/lidar_policy_gpu_filter.cc", + "policy/lidar_policy_gpu_fusion.cc", + ], + hdrs=[ + "policy/cpu_lidar_policy.h", + "policy/gpu_lidar_policy.h", + "policy/lidar_policy_common.h", + "policy/lidar_policy_interface.h", + "policy/lidar_policy_cuda_kernels.h", + ], + copts = select({ + ":lidar_gpu_backend_enabled": ["-DAPOLLO_LIDAR_POLICY_GPU_ENABLED"], + "//conditions:default": [], + }) + select({ + ":lidar_mode_force_cpu": ["-DAPOLLO_LIDAR_POLICY_FORCE_CPU"], + ":lidar_mode_force_gpu": ["-DAPOLLO_LIDAR_POLICY_FORCE_GPU"], + "//conditions:default": [], + }), + deps=[ + "//cyber", + "//modules/common_msgs/sensor_msgs:pointcloud_cc_proto", + "//modules/drivers/lidar/proto:lidar_unified_component_config_cc_proto", + "//modules/transform:buffer", + "//modules/transform:transform_query_core", + "@eigen", + ] + select({ + ":lidar_gpu_backend_enabled": [":lidar_policy_cuda_kernels_lib"], + "//conditions:default": [], + }), +) + +cc_library( + name="safety_policy_lib", + srcs=[ + "safety/degrade_policy.cc", + "safety/dtc_reporter.cc", + "safety/ts_sanity.cc", + ], + hdrs=[ + "safety/degrade_policy.h", + "safety/dtc_reporter.h", + "safety/ts_sanity.h", + ], + deps=[ + "//cyber", + ], +) + +cc_library( + name="lidar_unified_component_lib", + srcs=[ + "control/pose_bins_builder.cc", + "control/sync_gate.cc", + "lidar_unified_component.cc", + "lidar_unified_component_config.cc", + "lidar_unified_component_ingest.cc", + "lidar_unified_component_observability.cc", + ], + hdrs=[ + "control/frame_handle.h", + "control/pose_bins_builder.h", + "control/sync_gate.h", + "lidar_unified_component.h", + "policy/cpu_lidar_policy.h", + "policy/gpu_lidar_policy.h", + "policy/lidar_policy_interface.h", + ], + copts=['-DMODULE_NAME=\\"lidar_unified_component\\"'] + select({ + ":lidar_mode_force_cpu": ["-DAPOLLO_LIDAR_POLICY_FORCE_CPU"], + ":lidar_mode_force_gpu": ["-DAPOLLO_LIDAR_POLICY_FORCE_GPU"], + "//conditions:default": [], + }), + deps=[ + "//cyber", + "//modules/common_msgs/sensor_msgs:pointcloud_cc_proto", + "//modules/drivers/lidar/proto:lidar_unified_component_config_cc_proto", + ":lidar_policy_lib", + ":safety_policy_lib", + "//modules/transform:buffer", + "//modules/transform:timed_transform_resolver", + "@boost.circular_buffer", + "@eigen", + ], + alwayslink=True, +) + +cc_test( + name="lidar_policy_test", + size="small", + srcs=[ + "lidar_policy_test.cc", + "lidar_policy_test_main.cc", + ], + copts = select({ + ":lidar_gpu_backend_enabled": ["-DAPOLLO_LIDAR_POLICY_GPU_ENABLED"], + "//conditions:default": [], + }) + select({ + ":lidar_mode_force_cpu": ["-DAPOLLO_LIDAR_POLICY_FORCE_CPU"], + ":lidar_mode_force_gpu": ["-DAPOLLO_LIDAR_POLICY_FORCE_GPU"], + "//conditions:default": [], + }), + deps=[ + ":lidar_unified_component_lib", + ":lidar_policy_lib", + "//modules/common_msgs/transform_msgs:transform_cc_proto", + "@com_google_googletest//:gtest", + ], +) + +cpplint() diff --git a/modules/drivers/lidar/processor/conf/lidar_unified_component_config.pb.txt b/modules/drivers/lidar/processor/conf/lidar_unified_component_config.pb.txt new file mode 100644 index 00000000..25b837b2 --- /dev/null +++ b/modules/drivers/lidar/processor/conf/lidar_unified_component_config.pb.txt @@ -0,0 +1,46 @@ +output_channel: "/apollo/sensor/lidar/unified/PointCloud2" + +auxiliary_lidar_inputs { + topic_name: "/apollo/sensor/rslidar/up/PointCloud2" +} + +sensor_buffer_size: 16 +max_full_pointcloud_points: 600000 +worker_num: 0 + +base_link_frame_id: "base_link" +world_frame_id: "world" + +enable_ego_query_filter: true +ego_box_forward_x: 2.8 +ego_box_backward_x: -2.8 +ego_box_forward_y: 1.4 +ego_box_backward_y: -1.4 + +voxel_size: 0.15 +max_ref_time_delta_ms: 80 +motion_compensation_bins: 12 +metrics_log_interval: 50 +strict_auxiliary_sync: false +sensor_pose_cache_duration_sec: 1.0 +sensor_pose_cache_max_extrapolation_sec: 0.15 +sensor_pose_query_timeout_sec: 0.02 +fixed_delay_ema_alpha: 0.2 +fixed_delay_update_limit_ms: 80.0 +clock_offset_ema_alpha: 0.2 +overlap_region_forward_x: 25.0 +overlap_region_backward_x: -5.0 +overlap_region_left_y: 12.0 +overlap_region_right_y: -12.0 +overlap_region_min_z: -3.0 +overlap_region_max_z: 3.0 +overlap_quality_ema_alpha: 0.2 +auxiliary_min_overlap_quality_weight: 0.1 +overlap_quality_sample_stride: 16 + +ts_sanity_enabled: true +ts_sanity_min_interval_ms: 40 +ts_sanity_max_interval_ms: 200 +ts_sanity_max_jump_ms: 500 +ts_sanity_max_consecutive_errors: 3 +degrade_on_ts_anomaly: true diff --git a/modules/drivers/lidar/processor/control/frame_handle.h b/modules/drivers/lidar/processor/control/frame_handle.h new file mode 100644 index 00000000..f4f8f7b8 --- /dev/null +++ b/modules/drivers/lidar/processor/control/frame_handle.h @@ -0,0 +1,35 @@ +#pragma once + +#include +#include +#include + +#include "Eigen/Geometry" + +#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +struct BufferedFrame { + std::shared_ptr point_cloud; + std::vector motion_sample_times; + std::vector motion_poses; + bool pose_prefetch_ok = false; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct FrameHandle { + std::string sensor_id; + std::shared_ptr point_cloud; + std::shared_ptr buffered_frame; + bool is_primary = false; + double clock_offset_residual_ms = 0.0; + double overlap_quality_weight = 1.0; +}; + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/control/pose_bins_builder.cc b/modules/drivers/lidar/processor/control/pose_bins_builder.cc new file mode 100644 index 00000000..3302a6aa --- /dev/null +++ b/modules/drivers/lidar/processor/control/pose_bins_builder.cc @@ -0,0 +1,82 @@ +#include "modules/drivers/lidar/processor/control/pose_bins_builder.h" + +#include "cyber/cyber.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +bool PoseBinsBuilder::Build( + const std::vector& frame_handles, + LidarDeskewPolicy* deskew_policy, std::vector* contexts, + std::vector>* motion_sample_times, + std::vector>* motion_poses, + size_t* required_points) const { + if (contexts == nullptr || motion_sample_times == nullptr || + motion_poses == nullptr || required_points == nullptr) { + return false; + } + + contexts->clear(); + motion_sample_times->clear(); + motion_poses->clear(); + *required_points = 0; + + contexts->reserve(frame_handles.size()); + motion_sample_times->reserve(frame_handles.size()); + motion_poses->reserve(frame_handles.size()); + + for (const auto& handle : frame_handles) { + if (handle.point_cloud == nullptr) { + if (handle.is_primary) { + AERROR << "Main sensor frame is null: " << handle.sensor_id; + return false; + } + AWARN << "Skip auxiliary sensor due to null frame: " << handle.sensor_id; + continue; + } + + SensorFrameContext context; + context.sensor_id = handle.sensor_id; + context.point_cloud = handle.point_cloud; + context.is_primary = handle.is_primary; + + std::vector sample_times; + std::vector poses; + if (handle.buffered_frame != nullptr && + handle.buffered_frame->pose_prefetch_ok && + !handle.buffered_frame->motion_sample_times.empty() && + handle.buffered_frame->motion_sample_times.size() == + handle.buffered_frame->motion_poses.size()) { + sample_times = handle.buffered_frame->motion_sample_times; + poses = handle.buffered_frame->motion_poses; + } else if (deskew_policy != nullptr && + deskew_policy->ComputeMotionCompensationPoses( + context, &sample_times, &poses) && + !sample_times.empty() && !poses.empty() && + sample_times.size() == poses.size()) { + // Legacy fallback for call sites that have not migrated to prefetched + // motion samples yet. + } else { + if (handle.is_primary) { + AERROR << "Failed to compute motion compensation poses for main sensor " + << handle.sensor_id; + return false; + } + AWARN << "Skip auxiliary sensor due to invalid motion compensation data: " + << handle.sensor_id; + continue; + } + + contexts->push_back(std::move(context)); + motion_sample_times->push_back(std::move(sample_times)); + motion_poses->push_back(std::move(poses)); + *required_points += static_cast(handle.point_cloud->point_size()); + } + + return !contexts->empty(); +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/control/pose_bins_builder.h b/modules/drivers/lidar/processor/control/pose_bins_builder.h new file mode 100644 index 00000000..d5902ed1 --- /dev/null +++ b/modules/drivers/lidar/processor/control/pose_bins_builder.h @@ -0,0 +1,27 @@ +#pragma once + +#include +#include + +#include "Eigen/Eigen" + +#include "modules/drivers/lidar/processor/control/frame_handle.h" +#include "modules/drivers/lidar/processor/policy/lidar_policy_interface.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +class PoseBinsBuilder { + public: + bool Build(const std::vector& frame_handles, + LidarDeskewPolicy* deskew_policy, + std::vector* contexts, + std::vector>* motion_sample_times, + std::vector>* motion_poses, + size_t* required_points) const; +}; + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/control/sync_gate.cc b/modules/drivers/lidar/processor/control/sync_gate.cc new file mode 100644 index 00000000..acc8d74f --- /dev/null +++ b/modules/drivers/lidar/processor/control/sync_gate.cc @@ -0,0 +1,81 @@ +#include "modules/drivers/lidar/processor/control/sync_gate.h" + +#include "cyber/cyber.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +bool SyncGate::SelectFrames(double ref_timestamp, + const std::string& primary_sensor_id, + const std::vector& auxiliary_topics, + uint32_t max_ref_time_delta_ms, + bool strict_auxiliary_sync, + const ResolveSensorIdByTopicFn& resolve_sensor_id, + const LookupNearestFrameFn& lookup_nearest_frame, + std::vector* frame_handles, + SyncGateMetrics* metrics) const { + if (primary_sensor_id.empty() || !resolve_sensor_id || + !lookup_nearest_frame || frame_handles == nullptr || metrics == nullptr) { + return false; + } + + frame_handles->clear(); + metrics->expected_sensor_count = 1 + auxiliary_topics.size(); + metrics->matched_sensor_count = 0; + metrics->missing_auxiliary_count = 0; + metrics->time_delta_exceeded_count = 0; + + FrameHandle primary_handle; + bool primary_time_delta_exceeded = false; + if (!lookup_nearest_frame(primary_sensor_id, ref_timestamp, + max_ref_time_delta_ms, &primary_handle, + &primary_time_delta_exceeded)) { + AERROR << "Primary sensor frame unavailable around reference timestamp"; + return false; + } + primary_handle.sensor_id = primary_sensor_id; + primary_handle.is_primary = true; + frame_handles->push_back(primary_handle); + + for (const auto& topic_name : auxiliary_topics) { + std::string sensor_id; + if (!resolve_sensor_id(topic_name, &sensor_id) || sensor_id.empty()) { + ++metrics->missing_auxiliary_count; + if (strict_auxiliary_sync) { + AERROR << "Auxiliary topic has not resolved sensor id yet: " + << topic_name; + return false; + } + continue; + } + + FrameHandle nearest_handle; + bool time_delta_exceeded = false; + if (!lookup_nearest_frame(sensor_id, ref_timestamp, max_ref_time_delta_ms, + &nearest_handle, &time_delta_exceeded)) { + ++metrics->missing_auxiliary_count; + if (time_delta_exceeded) { + ++metrics->time_delta_exceeded_count; + } + if (strict_auxiliary_sync) { + AERROR << "Auxiliary sensor sync failed: " << sensor_id; + return false; + } + AWARN << "Skip auxiliary sensor " << sensor_id + << " due to sync miss, topic=" << topic_name; + continue; + } + + nearest_handle.sensor_id = sensor_id; + nearest_handle.is_primary = false; + frame_handles->push_back(nearest_handle); + } + + metrics->matched_sensor_count = frame_handles->size(); + return !frame_handles->empty(); +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/control/sync_gate.h b/modules/drivers/lidar/processor/control/sync_gate.h new file mode 100644 index 00000000..bc9e5477 --- /dev/null +++ b/modules/drivers/lidar/processor/control/sync_gate.h @@ -0,0 +1,42 @@ +#pragma once + +#include +#include +#include +#include + +#include "modules/drivers/lidar/processor/control/frame_handle.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +struct SyncGateMetrics { + size_t expected_sensor_count = 0; + size_t matched_sensor_count = 0; + size_t missing_auxiliary_count = 0; + size_t time_delta_exceeded_count = 0; +}; + +class SyncGate { + public: + using ResolveSensorIdByTopicFn = std::function; + + using LookupNearestFrameFn = + std::function; + + bool SelectFrames(double ref_timestamp, const std::string& primary_sensor_id, + const std::vector& auxiliary_topics, + uint32_t max_ref_time_delta_ms, bool strict_auxiliary_sync, + const ResolveSensorIdByTopicFn& resolve_sensor_id, + const LookupNearestFrameFn& lookup_nearest_frame, + std::vector* frame_handles, + SyncGateMetrics* metrics) const; +}; + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/dag/lidar_unified_processor.dag b/modules/drivers/lidar/processor/dag/lidar_unified_processor.dag new file mode 100644 index 00000000..199aae4a --- /dev/null +++ b/modules/drivers/lidar/processor/dag/lidar_unified_processor.dag @@ -0,0 +1,11 @@ +module_config { + module_library : "/apollo/bazel-bin/modules/drivers/lidar/processor/liblidar_unified_component.so" + components { + class_name : "LidarUnifiedComponent" + config { + name : "lidar_unified_processor" + config_file_path : "/apollo/modules/drivers/lidar/processor/conf/lidar_unified_component_config.pb.txt" + readers {channel: "/apollo/sensor/lslidar_main/PointCloud2"} + } + } +} diff --git a/modules/drivers/lidar/processor/launch/lidar_unified_processor.launch b/modules/drivers/lidar/processor/launch/lidar_unified_processor.launch new file mode 100644 index 00000000..bd5e35af --- /dev/null +++ b/modules/drivers/lidar/processor/launch/lidar_unified_processor.launch @@ -0,0 +1,7 @@ + + + lidar_unified_processor + /apollo/modules/drivers/lidar/processor/dag/lidar_unified_processor.dag + lidar_unified_processor + + diff --git a/modules/drivers/lidar/processor/lidar_policy_test.cc b/modules/drivers/lidar/processor/lidar_policy_test.cc new file mode 100644 index 00000000..740cbd16 --- /dev/null +++ b/modules/drivers/lidar/processor/lidar_policy_test.cc @@ -0,0 +1,854 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "Eigen/Geometry" + +#include "cyber/time/time.h" +#include "modules/drivers/lidar/processor/lidar_unified_component.h" +#include "modules/drivers/lidar/processor/policy/cpu_lidar_policy.h" +#include "modules/drivers/lidar/processor/policy/gpu_lidar_policy.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +namespace { + +constexpr uint64_t kSecondToNano = 1000000000ULL; + +class MockBuffer : public apollo::transform::BufferInterface { + public: + using Key = std::tuple; + + void AddTransform(const std::string& target_frame, + const std::string& source_frame, const cyber::Time& time, + const Eigen::Affine3d& transform) { + transforms_[Key(target_frame, source_frame, time.ToNanosecond())] = + ToStamped(target_frame, source_frame, time, transform); + } + + apollo::transform::TransformStamped lookupTransform( + const std::string& target_frame, const std::string& source_frame, + const cyber::Time& time, const float timeout_second) const override { + (void)timeout_second; + return Get(target_frame, source_frame, time); + } + + apollo::transform::TransformStamped lookupTransform( + const std::string& target_frame, const cyber::Time& target_time, + const std::string& source_frame, const cyber::Time& source_time, + const std::string& fixed_frame, + const float timeout_second) const override { + (void)target_time; + (void)fixed_frame; + (void)timeout_second; + return Get(target_frame, source_frame, source_time); + } + + bool canTransform(const std::string& target_frame, + const std::string& source_frame, const cyber::Time& time, + const float timeout_second, + std::string* errstr) const override { + (void)timeout_second; + return Exists(target_frame, source_frame, time, errstr); + } + + bool canTransform(const std::string& target_frame, + const cyber::Time& target_time, + const std::string& source_frame, + const cyber::Time& source_time, + const std::string& fixed_frame, const float timeout_second, + std::string* errstr) const override { + (void)target_time; + (void)fixed_frame; + (void)timeout_second; + return Exists(target_frame, source_frame, source_time, errstr); + } + + bool GetLatestStaticTransform( + const std::string& target_frame, const std::string& source_frame, + apollo::transform::TransformStamped* transform) const override { + if (transform == nullptr) { + return false; + } + const auto it = + transforms_.lower_bound(Key(target_frame, source_frame, 0U)); + if (it == transforms_.end() || std::get<0>(it->first) != target_frame || + std::get<1>(it->first) != source_frame) { + return false; + } + *transform = it->second; + return true; + } + + private: + static apollo::transform::TransformStamped ToStamped( + const std::string& target_frame, const std::string& source_frame, + const cyber::Time& time, const Eigen::Affine3d& transform) { + apollo::transform::TransformStamped stamped; + stamped.mutable_header()->set_frame_id(target_frame); + stamped.mutable_header()->set_timestamp_sec(time.ToSecond()); + stamped.set_child_frame_id(source_frame); + stamped.mutable_transform()->mutable_translation()->set_x( + transform.translation().x()); + stamped.mutable_transform()->mutable_translation()->set_y( + transform.translation().y()); + stamped.mutable_transform()->mutable_translation()->set_z( + transform.translation().z()); + const Eigen::Quaterniond rotation(transform.linear()); + stamped.mutable_transform()->mutable_rotation()->set_qw(rotation.w()); + stamped.mutable_transform()->mutable_rotation()->set_qx(rotation.x()); + stamped.mutable_transform()->mutable_rotation()->set_qy(rotation.y()); + stamped.mutable_transform()->mutable_rotation()->set_qz(rotation.z()); + return stamped; + } + + apollo::transform::TransformStamped Get(const std::string& target_frame, + const std::string& source_frame, + const cyber::Time& time) const { + const auto it = + transforms_.find(Key(target_frame, source_frame, time.ToNanosecond())); + if (it == transforms_.end()) { + throw std::runtime_error("Missing mock transform"); + } + return it->second; + } + + bool Exists(const std::string& target_frame, const std::string& source_frame, + const cyber::Time& time, std::string* errstr) const { + const bool found = transforms_.count(Key(target_frame, source_frame, + time.ToNanosecond())) > 0; + if (!found && errstr != nullptr) { + *errstr = "mock transform not found"; + } + return found; + } + + std::map transforms_; +}; + +std::shared_ptr MakePointCloud( + const std::string& frame_id, double measurement_time, + const std::vector>& points) { + auto cloud = std::make_shared(); + cloud->set_frame_id(frame_id); + cloud->set_measurement_time(measurement_time); + cloud->set_is_dense(true); + for (const auto& point : points) { + auto* out = cloud->add_point(); + out->set_x(std::get<0>(point)); + out->set_y(std::get<1>(point)); + out->set_z(std::get<2>(point)); + out->set_timestamp(std::get<3>(point)); + } + cloud->set_width(cloud->point_size()); + cloud->set_height(1); + return cloud; +} + +LidarUnifiedComponentConfig MakeConfig() { + LidarUnifiedComponentConfig config; + config.set_map_frame_id("map"); + config.set_base_link_frame_id("base_link"); + config.set_motion_compensation_bins(3); + config.set_voxel_size(1.0f); + config.set_enable_ego_query_filter(true); + config.set_ego_box_forward_x(0.5f); + config.set_ego_box_backward_x(-0.5f); + config.set_ego_box_forward_y(0.5f); + config.set_ego_box_backward_y(-0.5f); + return config; +} + +} // namespace + +TEST(LidarPolicyFactoryTest, CreatesPoliciesForKnownModes) { +#ifdef APOLLO_LIDAR_POLICY_FORCE_CPU + EXPECT_NE(LidarPolicyFactory::CreateDeskewPolicy("cpu"), nullptr); + EXPECT_EQ(LidarPolicyFactory::CreateDeskewPolicy("gpu"), nullptr); + EXPECT_NE(LidarPolicyFactory::CreateFusionPolicy("cpu"), nullptr); + EXPECT_EQ(LidarPolicyFactory::CreateFusionPolicy("gpu"), nullptr); + EXPECT_NE(LidarPolicyFactory::CreateFilterPolicy("cpu"), nullptr); + EXPECT_EQ(LidarPolicyFactory::CreateFilterPolicy("gpu"), nullptr); +#elif defined(APOLLO_LIDAR_POLICY_FORCE_GPU) + EXPECT_EQ(LidarPolicyFactory::CreateDeskewPolicy("cpu"), nullptr); +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED + EXPECT_NE(LidarPolicyFactory::CreateDeskewPolicy("gpu"), nullptr); +#else + EXPECT_EQ(LidarPolicyFactory::CreateDeskewPolicy("gpu"), nullptr); +#endif + EXPECT_EQ(LidarPolicyFactory::CreateFusionPolicy("cpu"), nullptr); +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED + EXPECT_NE(LidarPolicyFactory::CreateFusionPolicy("gpu"), nullptr); +#else + EXPECT_EQ(LidarPolicyFactory::CreateFusionPolicy("gpu"), nullptr); +#endif + EXPECT_EQ(LidarPolicyFactory::CreateFilterPolicy("cpu"), nullptr); +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED + EXPECT_NE(LidarPolicyFactory::CreateFilterPolicy("gpu"), nullptr); +#else + EXPECT_EQ(LidarPolicyFactory::CreateFilterPolicy("gpu"), nullptr); +#endif +#else + EXPECT_NE(LidarPolicyFactory::CreateDeskewPolicy("cpu"), nullptr); +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED + EXPECT_NE(LidarPolicyFactory::CreateDeskewPolicy("gpu"), nullptr); +#else + EXPECT_EQ(LidarPolicyFactory::CreateDeskewPolicy("gpu"), nullptr); +#endif + EXPECT_NE(LidarPolicyFactory::CreateFusionPolicy("cpu"), nullptr); +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED + EXPECT_NE(LidarPolicyFactory::CreateFusionPolicy("gpu"), nullptr); +#else + EXPECT_EQ(LidarPolicyFactory::CreateFusionPolicy("gpu"), nullptr); +#endif + EXPECT_NE(LidarPolicyFactory::CreateFilterPolicy("cpu"), nullptr); +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED + EXPECT_NE(LidarPolicyFactory::CreateFilterPolicy("gpu"), nullptr); +#else + EXPECT_EQ(LidarPolicyFactory::CreateFilterPolicy("gpu"), nullptr); +#endif + EXPECT_EQ(LidarPolicyFactory::CreateFilterPolicy("unknown"), nullptr); +#endif +} + +TEST(CpuLidarDeskewPolicyTest, ComputesSampledPosesFromPointTimestamps) { + MockBuffer tf_buffer; + tf_buffer.AddTransform( + "map", "lidar", cyber::Time(10.0), + Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + tf_buffer.AddTransform( + "map", "lidar", cyber::Time(11.0), + Eigen::Translation3d(1.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + tf_buffer.AddTransform( + "map", "lidar", cyber::Time(12.0), + Eigen::Translation3d(2.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + + CpuLidarDeskewPolicy policy; + ASSERT_TRUE(policy.Init(MakeConfig(), &tf_buffer)); + + SensorFrameContext frame_context; + frame_context.sensor_id = "lidar"; + frame_context.point_cloud = + MakePointCloud("lidar", 12.0, + {{0.0f, 0.0f, 0.0f, 10 * kSecondToNano}, + {0.0f, 0.0f, 0.0f, 12 * kSecondToNano}}); + + std::vector sample_times; + std::vector poses; + ASSERT_TRUE(policy.ComputeMotionCompensationPoses(frame_context, + &sample_times, &poses)); + ASSERT_EQ(sample_times.size(), 3U); + ASSERT_EQ(poses.size(), 3U); + EXPECT_DOUBLE_EQ(sample_times[0], 10.0); + EXPECT_DOUBLE_EQ(sample_times[1], 11.0); + EXPECT_DOUBLE_EQ(sample_times[2], 12.0); + EXPECT_DOUBLE_EQ(poses[0].translation().x(), 0.0); + EXPECT_DOUBLE_EQ(poses[1].translation().x(), 1.0); + EXPECT_DOUBLE_EQ(poses[2].translation().x(), 2.0); +} + +TEST(CpuLidarDeskewPolicyTest, + FallsBackToMeasurementTimeForInvalidPointTimestamps) { + MockBuffer tf_buffer; + tf_buffer.AddTransform( + "map", "lidar", cyber::Time(12.0), + Eigen::Translation3d(2.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + + CpuLidarDeskewPolicy policy; + ASSERT_TRUE(policy.Init(MakeConfig(), &tf_buffer)); + + SensorFrameContext frame_context; + frame_context.sensor_id = "lidar"; + frame_context.point_cloud = MakePointCloud( + "lidar", 12.0, + {{0.0f, 0.0f, 0.0f, 2085983134000164270ULL}, + {0.0f, 0.0f, 0.0f, 2085983134000263691ULL}}); + + std::vector sample_times; + std::vector poses; + ASSERT_TRUE(policy.ComputeMotionCompensationPoses(frame_context, + &sample_times, &poses)); + ASSERT_EQ(sample_times.size(), 3U); + ASSERT_EQ(poses.size(), 3U); + EXPECT_DOUBLE_EQ(sample_times[0], 12.0); + EXPECT_DOUBLE_EQ(sample_times[1], 12.0); + EXPECT_DOUBLE_EQ(sample_times[2], 12.0); + EXPECT_DOUBLE_EQ(poses[0].translation().x(), 2.0); + EXPECT_DOUBLE_EQ(poses[1].translation().x(), 2.0); + EXPECT_DOUBLE_EQ(poses[2].translation().x(), 2.0); +} + +TEST(CpuLidarFusionPolicyTest, FusesPointsIntoReferenceBaseFrame) { + MockBuffer tf_buffer; + CpuLidarFusionPolicy policy; + ASSERT_TRUE(policy.Init(MakeConfig(), &tf_buffer)); + const Eigen::Affine3d map2base_ref = + (Eigen::Translation3d(2.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()) + .inverse(); + + SensorFrameContext frame_context; + frame_context.sensor_id = "lidar"; + frame_context.point_cloud = + MakePointCloud("lidar", 12.0, + {{1.0f, 0.0f, 0.0f, 10 * kSecondToNano}, + {1.0f, 0.0f, 0.0f, 12 * kSecondToNano}}); + + std::vector storage(8); + PointCloudBuffer buffer; + buffer.data_ptr = storage.data(); + buffer.capacity = storage.size(); + buffer.valid_count = 0; + buffer.item_size = sizeof(PointXYZIT); + buffer.device_type = MemoryDeviceType::kHost; + buffer.device_id = -1; + + const std::vector> sample_times{{10.0, 12.0}}; + const std::vector> poses{{ + Eigen::Translation3d(5.0, 0.0, 0.0) * Eigen::Quaterniond::Identity(), + Eigen::Translation3d(7.0, 0.0, 0.0) * Eigen::Quaterniond::Identity(), + }}; + + ASSERT_TRUE(policy.FuseToBaseLink(12.0, map2base_ref, {frame_context}, poses, + sample_times, &buffer)); + ASSERT_EQ(buffer.valid_count, 2U); + EXPECT_FLOAT_EQ(storage[0].x(), 4.0f); + EXPECT_FLOAT_EQ(storage[1].x(), 6.0f); +} + +TEST(CpuLidarFusionPolicyTest, InterpolatesIntermediatePoseBins) { + MockBuffer tf_buffer; + CpuLidarFusionPolicy policy; + ASSERT_TRUE(policy.Init(MakeConfig(), &tf_buffer)); + const Eigen::Affine3d map2base_ref = Eigen::Affine3d::Identity(); + + SensorFrameContext frame_context; + frame_context.sensor_id = "lidar"; + frame_context.point_cloud = + MakePointCloud("lidar", 11.0, {{0.0f, 0.0f, 0.0f, 11 * kSecondToNano}}); + + std::vector storage(4); + PointCloudBuffer buffer; + buffer.data_ptr = storage.data(); + buffer.capacity = storage.size(); + buffer.valid_count = 0; + buffer.item_size = sizeof(PointXYZIT); + buffer.device_type = MemoryDeviceType::kHost; + buffer.device_id = -1; + + const std::vector> sample_times{{10.0, 12.0}}; + const std::vector> poses{{ + Eigen::Translation3d(5.0, 0.0, 0.0) * Eigen::Quaterniond::Identity(), + Eigen::Translation3d(7.0, 0.0, 0.0) * Eigen::Quaterniond::Identity(), + }}; + + ASSERT_TRUE(policy.FuseToBaseLink(11.0, map2base_ref, {frame_context}, poses, + sample_times, &buffer)); + ASSERT_EQ(buffer.valid_count, 1U); + EXPECT_FLOAT_EQ(storage[0].x(), 6.0f); +} + +TEST(CpuLidarFilterPolicyTest, AppliesEgoFilterBeforeVoxelDownsample) { + CpuLidarFilterPolicy policy; + ASSERT_TRUE(policy.Init(MakeConfig())); + + std::vector storage(4); + storage[0].set_x(0.1f); + storage[0].set_y(0.1f); + storage[1].set_x(0.6f); + storage[1].set_y(0.1f); + storage[2].set_x(1.6f); + storage[2].set_y(0.1f); + + PointCloudBuffer buffer; + buffer.data_ptr = storage.data(); + buffer.capacity = storage.size(); + buffer.valid_count = 3; + buffer.item_size = sizeof(PointXYZIT); + buffer.device_type = MemoryDeviceType::kHost; + buffer.device_id = -1; + + size_t ego_filtered_count = 0; + size_t voxel_filtered_count = 0; + const size_t final_count = + policy.ApplyFilters(&buffer, &ego_filtered_count, &voxel_filtered_count); + EXPECT_EQ(final_count, 2U); + EXPECT_EQ(buffer.valid_count, 2U); + EXPECT_EQ(ego_filtered_count, 1U); + EXPECT_EQ(voxel_filtered_count, 0U); + EXPECT_FLOAT_EQ(storage[0].x(), 0.6f); + EXPECT_FLOAT_EQ(storage[1].x(), 1.6f); +} + +TEST(CpuLidarFilterPolicyTest, + AggregatesDeterministicVoxelCentroidAndIntensity) { + CpuLidarFilterPolicy policy; + auto config = MakeConfig(); + config.set_enable_ego_query_filter(false); + ASSERT_TRUE(policy.Init(config)); + + std::vector storage(4); + storage[0].set_x(0.1f); + storage[0].set_y(0.1f); + storage[0].set_z(0.0f); + storage[0].set_intensity(1.0f); + storage[0].set_timestamp(10U); + storage[1].set_x(0.3f); + storage[1].set_y(0.3f); + storage[1].set_z(0.0f); + storage[1].set_intensity(3.0f); + storage[1].set_timestamp(14U); + + PointCloudBuffer buffer; + buffer.data_ptr = storage.data(); + buffer.capacity = storage.size(); + buffer.valid_count = 2; + buffer.item_size = sizeof(PointXYZIT); + buffer.device_type = MemoryDeviceType::kHost; + buffer.device_id = -1; + + size_t ego_filtered_count = 0; + size_t voxel_filtered_count = 0; + const size_t final_count = + policy.ApplyFilters(&buffer, &ego_filtered_count, &voxel_filtered_count); + EXPECT_EQ(final_count, 1U); + EXPECT_EQ(voxel_filtered_count, 1U); + EXPECT_FLOAT_EQ(storage[0].x(), 0.2f); + EXPECT_FLOAT_EQ(storage[0].y(), 0.2f); + EXPECT_FLOAT_EQ(storage[0].intensity(), 2.0f); + EXPECT_EQ(storage[0].timestamp(), 12U); +} + +TEST(LidarUnifiedComponentTest, RejectsPrimarySensorIdDrift) { + LidarUnifiedComponent component; + component.primary_sensor_id_ = "lidar_primary"; + + EXPECT_FALSE(component.OnReceiveMainLidar( + MakePointCloud("lidar_secondary", 12.0, {{0.0f, 0.0f, 0.0f, 0U}}))); + EXPECT_EQ(component.primary_sensor_id_, "lidar_primary"); +} + +TEST(LidarUnifiedComponentTest, FindsNearestFrameFromOutOfOrderBuffer) { + LidarUnifiedComponent component; + auto sensor_state = std::make_shared(4); + auto make_buffered_frame = [](double timestamp_sec) { + auto frame = std::make_shared(); + frame->point_cloud = MakePointCloud("lidar", timestamp_sec, {}); + frame->pose_prefetch_ok = true; + return frame; + }; + sensor_state->frames.push_back(make_buffered_frame(10.20)); + sensor_state->frames.push_back(make_buffered_frame(10.00)); + sensor_state->frames.push_back(make_buffered_frame(10.10)); + + FrameHandle nearest_frame; + auto failure_reason = LidarUnifiedComponent::FrameLookupFailureReason::kNone; + ASSERT_TRUE(component.FindNearestFrame(sensor_state, "lidar", 10.05, 100, + &nearest_frame, &failure_reason)); + ASSERT_NE(nearest_frame.point_cloud, nullptr); + EXPECT_EQ(failure_reason, + LidarUnifiedComponent::FrameLookupFailureReason::kNone); + EXPECT_DOUBLE_EQ(nearest_frame.point_cloud->measurement_time(), 10.00); +} + +TEST(LidarUnifiedComponentTest, ReportsTimeDeltaExceededForNearestFrame) { + LidarUnifiedComponent component; + auto sensor_state = std::make_shared(4); + auto late_frame = std::make_shared(); + late_frame->point_cloud = MakePointCloud("lidar", 10.30, {}); + late_frame->pose_prefetch_ok = true; + auto early_frame = std::make_shared(); + early_frame->point_cloud = MakePointCloud("lidar", 10.00, {}); + early_frame->pose_prefetch_ok = true; + sensor_state->frames.push_back(late_frame); + sensor_state->frames.push_back(early_frame); + + FrameHandle nearest_frame; + auto failure_reason = LidarUnifiedComponent::FrameLookupFailureReason::kNone; + EXPECT_FALSE(component.FindNearestFrame(sensor_state, "lidar", 10.05, 40, + &nearest_frame, &failure_reason)); + EXPECT_EQ( + failure_reason, + LidarUnifiedComponent::FrameLookupFailureReason::kTimeDeltaExceeded); +} + +TEST(LidarUnifiedComponentTest, AppliesFixedDelayDuringFrameLookup) { + LidarUnifiedComponent component; + auto sensor_state = std::make_shared(4); + sensor_state->fixed_delay_initialized = true; + sensor_state->fixed_delay_sec = -0.02; + + auto aligned_frame = std::make_shared(); + aligned_frame->point_cloud = MakePointCloud("lidar", 10.02, {}); + aligned_frame->pose_prefetch_ok = true; + auto farther_frame = std::make_shared(); + farther_frame->point_cloud = MakePointCloud("lidar", 10.05, {}); + farther_frame->pose_prefetch_ok = true; + sensor_state->frames.push_back(farther_frame); + sensor_state->frames.push_back(aligned_frame); + + FrameHandle nearest_frame; + auto failure_reason = LidarUnifiedComponent::FrameLookupFailureReason::kNone; + ASSERT_TRUE(component.FindNearestFrame(sensor_state, "lidar", 10.0, 40, + &nearest_frame, &failure_reason)); + EXPECT_DOUBLE_EQ(nearest_frame.point_cloud->measurement_time(), 10.02); + EXPECT_NEAR(nearest_frame.clock_offset_residual_ms, 0.0, 1e-6); +} + +TEST(LidarUnifiedComponentTest, UpdatesSensorTimingModel) { + LidarUnifiedComponent component; + component.config_.set_fixed_delay_ema_alpha(0.5); + component.config_.set_fixed_delay_update_limit_ms(100.0); + component.config_.set_clock_offset_ema_alpha(0.5); + + auto sensor_state = std::make_shared(4); + component.sensor_states_["aux_lidar"] = sensor_state; + + LidarUnifiedComponent::FrameMetrics metrics; + FrameHandle first_handle; + first_handle.sensor_id = "aux_lidar"; + first_handle.point_cloud = MakePointCloud("aux_lidar", 10.02, {}); + first_handle.clock_offset_residual_ms = 4.0; + component.UpdateSensorTimingModel(first_handle, 10.0, &metrics); + + EXPECT_TRUE(sensor_state->fixed_delay_initialized); + EXPECT_NEAR(sensor_state->fixed_delay_sec, -0.02, 1e-9); + EXPECT_DOUBLE_EQ(sensor_state->smoothed_clock_offset_ms, 4.0); + EXPECT_DOUBLE_EQ(metrics.max_abs_clock_offset_ms, 4.0); + + FrameHandle second_handle; + second_handle.sensor_id = "aux_lidar"; + second_handle.point_cloud = MakePointCloud("aux_lidar", 10.01, {}); + second_handle.clock_offset_residual_ms = 2.0; + component.UpdateSensorTimingModel(second_handle, 10.0, &metrics); + + EXPECT_NEAR(sensor_state->fixed_delay_sec, -0.015, 1e-9); + EXPECT_DOUBLE_EQ(sensor_state->smoothed_clock_offset_ms, 3.0); +} + +TEST(LidarUnifiedComponentTest, + UpdatesLargeFixedDelayWhenInnovationIsWithinLimit) { + LidarUnifiedComponent component; + component.config_.set_fixed_delay_ema_alpha(0.5); + component.config_.set_fixed_delay_update_limit_ms(10.0); + component.config_.set_clock_offset_ema_alpha(0.5); + + auto sensor_state = std::make_shared(4); + sensor_state->fixed_delay_initialized = true; + sensor_state->fixed_delay_sec = 0.09; + component.sensor_states_["aux_lidar"] = sensor_state; + + LidarUnifiedComponent::FrameMetrics metrics; + FrameHandle handle; + handle.sensor_id = "aux_lidar"; + handle.point_cloud = MakePointCloud("aux_lidar", 9.905, {}); + handle.clock_offset_residual_ms = 5.0; + component.UpdateSensorTimingModel(handle, 10.0, &metrics); + + EXPECT_NEAR(sensor_state->fixed_delay_sec, 0.0925, 1e-9); + EXPECT_DOUBLE_EQ(sensor_state->smoothed_clock_offset_ms, 5.0); +} + +TEST(LidarUnifiedComponentTest, CollectNearestFramesSkipsLowQualityAuxiliary) { + LidarUnifiedComponent component; + component.config_.set_strict_auxiliary_sync(false); + component.config_.set_max_ref_time_delta_ms(100); + component.config_.set_auxiliary_min_overlap_quality_weight(0.2); + component.auxiliary_inputs_.push_back( + LidarUnifiedComponent::SensorInput{"/aux"}); + component.auxiliary_sensor_ids_by_topic_["/aux"] = "aux_lidar"; + + auto make_buffered_frame = [](const std::string& sensor_id, + double timestamp_sec) { + auto frame = std::make_shared(); + frame->point_cloud = MakePointCloud(sensor_id, timestamp_sec, {}); + frame->pose_prefetch_ok = true; + return frame; + }; + + auto primary_state = std::make_shared(4); + primary_state->frames.push_back(make_buffered_frame("primary", 10.0)); + component.sensor_states_["primary"] = primary_state; + + auto auxiliary_state = + std::make_shared(4); + auxiliary_state->overlap_quality_weight = 0.1; + auxiliary_state->frames.push_back(make_buffered_frame("aux_lidar", 10.0)); + component.sensor_states_["aux_lidar"] = auxiliary_state; + + std::vector frame_handles; + LidarUnifiedComponent::FrameMetrics metrics; + ASSERT_TRUE(component.CollectNearestFrames(10.0, "primary", &frame_handles, + &metrics)); + ASSERT_EQ(frame_handles.size(), 1U); + EXPECT_TRUE(frame_handles.front().is_primary); + EXPECT_EQ(metrics.missing_auxiliary_count, 1U); +} + +TEST(LidarUnifiedComponentTest, EstimatesOverlapQualityWeight) { + LidarUnifiedComponent component; + component.config_.set_overlap_quality_sample_stride(1); + component.config_.set_overlap_region_forward_x(1.0); + component.config_.set_overlap_region_backward_x(-1.0); + component.config_.set_overlap_region_left_y(1.0); + component.config_.set_overlap_region_right_y(-1.0); + component.config_.set_overlap_region_min_z(-1.0); + component.config_.set_overlap_region_max_z(1.0); + + BufferedFrame frame; + frame.point_cloud = MakePointCloud( + "lidar", 10.0, {{0.0f, 0.0f, 0.0f, 10U}, {5.0f, 5.0f, 0.0f, 10U}}); + frame.motion_sample_times = {10.0}; + frame.motion_poses = {Eigen::Affine3d::Identity()}; + frame.pose_prefetch_ok = true; + + const double overlap_weight = component.EstimateOverlapQualityWeight( + frame, Eigen::Affine3d::Identity()); + EXPECT_DOUBLE_EQ(overlap_weight, 0.5); +} + +TEST(GpuLidarFilterPolicyTest, AppliesGpuFilteringOrFailsWithoutBackend) { + GpuLidarFilterPolicy policy; +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED + ASSERT_TRUE(policy.Init(MakeConfig())); +#else + EXPECT_FALSE(policy.Init(MakeConfig())); + return; +#endif + + std::vector storage(4); + storage[0].set_x(0.1f); + storage[0].set_y(0.1f); + storage[1].set_x(0.6f); + storage[1].set_y(0.1f); + storage[2].set_x(1.6f); + storage[2].set_y(0.1f); + + PointCloudBuffer buffer; + buffer.data_ptr = storage.data(); + buffer.capacity = storage.size(); + buffer.valid_count = 3; + buffer.item_size = sizeof(PointXYZIT); + buffer.device_type = MemoryDeviceType::kHost; + buffer.device_id = 0; + + size_t ego_filtered_count = 0; + size_t voxel_filtered_count = 0; + const size_t final_count = + policy.ApplyFilters(&buffer, &ego_filtered_count, &voxel_filtered_count); + EXPECT_EQ(final_count, 2U); + EXPECT_EQ(ego_filtered_count, 1U); + EXPECT_EQ(voxel_filtered_count, 0U); +} + +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED +TEST(GpuLidarFilterPolicyTest, + AggregatesDeterministicVoxelCentroidAndIntensity) { + GpuLidarFilterPolicy policy; + auto config = MakeConfig(); + config.set_enable_ego_query_filter(false); + config.set_gpu_device_id(0); + ASSERT_TRUE(policy.Init(config)); + + std::vector storage(4); + storage[0].set_x(0.1f); + storage[0].set_y(0.1f); + storage[0].set_intensity(1.0f); + storage[0].set_timestamp(10U); + storage[1].set_x(0.3f); + storage[1].set_y(0.3f); + storage[1].set_intensity(3.0f); + storage[1].set_timestamp(14U); + + PointCloudBuffer buffer; + buffer.data_ptr = storage.data(); + buffer.capacity = storage.size(); + buffer.valid_count = 2; + buffer.item_size = sizeof(PointXYZIT); + buffer.device_type = MemoryDeviceType::kHost; + buffer.device_id = 0; + + size_t ego_filtered_count = 0; + size_t voxel_filtered_count = 0; + const size_t final_count = + policy.ApplyFilters(&buffer, &ego_filtered_count, &voxel_filtered_count); + EXPECT_EQ(final_count, 1U); + EXPECT_EQ(voxel_filtered_count, 1U); + EXPECT_FLOAT_EQ(storage[0].x(), 0.2f); + EXPECT_FLOAT_EQ(storage[0].y(), 0.2f); + EXPECT_FLOAT_EQ(storage[0].intensity(), 2.0f); + EXPECT_EQ(storage[0].timestamp(), 12U); +} + +TEST(GpuLidarFusionPolicyTest, FusesPointsIntoReferenceBaseFrame) { + MockBuffer tf_buffer; + GpuLidarFusionPolicy policy; + auto config = MakeConfig(); + config.set_gpu_device_id(0); + ASSERT_TRUE(policy.Init(config, &tf_buffer)); + const Eigen::Affine3d map2base_ref = + (Eigen::Translation3d(2.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()) + .inverse(); + + SensorFrameContext frame_context; + frame_context.sensor_id = "lidar"; + frame_context.point_cloud = + MakePointCloud("lidar", 12.0, + {{1.0f, 0.0f, 0.0f, 10 * kSecondToNano}, + {1.0f, 0.0f, 0.0f, 12 * kSecondToNano}}); + + std::vector storage(8); + PointCloudBuffer buffer; + buffer.data_ptr = storage.data(); + buffer.capacity = storage.size(); + buffer.valid_count = 0; + buffer.item_size = sizeof(PointXYZIT); + buffer.device_type = MemoryDeviceType::kHost; + buffer.device_id = -1; + + const std::vector> sample_times{{10.0, 12.0}}; + const std::vector> poses{{ + Eigen::Translation3d(5.0, 0.0, 0.0) * Eigen::Quaterniond::Identity(), + Eigen::Translation3d(7.0, 0.0, 0.0) * Eigen::Quaterniond::Identity(), + }}; + + ASSERT_TRUE(policy.FuseToBaseLink(12.0, map2base_ref, {frame_context}, + poses, sample_times, &buffer)); + ASSERT_EQ(buffer.valid_count, 2U); + std::vector xs{storage[0].x(), storage[1].x()}; + std::sort(xs.begin(), xs.end()); + EXPECT_FLOAT_EQ(xs[0], 4.0f); + EXPECT_FLOAT_EQ(xs[1], 6.0f); +} + +TEST(GpuLidarFusionPolicyTest, InterpolatesIntermediatePoseBins) { + MockBuffer tf_buffer; + GpuLidarFusionPolicy policy; + auto config = MakeConfig(); + config.set_gpu_device_id(0); + ASSERT_TRUE(policy.Init(config, &tf_buffer)); + const Eigen::Affine3d map2base_ref = Eigen::Affine3d::Identity(); + + SensorFrameContext frame_context; + frame_context.sensor_id = "lidar"; + frame_context.point_cloud = + MakePointCloud("lidar", 11.0, {{0.0f, 0.0f, 0.0f, 11 * kSecondToNano}}); + + std::vector storage(4); + PointCloudBuffer buffer; + buffer.data_ptr = storage.data(); + buffer.capacity = storage.size(); + buffer.valid_count = 0; + buffer.item_size = sizeof(PointXYZIT); + buffer.device_type = MemoryDeviceType::kHost; + buffer.device_id = -1; + + const std::vector> sample_times{{10.0, 12.0}}; + const std::vector> poses{{ + Eigen::Translation3d(5.0, 0.0, 0.0) * Eigen::Quaterniond::Identity(), + Eigen::Translation3d(7.0, 0.0, 0.0) * Eigen::Quaterniond::Identity(), + }}; + + ASSERT_TRUE(policy.FuseToBaseLink(11.0, map2base_ref, {frame_context}, + poses, sample_times, &buffer)); + ASSERT_EQ(buffer.valid_count, 1U); + EXPECT_NEAR(storage[0].x(), 6.0f, 1e-4f); +} + +TEST(GpuLidarFusionPolicyTest, + PreservesRelativePrecisionWithLargeMapOffsets) { + constexpr double kMapOffset = 1e8; + + MockBuffer tf_buffer; + + GpuLidarFusionPolicy policy; + auto config = MakeConfig(); + config.set_gpu_device_id(0); + ASSERT_TRUE(policy.Init(config, &tf_buffer)); + const Eigen::Affine3d map2base_ref = + (Eigen::Translation3d(kMapOffset + 2.0, 0.0, 0.0) * + Eigen::Quaterniond::Identity()) + .inverse(); + + SensorFrameContext frame_context; + frame_context.sensor_id = "lidar"; + frame_context.point_cloud = + MakePointCloud("lidar", 12.0, + {{1.0f, 0.0f, 0.0f, 10 * kSecondToNano}, + {1.0f, 0.0f, 0.0f, 12 * kSecondToNano}}); + + std::vector storage(8); + PointCloudBuffer buffer; + buffer.data_ptr = storage.data(); + buffer.capacity = storage.size(); + buffer.valid_count = 0; + buffer.item_size = sizeof(PointXYZIT); + buffer.device_type = MemoryDeviceType::kHost; + buffer.device_id = -1; + + const std::vector> sample_times{{10.0, 12.0}}; + const std::vector> poses{{ + Eigen::Translation3d(kMapOffset + 5.0, 0.0, 0.0) * + Eigen::Quaterniond::Identity(), + Eigen::Translation3d(kMapOffset + 7.0, 0.0, 0.0) * + Eigen::Quaterniond::Identity(), + }}; + + ASSERT_TRUE(policy.FuseToBaseLink(12.0, map2base_ref, {frame_context}, + poses, sample_times, &buffer)); + ASSERT_EQ(buffer.valid_count, 2U); + std::vector xs{storage[0].x(), storage[1].x()}; + std::sort(xs.begin(), xs.end()); + EXPECT_NEAR(xs[0], 4.0f, 1e-3f); + EXPECT_NEAR(xs[1], 6.0f, 1e-3f); +} + +TEST(GpuLidarDeskewPolicyTest, ComputesSampledPosesFromPointTimestamps) { + MockBuffer tf_buffer; + tf_buffer.AddTransform( + "map", "lidar", cyber::Time(10.0), + Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + tf_buffer.AddTransform( + "map", "lidar", cyber::Time(11.0), + Eigen::Translation3d(1.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + tf_buffer.AddTransform( + "map", "lidar", cyber::Time(12.0), + Eigen::Translation3d(2.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + + GpuLidarDeskewPolicy policy; + auto config = MakeConfig(); + config.set_gpu_device_id(0); + ASSERT_TRUE(policy.Init(config, &tf_buffer)); + + SensorFrameContext frame_context; + frame_context.sensor_id = "lidar"; + frame_context.point_cloud = + MakePointCloud("lidar", 12.0, + {{0.0f, 0.0f, 0.0f, 10 * kSecondToNano}, + {0.0f, 0.0f, 0.0f, 12 * kSecondToNano}}); + + std::vector sample_times; + std::vector poses; + ASSERT_TRUE(policy.ComputeMotionCompensationPoses(frame_context, + &sample_times, &poses)); + ASSERT_EQ(sample_times.size(), 3U); + ASSERT_EQ(poses.size(), 3U); + EXPECT_DOUBLE_EQ(sample_times.front(), 10.0); + EXPECT_DOUBLE_EQ(sample_times.back(), 12.0); + EXPECT_DOUBLE_EQ(poses[0].translation().x(), 0.0); + EXPECT_DOUBLE_EQ(poses[2].translation().x(), 2.0); +} +#endif + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/lidar_policy_test_main.cc b/modules/drivers/lidar/processor/lidar_policy_test_main.cc new file mode 100644 index 00000000..25195d71 --- /dev/null +++ b/modules/drivers/lidar/processor/lidar_policy_test_main.cc @@ -0,0 +1,11 @@ +#include +#include + +#include + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + const int result = RUN_ALL_TESTS(); + std::fflush(nullptr); + std::_Exit(result); +} diff --git a/modules/drivers/lidar/processor/lidar_unified_component.cc b/modules/drivers/lidar/processor/lidar_unified_component.cc new file mode 100644 index 00000000..9bb5c198 --- /dev/null +++ b/modules/drivers/lidar/processor/lidar_unified_component.cc @@ -0,0 +1,671 @@ +#include "modules/drivers/lidar/processor/lidar_unified_component.h" + +#include +#include +#include +#include +#include +#include + +#include "modules/drivers/lidar/processor/policy/lidar_policy_common.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +namespace { + +constexpr int kPosePrefetchLogFrequency = 10; + +std::string FormatTimestampForLog(double timestamp_sec) { + std::ostringstream stream; + stream << std::fixed << std::setprecision(9) << timestamp_sec << "s"; + if (timestamp_sec > 0.0) { + stream << " [" << cyber::Time(timestamp_sec).ToString() << "]"; + } + return stream.str(); +} + +std::string FormatDeltaForLog(double delta_sec) { + const double abs_delta_sec = std::fabs(delta_sec); + std::ostringstream stream; + stream << std::showpos << std::fixed + << std::setprecision(abs_delta_sec >= 1.0 ? 3 : 6) << delta_sec + << "s" << std::noshowpos; + if (abs_delta_sec >= 86400.0) { + stream << " (~" << std::fixed << std::setprecision(3) + << abs_delta_sec / 86400.0 << " days)"; + } else if (abs_delta_sec >= 3600.0) { + stream << " (~" << std::fixed << std::setprecision(3) + << abs_delta_sec / 3600.0 << " h)"; + } else if (abs_delta_sec >= 60.0) { + stream << " (~" << std::fixed << std::setprecision(3) + << abs_delta_sec / 60.0 << " min)"; + } else if (abs_delta_sec > 0.0 && abs_delta_sec < 1.0) { + stream << " (~" << std::fixed << std::setprecision(3) + << abs_delta_sec * 1000.0 << " ms)"; + } + return stream.str(); +} + +const char* DescribeTimeRelation(double delta_sec) { + return std::fabs(delta_sec) <= 1e-6 ? "aligned" + : (delta_sec > 0.0 ? "ahead" + : "behind"); +} + +std::string BuildPointCloudTimeSummary(const PointCloud& point_cloud) { + double min_point_time_sec = point_cloud.measurement_time(); + double max_point_time_sec = point_cloud.measurement_time(); + ResolvePointTimestampBounds(point_cloud, &min_point_time_sec, + &max_point_time_sec); + + const double measurement_time_sec = point_cloud.measurement_time(); + const double now_sec = cyber::Time::Now().ToSecond(); + const double measurement_vs_now_sec = measurement_time_sec - now_sec; + + std::ostringstream message; + message << "measurement=" << FormatTimestampForLog(measurement_time_sec) + << ", point_range=[" << FormatTimestampForLog(min_point_time_sec) + << ", " << FormatTimestampForLog(max_point_time_sec) << "]" + << ", measurement_vs_now=" + << FormatDeltaForLog(measurement_vs_now_sec) << " (" + << DescribeTimeRelation(measurement_vs_now_sec) << ")"; + if (std::fabs(max_point_time_sec - min_point_time_sec) > 1e-6) { + message << ", scan_span=" + << FormatDeltaForLog(max_point_time_sec - min_point_time_sec); + } + return message.str(); +} + +std::string ResolvePolicyMode(const LidarUnifiedComponentConfig& config) { +#ifdef APOLLO_LIDAR_POLICY_FORCE_CPU + (void)config; + return "cpu"; +#endif +#ifdef APOLLO_LIDAR_POLICY_FORCE_GPU + (void)config; + return "gpu"; +#endif + switch (config.compute_mode()) { + case LidarUnifiedComponentConfig::COMPUTE_MODE_GPU: + return "gpu"; + case LidarUnifiedComponentConfig::COMPUTE_MODE_CPU: + default: + return "cpu"; + } +} + +apollo::transform::TimedTransformResolverOptions BuildTransformResolverOptions( + const LidarUnifiedComponentConfig& config) { + apollo::transform::TimedTransformResolverOptions options; + options.query_timeout_sec = + static_cast(config.sensor_pose_query_timeout_sec()); + options.cache_duration_sec = config.sensor_pose_cache_duration_sec(); + options.max_extrapolation_sec = + config.sensor_pose_cache_max_extrapolation_sec(); + return options; +} + +} // namespace + +bool LidarUnifiedComponent::Init() { + if (!GetProtoConfig(&config_)) { + AERROR << "Load config failed, config file: " << ConfigFilePath(); + return false; + } + + if (!ValidateConfig()) { + return false; + } + + writer_ = node_->CreateWriter<::apollo::drivers::PointCloud>( + config_.output_channel()); + tf_buffer_ = apollo::transform::Buffer::Instance(); + + const std::string policy_mode = ResolvePolicyMode(config_); + deskew_policy_ = LidarPolicyFactory::CreateDeskewPolicy(policy_mode); + fusion_policy_ = LidarPolicyFactory::CreateFusionPolicy(policy_mode); + filter_policy_ = LidarPolicyFactory::CreateFilterPolicy(policy_mode); + if (deskew_policy_ == nullptr || fusion_policy_ == nullptr || + filter_policy_ == nullptr) { + AERROR << "Failed to create lidar policies for mode=" << policy_mode; + return false; + } + if (!deskew_policy_->Init(config_, tf_buffer_) || + !fusion_policy_->Init(config_, tf_buffer_) || + !filter_policy_->Init(config_)) { + AERROR << "Failed to initialize lidar policies for mode=" << policy_mode; + return false; + } + + ts_sanity_.SetConfig(config_.ts_sanity_min_interval_ms(), + config_.ts_sanity_max_interval_ms(), + config_.ts_sanity_max_jump_ms()); + degrade_policy_.SetConfig(config_.degrade_on_ts_anomaly(), + config_.ts_sanity_max_consecutive_errors()); + + sensor_buffer_capacity_ = + std::max(2, static_cast(config_.sensor_buffer_size())); + + auxiliary_inputs_.clear(); + auxiliary_readers_.clear(); + auxiliary_sensor_ids_by_topic_.clear(); + sensor_states_.clear(); + primary_sensor_id_.clear(); + base_link_pose_resolver_ = + std::make_unique( + tf_buffer_, config_.map_frame_id(), config_.base_link_frame_id(), + BuildTransformResolverOptions(config_)); + + for (const auto& input_cfg : config_.auxiliary_lidar_inputs()) { + auxiliary_inputs_.push_back(SensorInput{input_cfg.topic_name()}); + + auto reader = node_->CreateReader<::apollo::drivers::PointCloud>( + input_cfg.topic_name(), + [this, topic_name = input_cfg.topic_name()]( + const std::shared_ptr<::apollo::drivers::PointCloud>& msg) { + OnAuxiliaryLidarMessage(topic_name, msg); + }); + auxiliary_readers_.emplace(input_cfg.topic_name(), reader); + } + + const size_t max_points = std::max( + 1, static_cast(config_.max_full_pointcloud_points())); + full_pointcloud_buffer_.resize(max_points); + + AINFO << "LidarUnifiedComponent initialized. compute_mode=" << policy_mode + << ", main_sensor=" + << ", aux_count=" << auxiliary_inputs_.size() + << ", max_points=" << max_points; + return true; +} + +bool LidarUnifiedComponent::Proc( + const std::shared_ptr<::apollo::drivers::PointCloud>& point_cloud) { + return OnReceiveMainLidar(point_cloud); +} + +bool LidarUnifiedComponent::OnReceiveMainLidar( + const PointCloudConstPtr& point_cloud) { + if (point_cloud == nullptr) { + AERROR << "Input point cloud is null"; + return false; + } + + const std::string sensor_id = ResolveSensorId(point_cloud, std::string()); + if (sensor_id.empty()) { + AERROR << "Failed to resolve primary lidar sensor id from incoming frame"; + return false; + } + + if (primary_sensor_id_.empty()) { + primary_sensor_id_ = sensor_id; + AINFO << "Resolved primary lidar sensor id from input stream: " + << primary_sensor_id_; + } else if (primary_sensor_id_ != sensor_id) { + AERROR << "Primary lidar sensor id changed from " << primary_sensor_id_ + << " to " << sensor_id << ", drop frame to avoid TF mismatch"; + return false; + } + + std::shared_ptr buffered_frame; + if (!PrepareBufferedFrame(primary_sensor_id_, point_cloud, &buffered_frame)) { + AINFO_EVERY(kPosePrefetchLogFrequency) + << "Drop primary lidar frame due to pose prefetch failure. sensor=" + << primary_sensor_id_ << ", " + << BuildPointCloudTimeSummary(*point_cloud); + return false; + } + PushToBuffer(primary_sensor_id_, buffered_frame); + + if (config_.ts_sanity_enabled()) { + const TsSanityResult ts_result = + ts_sanity_.Check(point_cloud->measurement_time()); + if (ts_result.status != TsSanityStatus::kOk && + ts_result.status != TsSanityStatus::kFirstFrame) { + dtc_reporter_.ReportTsAnomaly(ts_result.status, ts_result.interval_ms, + ts_result.consecutive_errors, + primary_sensor_id_); + const DegradeEvent event = + degrade_policy_.OnTsAnomaly(ts_result.consecutive_errors); + dtc_reporter_.ReportDegradeTransition(event); + } + } + + FrameMetrics frame_metrics; + std::vector frame_handles; + if (!CollectNearestFrames(point_cloud->measurement_time(), primary_sensor_id_, + &frame_handles, &frame_metrics)) { + AERROR << "Failed to collect nearest frames for ref timestamp " + << point_cloud->measurement_time(); + return false; + } + + for (const auto& frame_handle : frame_handles) { + if (!frame_handle.is_primary) { + UpdateSensorTimingModel(frame_handle, point_cloud->measurement_time(), + &frame_metrics); + } + } + + if (degrade_policy_.CurrentMode() == DegradeMode::kSingleLidar) { + frame_handles.erase( + std::remove_if( + frame_handles.begin(), frame_handles.end(), + [](const FrameHandle& handle) { return !handle.is_primary; }), + frame_handles.end()); + frame_metrics.expected_sensor_count = 1; + frame_metrics.matched_sensor_count = frame_handles.size(); + frame_metrics.missing_auxiliary_count = 0; + frame_metrics.time_delta_exceeded_count = 0; + } + + std::shared_ptr<::apollo::drivers::PointCloud> unified_output; + if (!BuildUnifiedPointCloud(point_cloud, frame_handles, &frame_metrics, + &unified_output)) { + AERROR << "Failed to build unified point cloud"; + return false; + } + + dtc_reporter_.ReportDegradeTransition(degrade_policy_.OnFrameOk()); + + LogFrameMetrics(frame_metrics); + writer_->Write(unified_output); + return true; +} + +void LidarUnifiedComponent::PushToBuffer( + const std::string& sensor_id, + const std::shared_ptr& buffered_frame) { + const auto sensor_state = GetSensorState(sensor_id); + if (sensor_state == nullptr || buffered_frame == nullptr) { + return; + } + std::lock_guard lock(sensor_state->mutex); + sensor_state->frames.push_back(buffered_frame); +} + +bool LidarUnifiedComponent::CollectNearestFrames( + double ref_timestamp, const std::string& primary_sensor_id, + std::vector* frame_handles, FrameMetrics* frame_metrics) { + if (frame_handles == nullptr || frame_metrics == nullptr) { + return false; + } + + std::vector auxiliary_topics; + auxiliary_topics.reserve(auxiliary_inputs_.size()); + for (const auto& auxiliary_input : auxiliary_inputs_) { + auxiliary_topics.push_back(auxiliary_input.topic_name); + } + + SyncGateMetrics sync_metrics; + const bool ok = sync_gate_.SelectFrames( + ref_timestamp, primary_sensor_id, auxiliary_topics, + config_.max_ref_time_delta_ms(), config_.strict_auxiliary_sync(), + [this](const std::string& topic_name, std::string* sensor_id) { + if (sensor_id == nullptr) { + return false; + } + std::lock_guard lock(sensor_registry_mutex_); + const auto it = auxiliary_sensor_ids_by_topic_.find(topic_name); + if (it == auxiliary_sensor_ids_by_topic_.end()) { + return false; + } + *sensor_id = it->second; + return !sensor_id->empty(); + }, + [this, primary_sensor_id]( + const std::string& sensor_id, double timestamp, uint32_t max_delta_ms, + FrameHandle* frame_handle, bool* time_delta_exceeded) { + if (time_delta_exceeded != nullptr) { + *time_delta_exceeded = false; + } + FrameLookupFailureReason failure_reason = + FrameLookupFailureReason::kNone; + const bool found = + FindNearestFrame(GetSensorState(sensor_id), sensor_id, timestamp, + max_delta_ms, frame_handle, &failure_reason); + if (found && sensor_id != primary_sensor_id && + frame_handle != nullptr && + frame_handle->overlap_quality_weight < + config_.auxiliary_min_overlap_quality_weight()) { + return false; + } + if (!found && time_delta_exceeded != nullptr) { + *time_delta_exceeded = + failure_reason == FrameLookupFailureReason::kTimeDeltaExceeded; + } + return found; + }, + frame_handles, &sync_metrics); + + frame_metrics->expected_sensor_count = sync_metrics.expected_sensor_count; + frame_metrics->matched_sensor_count = sync_metrics.matched_sensor_count; + frame_metrics->missing_auxiliary_count = sync_metrics.missing_auxiliary_count; + frame_metrics->time_delta_exceeded_count = + sync_metrics.time_delta_exceeded_count; + return ok; +} + +bool LidarUnifiedComponent::FindNearestFrame( + const std::shared_ptr& sensor_state, + const std::string& sensor_id, double ref_timestamp, + uint32_t max_ref_time_delta_ms, FrameHandle* frame_handle, + FrameLookupFailureReason* failure_reason) const { + if (frame_handle == nullptr || failure_reason == nullptr) { + return false; + } + + *failure_reason = FrameLookupFailureReason::kNone; + if (sensor_state == nullptr) { + *failure_reason = FrameLookupFailureReason::kBufferEmpty; + return false; + } + + std::lock_guard lock(sensor_state->mutex); + if (sensor_state->frames.empty()) { + *failure_reason = FrameLookupFailureReason::kBufferEmpty; + return false; + } + + std::shared_ptr best_frame; + double best_delta_ms = std::numeric_limits::max(); + double best_timestamp = 0.0; + constexpr double kTimestampTieEpsilonMs = 1e-6; + const double fixed_delay_sec = sensor_state->fixed_delay_initialized + ? sensor_state->fixed_delay_sec + : 0.0; + + // Auxiliary lidar frames can arrive slightly out of order, so choose the + // nearest sample with a linear scan instead of assuming a sorted buffer. + for (const auto& candidate : sensor_state->frames) { + if (candidate == nullptr || candidate->point_cloud == nullptr || + !candidate->pose_prefetch_ok) { + continue; + } + + const double candidate_timestamp = + candidate->point_cloud->measurement_time(); + const double delta_ms = + std::fabs((candidate_timestamp + fixed_delay_sec) - ref_timestamp) * + 1000.0; + const bool same_delta = + std::fabs(delta_ms - best_delta_ms) <= kTimestampTieEpsilonMs; + if (delta_ms + kTimestampTieEpsilonMs < best_delta_ms || + (same_delta && candidate_timestamp < best_timestamp)) { + best_frame = candidate; + best_delta_ms = delta_ms; + best_timestamp = candidate_timestamp; + } + } + + if (best_frame == nullptr) { + *failure_reason = FrameLookupFailureReason::kBufferEmpty; + return false; + } + + if (best_delta_ms > static_cast(max_ref_time_delta_ms)) { + *failure_reason = FrameLookupFailureReason::kTimeDeltaExceeded; + return false; + } + + frame_handle->sensor_id = sensor_id; + frame_handle->point_cloud = best_frame->point_cloud; + frame_handle->buffered_frame = best_frame; + frame_handle->clock_offset_residual_ms = + (ref_timestamp - + (best_frame->point_cloud->measurement_time() + fixed_delay_sec)) * + 1000.0; + frame_handle->overlap_quality_weight = sensor_state->overlap_quality_weight; + return true; +} + +bool LidarUnifiedComponent::ResolveMapToBase(double ref_timestamp_sec, + Eigen::Affine3d* map2base_ref) { + if (map2base_ref == nullptr || base_link_pose_resolver_ == nullptr) { + return false; + } + + if (!base_link_pose_resolver_->Prefetch(ref_timestamp_sec)) { + total_tf_query_failures_.fetch_add(1); + } + + Eigen::Affine3d map_from_base = Eigen::Affine3d::Identity(); + if (!base_link_pose_resolver_->QueryCached(ref_timestamp_sec, + &map_from_base)) { + total_tf_query_failures_.fetch_add(1); + return false; + } + + *map2base_ref = map_from_base.inverse(); + return true; +} + +void LidarUnifiedComponent::UpdateSensorTimingModel( + const FrameHandle& frame_handle, double ref_timestamp_sec, + FrameMetrics* frame_metrics) { + if (frame_handle.is_primary || frame_handle.point_cloud == nullptr) { + return; + } + + const auto sensor_state = GetSensorState(frame_handle.sensor_id); + if (sensor_state == nullptr) { + return; + } + + const double observed_delay_sec = + ref_timestamp_sec - frame_handle.point_cloud->measurement_time(); + std::lock_guard lock(sensor_state->mutex); + if (!sensor_state->fixed_delay_initialized) { + sensor_state->fixed_delay_sec = observed_delay_sec; + sensor_state->fixed_delay_initialized = true; + } else if (std::fabs((observed_delay_sec - sensor_state->fixed_delay_sec) * + 1000.0) <= config_.fixed_delay_update_limit_ms()) { + const double alpha = config_.fixed_delay_ema_alpha(); + sensor_state->fixed_delay_sec = + sensor_state->fixed_delay_sec * (1.0 - alpha) + + observed_delay_sec * alpha; + } + + if (sensor_state->timing_observation_count == 0U) { + sensor_state->smoothed_clock_offset_ms = + frame_handle.clock_offset_residual_ms; + } else { + const double alpha = config_.clock_offset_ema_alpha(); + sensor_state->smoothed_clock_offset_ms = + sensor_state->smoothed_clock_offset_ms * (1.0 - alpha) + + frame_handle.clock_offset_residual_ms * alpha; + } + sensor_state->last_clock_offset_ms = frame_handle.clock_offset_residual_ms; + ++sensor_state->timing_observation_count; + + if (frame_metrics != nullptr) { + frame_metrics->max_abs_clock_offset_ms = + std::max(frame_metrics->max_abs_clock_offset_ms, + std::fabs(frame_handle.clock_offset_residual_ms)); + } +} + +void LidarUnifiedComponent::UpdateOverlapQualityWeights( + const std::vector& frame_handles, + const Eigen::Affine3d& map2base_ref, FrameMetrics* frame_metrics) { + for (const auto& frame_handle : frame_handles) { + if (frame_handle.is_primary || frame_handle.buffered_frame == nullptr) { + continue; + } + + const auto sensor_state = GetSensorState(frame_handle.sensor_id); + if (sensor_state == nullptr) { + continue; + } + + const double observed_weight = EstimateOverlapQualityWeight( + *frame_handle.buffered_frame, map2base_ref); + std::lock_guard lock(sensor_state->mutex); + if (sensor_state->overlap_quality_samples == 0U) { + sensor_state->overlap_quality_weight = observed_weight; + } else { + const double alpha = config_.overlap_quality_ema_alpha(); + sensor_state->overlap_quality_weight = + sensor_state->overlap_quality_weight * (1.0 - alpha) + + observed_weight * alpha; + } + ++sensor_state->overlap_quality_samples; + + if (frame_metrics != nullptr) { + frame_metrics->min_overlap_quality_weight = + std::min(frame_metrics->min_overlap_quality_weight, + sensor_state->overlap_quality_weight); + } + } +} + +double LidarUnifiedComponent::EstimateOverlapQualityWeight( + const BufferedFrame& buffered_frame, + const Eigen::Affine3d& map2base_ref) const { + if (buffered_frame.point_cloud == nullptr || + buffered_frame.motion_sample_times.empty() || + buffered_frame.motion_sample_times.size() != + buffered_frame.motion_poses.size()) { + return 1.0; + } + + const size_t stride = std::max( + 1, static_cast(config_.overlap_quality_sample_stride())); + size_t sampled_points = 0; + size_t overlap_points = 0; + for (size_t index = 0; + index < static_cast(buffered_frame.point_cloud->point_size()); + index += stride) { + PointXYZIT transformed_point; + if (!TransformPointToBase( + buffered_frame.point_cloud->point(static_cast(index)), + buffered_frame.point_cloud->measurement_time(), + buffered_frame.motion_sample_times, buffered_frame.motion_poses, + map2base_ref, &transformed_point)) { + continue; + } + ++sampled_points; + if (IsPointInOverlapRegion(transformed_point)) { + ++overlap_points; + } + } + + if (sampled_points == 0U) { + return 1.0; + } + return static_cast(overlap_points) / + static_cast(sampled_points); +} + +bool LidarUnifiedComponent::IsPointInOverlapRegion( + const ::apollo::drivers::PointXYZIT& point) const { + return point.x() <= config_.overlap_region_forward_x() && + point.x() >= config_.overlap_region_backward_x() && + point.y() <= config_.overlap_region_left_y() && + point.y() >= config_.overlap_region_right_y() && + point.z() <= config_.overlap_region_max_z() && + point.z() >= config_.overlap_region_min_z(); +} + +bool LidarUnifiedComponent::BuildUnifiedPointCloud( + const PointCloudConstPtr& main_frame, + const std::vector& frame_handles, FrameMetrics* frame_metrics, + std::shared_ptr<::apollo::drivers::PointCloud>* output) { + if (main_frame == nullptr || frame_metrics == nullptr || output == nullptr || + frame_handles.empty() || deskew_policy_ == nullptr || + fusion_policy_ == nullptr || filter_policy_ == nullptr) { + return false; + } + + std::vector contexts; + std::vector> motion_sample_times; + std::vector> motion_poses; + contexts.reserve(frame_handles.size()); + motion_sample_times.reserve(frame_handles.size()); + motion_poses.reserve(frame_handles.size()); + + size_t required_points = 0; + if (!pose_bins_builder_.Build(frame_handles, deskew_policy_.get(), &contexts, + &motion_sample_times, &motion_poses, + &required_points)) { + AERROR << "No valid sensor context for fusion"; + return false; + } + + frame_metrics->total_input_points = required_points; + frame_metrics->matched_sensor_count = contexts.size(); + + if (required_points > full_pointcloud_buffer_.size()) { + AWARN << "Input points exceed preallocated buffer, required=" + << required_points << ", capacity=" << full_pointcloud_buffer_.size(); + } + + Eigen::Affine3d map2base_ref = Eigen::Affine3d::Identity(); + if (!ResolveMapToBase(main_frame->measurement_time(), &map2base_ref)) { + AERROR << "Failed to resolve cached base pose at ref timestamp " + << main_frame->measurement_time(); + return false; + } + UpdateOverlapQualityWeights(frame_handles, map2base_ref, frame_metrics); + + PointCloudBuffer fused_buffer; + fused_buffer.data_ptr = full_pointcloud_buffer_.data(); + fused_buffer.capacity = full_pointcloud_buffer_.size(); + fused_buffer.valid_count = 0; + fused_buffer.item_size = sizeof(apollo::drivers::PointXYZIT); + fused_buffer.device_type = MemoryDeviceType::kHost; + fused_buffer.device_id = + config_.compute_mode() == LidarUnifiedComponentConfig::COMPUTE_MODE_GPU + ? static_cast(config_.gpu_device_id()) + : -1; + + if (!fusion_policy_->FuseToBaseLink(main_frame->measurement_time(), + map2base_ref, contexts, motion_poses, + motion_sample_times, &fused_buffer)) { + AERROR << "Failed to fuse point clouds for ref timestamp " + << main_frame->measurement_time(); + return false; + } + + const size_t compact_points = fused_buffer.valid_count; + size_t ego_filtered_points = 0; + size_t voxel_filtered_points = 0; + const size_t valid_size = filter_policy_->ApplyFilters( + &fused_buffer, &ego_filtered_points, &voxel_filtered_points); + frame_metrics->compact_points = compact_points; + frame_metrics->ego_filtered_points = ego_filtered_points; + frame_metrics->voxel_filtered_points = voxel_filtered_points; + frame_metrics->output_points = valid_size; + + auto unified = std::make_shared<::apollo::drivers::PointCloud>(); + unified->mutable_header()->CopyFrom(main_frame->header()); + unified->mutable_header()->set_frame_id(config_.base_link_frame_id()); + unified->mutable_header()->set_module_name("lidar_unified_processor"); + unified->mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); + unified->mutable_header()->set_sequence_num(sequence_num_.fetch_add(1) + 1); + unified->set_frame_id(config_.base_link_frame_id()); + unified->set_measurement_time(main_frame->measurement_time()); + unified->set_is_dense(true); + unified->mutable_point()->Reserve(static_cast(valid_size)); + + for (size_t i = 0; i < valid_size; ++i) { + const auto& point = full_pointcloud_buffer_[i]; + auto* out_pt = unified->add_point(); + out_pt->set_x(point.x()); + out_pt->set_y(point.y()); + out_pt->set_z(point.z()); + out_pt->set_intensity(point.intensity()); + out_pt->set_timestamp(point.timestamp()); + } + + unified->set_height(1); + unified->set_width(unified->point_size()); + + *output = unified; + return true; +} +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/lidar_unified_component.h b/modules/drivers/lidar/processor/lidar_unified_component.h new file mode 100644 index 00000000..26e50235 --- /dev/null +++ b/modules/drivers/lidar/processor/lidar_unified_component.h @@ -0,0 +1,189 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "Eigen/Eigen" + +#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h" +#include "modules/drivers/lidar/proto/lidar_unified_component_config.pb.h" + +#include "cyber/cyber.h" +#include "modules/drivers/lidar/processor/control/frame_handle.h" +#include "modules/drivers/lidar/processor/control/pose_bins_builder.h" +#include "modules/drivers/lidar/processor/control/sync_gate.h" +#include "modules/drivers/lidar/processor/policy/lidar_policy_interface.h" +#include "modules/drivers/lidar/processor/safety/degrade_policy.h" +#include "modules/drivers/lidar/processor/safety/dtc_reporter.h" +#include "modules/drivers/lidar/processor/safety/ts_sanity.h" +#include "modules/transform/buffer.h" +#include "modules/transform/timed_transform_resolver.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +class LidarUnifiedComponent + : public apollo::cyber::Component<::apollo::drivers::PointCloud> { + public: + using PointCloudConstPtr = + std::shared_ptr; + + bool Init() override; + bool Proc(const std::shared_ptr<::apollo::drivers::PointCloud>& point_cloud) + override; + + bool OnReceiveMainLidar(const PointCloudConstPtr& point_cloud); + + private: + friend class LidarUnifiedComponentTest_RejectsPrimarySensorIdDrift_Test; + friend class + LidarUnifiedComponentTest_FindsNearestFrameFromOutOfOrderBuffer_Test; + friend class + LidarUnifiedComponentTest_ReportsTimeDeltaExceededForNearestFrame_Test; + friend class + LidarUnifiedComponentTest_AppliesFixedDelayDuringFrameLookup_Test; + friend class LidarUnifiedComponentTest_UpdatesSensorTimingModel_Test; + friend class + LidarUnifiedComponentTest_UpdatesLargeFixedDelayWhenInnovationIsWithinLimit_Test; + friend class + LidarUnifiedComponentTest_CollectNearestFramesSkipsLowQualityAuxiliary_Test; + friend class LidarUnifiedComponentTest_EstimatesOverlapQualityWeight_Test; + + enum class FrameLookupFailureReason { + kNone = 0, + kBufferEmpty = 1, + kTimeDeltaExceeded = 2, + }; + + struct SensorState { + explicit SensorState(size_t capacity = 1) : frames(capacity) {} + + boost::circular_buffer> frames; + std::unique_ptr pose_resolver; + double fixed_delay_sec = 0.0; + bool fixed_delay_initialized = false; + uint64_t timing_observation_count = 0; + double last_clock_offset_ms = 0.0; + double smoothed_clock_offset_ms = 0.0; + double overlap_quality_weight = 1.0; + uint64_t overlap_quality_samples = 0; + uint64_t pose_prefetch_timeout_count = 0; + mutable std::mutex mutex; + }; + + struct SensorInput { + std::string topic_name; + }; + + struct FrameMetrics { + size_t expected_sensor_count = 0; + size_t matched_sensor_count = 0; + size_t missing_auxiliary_count = 0; + size_t time_delta_exceeded_count = 0; + size_t total_input_points = 0; + size_t compact_points = 0; + size_t voxel_filtered_points = 0; + size_t ego_filtered_points = 0; + size_t output_points = 0; + double max_abs_clock_offset_ms = 0.0; + double min_overlap_quality_weight = 1.0; + }; + + bool PrepareBufferedFrame(const std::string& sensor_id, + const PointCloudConstPtr& point_cloud, + std::shared_ptr* buffered_frame); + void PushToBuffer(const std::string& sensor_id, + const std::shared_ptr& buffered_frame); + void OnAuxiliaryLidarMessage(const std::string& topic_name, + const PointCloudConstPtr& point_cloud); + bool ValidateConfig() const; + std::string ResolveSensorId(const PointCloudConstPtr& point_cloud, + const std::string& topic_name) const; + std::string MakeFallbackSensorId(const std::string& topic_name) const; + std::shared_ptr EnsureSensorState(const std::string& sensor_id); + std::shared_ptr GetSensorState( + const std::string& sensor_id) const; + + bool CollectNearestFrames(double ref_timestamp, + const std::string& primary_sensor_id, + std::vector* frame_handles, + FrameMetrics* frame_metrics); + bool FindNearestFrame(const std::shared_ptr& sensor_state, + const std::string& sensor_id, double ref_timestamp, + uint32_t max_ref_time_delta_ms, + FrameHandle* frame_handle, + FrameLookupFailureReason* failure_reason) const; + void UpdateSensorTimingModel(const FrameHandle& frame_handle, + double ref_timestamp_sec, + FrameMetrics* frame_metrics); + bool ResolveMapToBase(double ref_timestamp_sec, + Eigen::Affine3d* map2base_ref); + void UpdateOverlapQualityWeights( + const std::vector& frame_handles, + const Eigen::Affine3d& map2base_ref, FrameMetrics* frame_metrics); + double EstimateOverlapQualityWeight( + const BufferedFrame& buffered_frame, + const Eigen::Affine3d& map2base_ref) const; + bool IsPointInOverlapRegion(const ::apollo::drivers::PointXYZIT& point) const; + + bool BuildUnifiedPointCloud( + const PointCloudConstPtr& main_frame, + const std::vector& frame_handles, + FrameMetrics* frame_metrics, + std::shared_ptr<::apollo::drivers::PointCloud>* output); + void LogFrameMetrics(const FrameMetrics& frame_metrics); + + private: + LidarUnifiedComponentConfig config_; + std::string primary_sensor_id_; + size_t sensor_buffer_capacity_ = 1; + + apollo::transform::Buffer* tf_buffer_ = nullptr; + std::unique_ptr + base_link_pose_resolver_; + mutable std::mutex sensor_registry_mutex_; + + std::map> sensor_states_; + std::map auxiliary_sensor_ids_by_topic_; + std::map< + std::string, + std::shared_ptr>> + auxiliary_readers_; + std::vector auxiliary_inputs_; + + std::unique_ptr deskew_policy_; + std::unique_ptr fusion_policy_; + std::unique_ptr filter_policy_; + PoseBinsBuilder pose_bins_builder_; + SyncGate sync_gate_; + TsSanity ts_sanity_; + DegradePolicy degrade_policy_; + DtcReporter dtc_reporter_; + std::shared_ptr> writer_; + + std::vector<::apollo::drivers::PointXYZIT> full_pointcloud_buffer_; + std::atomic sequence_num_{0}; + std::atomic frames_total_{0}; + std::atomic total_input_points_{0}; + std::atomic total_output_points_{0}; + std::atomic total_missing_auxiliary_frames_{0}; + std::atomic total_time_delta_exceeded_{0}; + std::atomic total_pose_prefetch_timeouts_{0}; + mutable std::atomic total_tf_query_failures_{0}; +}; + +CYBER_REGISTER_COMPONENT(LidarUnifiedComponent) + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/lidar_unified_component_config.cc b/modules/drivers/lidar/processor/lidar_unified_component_config.cc new file mode 100644 index 00000000..aae0a92c --- /dev/null +++ b/modules/drivers/lidar/processor/lidar_unified_component_config.cc @@ -0,0 +1,151 @@ +#include "modules/drivers/lidar/processor/lidar_unified_component.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +bool LidarUnifiedComponent::ValidateConfig() const { + if (config_.output_channel().empty()) { + AERROR << "output_channel is required"; + return false; + } + + if (config_.base_link_frame_id().empty()) { + AERROR << "base_link_frame_id is required"; + return false; + } + + if (config_.map_frame_id().empty()) { + AERROR << "map_frame_id is required"; + return false; + } + + if (config_.max_ref_time_delta_ms() == 0) { + AERROR << "max_ref_time_delta_ms must be > 0"; + return false; + } + + if (config_.motion_compensation_bins() == 0) { + AERROR << "motion_compensation_bins must be > 0"; + return false; + } + + if (config_.sensor_buffer_size() == 0) { + AERROR << "sensor_buffer_size must be > 0"; + return false; + } + + if (config_.max_full_pointcloud_points() == 0) { + AERROR << "max_full_pointcloud_points must be > 0"; + return false; + } + + if (config_.metrics_log_interval() == 0) { + AERROR << "metrics_log_interval must be > 0"; + return false; + } + + if (config_.voxel_size() <= 0.0) { + AERROR << "voxel_size must be > 0"; + return false; + } + + if (config_.sensor_pose_cache_duration_sec() <= 0.0) { + AERROR << "sensor_pose_cache_duration_sec must be > 0"; + return false; + } + + if (config_.sensor_pose_cache_max_extrapolation_sec() < 0.0) { + AERROR << "sensor_pose_cache_max_extrapolation_sec must be >= 0"; + return false; + } + + if (config_.sensor_pose_query_timeout_sec() < 0.0) { + AERROR << "sensor_pose_query_timeout_sec must be >= 0"; + return false; + } + + if (config_.fixed_delay_ema_alpha() <= 0.0 || + config_.fixed_delay_ema_alpha() > 1.0) { + AERROR << "fixed_delay_ema_alpha must be in (0, 1]"; + return false; + } + + if (config_.fixed_delay_update_limit_ms() < 0.0) { + AERROR << "fixed_delay_update_limit_ms must be >= 0"; + return false; + } + + if (config_.clock_offset_ema_alpha() <= 0.0 || + config_.clock_offset_ema_alpha() > 1.0) { + AERROR << "clock_offset_ema_alpha must be in (0, 1]"; + return false; + } + + if (config_.overlap_quality_ema_alpha() <= 0.0 || + config_.overlap_quality_ema_alpha() > 1.0) { + AERROR << "overlap_quality_ema_alpha must be in (0, 1]"; + return false; + } + + if (config_.auxiliary_min_overlap_quality_weight() < 0.0 || + config_.auxiliary_min_overlap_quality_weight() > 1.0) { + AERROR << "auxiliary_min_overlap_quality_weight must be in [0, 1]"; + return false; + } + + if (config_.overlap_quality_sample_stride() == 0) { + AERROR << "overlap_quality_sample_stride must be > 0"; + return false; + } + + if (config_.overlap_region_forward_x() < + config_.overlap_region_backward_x()) { + AERROR << "overlap_region_forward_x must be >= overlap_region_backward_x"; + return false; + } + + if (config_.overlap_region_left_y() < config_.overlap_region_right_y()) { + AERROR << "overlap_region_left_y must be >= overlap_region_right_y"; + return false; + } + + if (config_.overlap_region_max_z() < config_.overlap_region_min_z()) { + AERROR << "overlap_region_max_z must be >= overlap_region_min_z"; + return false; + } + + if (config_.ts_sanity_enabled()) { + if (config_.ts_sanity_min_interval_ms() == 0) { + AERROR << "ts_sanity_min_interval_ms must be > 0"; + return false; + } + if (config_.ts_sanity_max_interval_ms() < + config_.ts_sanity_min_interval_ms()) { + AERROR + << "ts_sanity_max_interval_ms must be >= ts_sanity_min_interval_ms"; + return false; + } + if (config_.ts_sanity_max_jump_ms() < config_.ts_sanity_max_interval_ms()) { + AERROR << "ts_sanity_max_jump_ms must be >= ts_sanity_max_interval_ms"; + return false; + } + if (config_.ts_sanity_max_consecutive_errors() == 0) { + AERROR << "ts_sanity_max_consecutive_errors must be > 0"; + return false; + } + } + + for (const auto& input_cfg : config_.auxiliary_lidar_inputs()) { + if (input_cfg.topic_name().empty()) { + AERROR << "auxiliary_lidar_inputs.topic_name is required"; + return false; + } + } + + return true; +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/lidar_unified_component_ingest.cc b/modules/drivers/lidar/processor/lidar_unified_component_ingest.cc new file mode 100644 index 00000000..502adad6 --- /dev/null +++ b/modules/drivers/lidar/processor/lidar_unified_component_ingest.cc @@ -0,0 +1,251 @@ +#include +#include +#include +#include +#include + +#include "modules/drivers/lidar/processor/lidar_unified_component.h" +#include "modules/drivers/lidar/processor/policy/lidar_policy_common.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +namespace { +constexpr char kPrimaryTopicFallback[] = "primary_lidar"; +constexpr int kPosePrefetchLogFrequency = 10; + +std::string FormatTimestampForLog(double timestamp_sec) { + std::ostringstream stream; + stream << std::fixed << std::setprecision(9) << timestamp_sec << "s"; + if (timestamp_sec > 0.0) { + stream << " [" << cyber::Time(timestamp_sec).ToString() << "]"; + } + return stream.str(); +} + +std::string FormatDeltaForLog(double delta_sec) { + const double abs_delta_sec = std::fabs(delta_sec); + std::ostringstream stream; + stream << std::showpos << std::fixed + << std::setprecision(abs_delta_sec >= 1.0 ? 3 : 6) << delta_sec + << "s" << std::noshowpos; + if (abs_delta_sec >= 86400.0) { + stream << " (~" << std::fixed << std::setprecision(3) + << abs_delta_sec / 86400.0 << " days)"; + } else if (abs_delta_sec >= 3600.0) { + stream << " (~" << std::fixed << std::setprecision(3) + << abs_delta_sec / 3600.0 << " h)"; + } else if (abs_delta_sec >= 60.0) { + stream << " (~" << std::fixed << std::setprecision(3) + << abs_delta_sec / 60.0 << " min)"; + } else if (abs_delta_sec > 0.0 && abs_delta_sec < 1.0) { + stream << " (~" << std::fixed << std::setprecision(3) + << abs_delta_sec * 1000.0 << " ms)"; + } + return stream.str(); +} + +const char* DescribeTimeRelation(double delta_sec) { + return std::fabs(delta_sec) <= 1e-6 ? "aligned" + : (delta_sec > 0.0 ? "ahead" + : "behind"); +} + +std::string BuildPointCloudTimeSummary(const PointCloud& point_cloud) { + double min_point_time_sec = point_cloud.measurement_time(); + double max_point_time_sec = point_cloud.measurement_time(); + ResolvePointTimestampBounds(point_cloud, &min_point_time_sec, + &max_point_time_sec); + + const double measurement_time_sec = point_cloud.measurement_time(); + const double now_sec = cyber::Time::Now().ToSecond(); + const double measurement_vs_now_sec = measurement_time_sec - now_sec; + + std::ostringstream message; + message << "measurement=" << FormatTimestampForLog(measurement_time_sec) + << ", point_range=[" << FormatTimestampForLog(min_point_time_sec) + << ", " << FormatTimestampForLog(max_point_time_sec) << "]" + << ", measurement_vs_now=" + << FormatDeltaForLog(measurement_vs_now_sec) << " (" + << DescribeTimeRelation(measurement_vs_now_sec) << ")"; + if (std::fabs(max_point_time_sec - min_point_time_sec) > 1e-6) { + message << ", scan_span=" + << FormatDeltaForLog(max_point_time_sec - min_point_time_sec); + } + return message.str(); +} + +apollo::transform::TimedTransformResolverOptions BuildTransformResolverOptions( + const LidarUnifiedComponentConfig& config) { + apollo::transform::TimedTransformResolverOptions options; + options.query_timeout_sec = + static_cast(config.sensor_pose_query_timeout_sec()); + options.cache_duration_sec = config.sensor_pose_cache_duration_sec(); + options.max_extrapolation_sec = + config.sensor_pose_cache_max_extrapolation_sec(); + return options; +} +} // namespace + +std::string LidarUnifiedComponent::MakeFallbackSensorId( + const std::string& topic_name) const { + if (topic_name.empty()) { + return kPrimaryTopicFallback; + } + + std::string sensor_id; + sensor_id.reserve(topic_name.size()); + for (const char ch : topic_name) { + if (std::isalnum(static_cast(ch))) { + sensor_id.push_back(ch); + } else if (!sensor_id.empty() && sensor_id.back() != '_') { + sensor_id.push_back('_'); + } + } + + while (!sensor_id.empty() && sensor_id.back() == '_') { + sensor_id.pop_back(); + } + + return sensor_id.empty() ? kPrimaryTopicFallback : sensor_id; +} + +std::string LidarUnifiedComponent::ResolveSensorId( + const PointCloudConstPtr& point_cloud, + const std::string& topic_name) const { + if (point_cloud != nullptr) { + if (!point_cloud->frame_id().empty()) { + return point_cloud->frame_id(); + } + if (point_cloud->has_header() && + !point_cloud->header().frame_id().empty()) { + return point_cloud->header().frame_id(); + } + } + + return MakeFallbackSensorId(topic_name); +} + +bool LidarUnifiedComponent::PrepareBufferedFrame( + const std::string& sensor_id, const PointCloudConstPtr& point_cloud, + std::shared_ptr* buffered_frame) { + if (sensor_id.empty() || point_cloud == nullptr || + buffered_frame == nullptr) { + return false; + } + + const auto sensor_state = EnsureSensorState(sensor_id); + if (sensor_state == nullptr || sensor_state->pose_resolver == nullptr) { + return false; + } + + const size_t bins = std::max( + 1, static_cast(config_.motion_compensation_bins())); + auto frame = std::make_shared(); + frame->point_cloud = point_cloud; + bool used_measurement_time_fallback = false; + if (!BuildMotionSampleTimes(*point_cloud, bins, + config_.compute_mode() == + LidarUnifiedComponentConfig::COMPUTE_MODE_GPU, + static_cast(config_.gpu_device_id()), + &frame->motion_sample_times, + &used_measurement_time_fallback)) { + AWARN << "Failed to build motion sample times for sensor " << sensor_id; + return false; + } + + if (used_measurement_time_fallback) { + AINFO_EVERY(kPosePrefetchLogFrequency) + << "Fallback to measurement_time and disable intra-frame deskew. " + << "sensor=" << sensor_id << ", target=" << config_.map_frame_id() + << ", " << BuildPointCloudTimeSummary(*point_cloud); + } + + if (!sensor_state->pose_resolver->PrefetchBatch(frame->motion_sample_times)) { + std::lock_guard lock(sensor_state->mutex); + ++sensor_state->pose_prefetch_timeout_count; + total_pose_prefetch_timeouts_.fetch_add(1); + } + + apollo::transform::TransformResolveStatus cache_status = + apollo::transform::TransformResolveStatus::kOk; + if (!sensor_state->pose_resolver->QueryCachedBatchStrict( + frame->motion_sample_times, &frame->motion_poses, &cache_status)) { + AINFO_EVERY(kPosePrefetchLogFrequency) + << "Pose prefetch unavailable. sensor=" << sensor_id + << ", target=" << config_.map_frame_id() + << ", status=" << static_cast(cache_status) << ", " + << BuildPointCloudTimeSummary(*point_cloud); + total_tf_query_failures_.fetch_add(1); + return false; + } + + frame->pose_prefetch_ok = true; + *buffered_frame = frame; + return true; +} + +std::shared_ptr +LidarUnifiedComponent::EnsureSensorState(const std::string& sensor_id) { + std::lock_guard lock(sensor_registry_mutex_); + auto it = sensor_states_.find(sensor_id); + if (it != sensor_states_.end()) { + return it->second; + } + + auto sensor_state = std::make_shared(sensor_buffer_capacity_); + sensor_state->pose_resolver = + std::make_unique( + tf_buffer_, config_.map_frame_id(), sensor_id, + BuildTransformResolverOptions(config_)); + sensor_states_.emplace(sensor_id, sensor_state); + return sensor_state; +} + +std::shared_ptr +LidarUnifiedComponent::GetSensorState(const std::string& sensor_id) const { + std::lock_guard lock(sensor_registry_mutex_); + const auto it = sensor_states_.find(sensor_id); + if (it == sensor_states_.end()) { + return nullptr; + } + return it->second; +} + +void LidarUnifiedComponent::OnAuxiliaryLidarMessage( + const std::string& topic_name, const PointCloudConstPtr& point_cloud) { + if (point_cloud == nullptr) { + return; + } + + const std::string sensor_id = ResolveSensorId(point_cloud, topic_name); + if (sensor_id.empty()) { + AWARN << "Skip auxiliary lidar message due to empty sensor id. topic=" + << topic_name; + return; + } + + std::shared_ptr buffered_frame; + if (!PrepareBufferedFrame(sensor_id, point_cloud, &buffered_frame)) { + AINFO_EVERY(kPosePrefetchLogFrequency) + << "Skip auxiliary lidar frame due to pose prefetch failure. sensor=" + << sensor_id << ", topic=" << topic_name; + return; + } + { + std::lock_guard lock(sensor_registry_mutex_); + auto& mapped_sensor_id = auxiliary_sensor_ids_by_topic_[topic_name]; + if (!mapped_sensor_id.empty() && mapped_sensor_id != sensor_id) { + AWARN << "Auxiliary topic sensor id changed from " << mapped_sensor_id + << " to " << sensor_id << ", topic=" << topic_name; + } + mapped_sensor_id = sensor_id; + } + + PushToBuffer(sensor_id, buffered_frame); +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/lidar_unified_component_observability.cc b/modules/drivers/lidar/processor/lidar_unified_component_observability.cc new file mode 100644 index 00000000..95299902 --- /dev/null +++ b/modules/drivers/lidar/processor/lidar_unified_component_observability.cc @@ -0,0 +1,83 @@ +#include + +#include "modules/drivers/lidar/processor/lidar_unified_component.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +namespace { + +const char* DegradeModeName(DegradeMode mode) { + switch (mode) { + case DegradeMode::kNormal: + return "normal"; + case DegradeMode::kSingleLidar: + return "single_lidar"; + default: + return "unknown"; + } +} + +} // namespace + +void LidarUnifiedComponent::LogFrameMetrics(const FrameMetrics& frame_metrics) { + const uint64_t frame_index = frames_total_.fetch_add(1) + 1; + total_input_points_.fetch_add(frame_metrics.total_input_points); + total_output_points_.fetch_add(frame_metrics.output_points); + total_missing_auxiliary_frames_.fetch_add( + frame_metrics.missing_auxiliary_count); + total_time_delta_exceeded_.fetch_add(frame_metrics.time_delta_exceeded_count); + + const uint32_t log_interval = + std::max(1, config_.metrics_log_interval()); + if (frame_index % log_interval != 0) { + return; + } + + const uint64_t total_frames = std::max(1, frames_total_.load()); + const uint64_t total_input = + std::max(1, total_input_points_.load()); + const uint64_t total_output = total_output_points_.load(); + const uint64_t total_aux_missing = total_missing_auxiliary_frames_.load(); + const uint64_t total_time_delta = total_time_delta_exceeded_.load(); + const uint64_t total_pose_prefetch_timeouts = + total_pose_prefetch_timeouts_.load(); + const uint64_t total_tf_failures = total_tf_query_failures_.load(); + const uint64_t ts_anomalies = dtc_reporter_.ts_anomaly_count(); + const uint64_t degrade_transitions = dtc_reporter_.degrade_transition_count(); + + AINFO << "LidarUnifiedProcessor metrics: frame=" << frame_index + << ", matched_sensors=" << frame_metrics.matched_sensor_count << "/" + << frame_metrics.expected_sensor_count + << ", degrade_mode=" << DegradeModeName(degrade_policy_.CurrentMode()) + << ", missing_aux=" << frame_metrics.missing_auxiliary_count + << ", input_points=" << frame_metrics.total_input_points + << ", compact_points=" << frame_metrics.compact_points + << ", voxel_filtered_points=" << frame_metrics.voxel_filtered_points + << ", ego_filtered_points=" << frame_metrics.ego_filtered_points + << ", output_points=" << frame_metrics.output_points + << ", max_abs_clock_offset_ms=" << frame_metrics.max_abs_clock_offset_ms + << ", min_overlap_quality_weight=" + << frame_metrics.min_overlap_quality_weight + << ", ts_anomalies=" << ts_anomalies + << ", degrade_transitions=" << degrade_transitions + << ", avg_output_ratio=" + << static_cast(total_output) / static_cast(total_input) + << ", avg_aux_drop_per_frame=" + << static_cast(total_aux_missing) / + static_cast(total_frames) + << ", avg_time_window_violations=" + << static_cast(total_time_delta) / + static_cast(total_frames) + << ", avg_pose_prefetch_timeouts=" + << static_cast(total_pose_prefetch_timeouts) / + static_cast(total_frames) + << ", avg_tf_failures=" + << static_cast(total_tf_failures) / + static_cast(total_frames); +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/cpu_lidar_policy.h b/modules/drivers/lidar/processor/policy/cpu_lidar_policy.h new file mode 100644 index 00000000..9a1c1002 --- /dev/null +++ b/modules/drivers/lidar/processor/policy/cpu_lidar_policy.h @@ -0,0 +1,56 @@ +#pragma once + +#include "modules/drivers/lidar/processor/policy/lidar_policy_interface.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +class CpuLidarDeskewPolicy : public LidarDeskewPolicy { + public: + bool Init(const LidarUnifiedComponentConfig& config, + apollo::transform::BufferInterface* tf_buffer) override; + + bool ComputeMotionCompensationPoses( + const SensorFrameContext& frame_context, + std::vector* sample_times, + std::vector* poses) override; + + private: + LidarUnifiedComponentConfig config_; + apollo::transform::BufferInterface* tf_buffer_ = nullptr; +}; + +class CpuLidarFusionPolicy : public LidarFusionPolicy { + public: + bool Init(const LidarUnifiedComponentConfig& config, + apollo::transform::BufferInterface* tf_buffer) override; + + bool FuseToBaseLink( + double reference_timestamp_sec, const Eigen::Affine3d& map2base_ref, + const std::vector& frames, + const std::vector>& frames_motion_poses, + const std::vector>& frames_motion_times, + PointCloudBuffer* output_buffer) override; + + private: + LidarUnifiedComponentConfig config_; + apollo::transform::BufferInterface* tf_buffer_ = nullptr; +}; + +class CpuLidarFilterPolicy : public LidarFilterPolicy { + public: + bool Init(const LidarUnifiedComponentConfig& config) override; + size_t ApplyFilters(PointCloudBuffer* io_buffer, size_t* ego_filtered_count, + size_t* voxel_filtered_count) override; + + private: + size_t ApplyVoxelFilter(PointCloudBuffer* io_buffer); + size_t ApplyEgoQueryFilter(PointCloudBuffer* io_buffer); + + LidarUnifiedComponentConfig config_; +}; + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/gpu_lidar_policy.h b/modules/drivers/lidar/processor/policy/gpu_lidar_policy.h new file mode 100644 index 00000000..6e1cfe33 --- /dev/null +++ b/modules/drivers/lidar/processor/policy/gpu_lidar_policy.h @@ -0,0 +1,93 @@ +#pragma once + +#include +#include +#include + +#include "modules/drivers/lidar/processor/policy/lidar_policy_cuda_kernels.h" +#include "modules/drivers/lidar/processor/policy/lidar_policy_interface.h" + +// Forward declaration of CUDA kernels or utility structures +struct cudaStream_t; + +namespace apollo { +namespace drivers { +namespace lidar { + +/// @brief GPU acceleration policy for generating Motion Compensation +/// transforms. Typically poses are sparse enough to compute on CPU, but this +/// allows for pure GPU pipelines. +class GpuLidarDeskewPolicy : public LidarDeskewPolicy { + public: + bool Init(const LidarUnifiedComponentConfig& config, + apollo::transform::BufferInterface* tf_buffer) override; + + bool ComputeMotionCompensationPoses( + const SensorFrameContext& frame_context, + std::vector* sample_times, + std::vector* poses) override; + + private: + LidarUnifiedComponentConfig config_; + apollo::transform::BufferInterface* tf_buffer_ = nullptr; +}; + +/// @brief GPU parallelized spatial fusion and deskew execution. +class GpuLidarFusionPolicy : public LidarFusionPolicy { + public: + GpuLidarFusionPolicy(); + ~GpuLidarFusionPolicy(); + + bool Init(const LidarUnifiedComponentConfig& config, + apollo::transform::BufferInterface* tf_buffer) override; + + bool FuseToBaseLink( + double reference_timestamp_sec, const Eigen::Affine3d& map2base_ref, + const std::vector& frames, + const std::vector>& frames_motion_poses, + const std::vector>& frames_motion_times, + PointCloudBuffer* output_buffer) override; + + private: + LidarUnifiedComponentConfig config_; + apollo::transform::BufferInterface* tf_buffer_ = nullptr; + cudaStream_t* cuda_stream_ = nullptr; // Asynchronous CUDA execution stream + std::mutex scratch_mutex_; + std::vector host_input_points_; + std::vector host_fused_points_; + std::vector host_pose_buffer_; + std::atomic metrics_calls_{0}; + std::atomic metrics_input_points_{0}; + std::atomic metrics_output_points_{0}; + std::atomic metrics_elapsed_ns_{0}; +}; + +/// @brief Extremely fast GPU-based bounding box filtering & parallel bitonic +/// voxel downsample. +class GpuLidarFilterPolicy : public LidarFilterPolicy { + public: + GpuLidarFilterPolicy(); + ~GpuLidarFilterPolicy(); + + bool Init(const LidarUnifiedComponentConfig& config) override; + + size_t ApplyFilters(PointCloudBuffer* io_buffer, size_t* ego_filtered_count, + size_t* voxel_filtered_count) override; + + private: + LidarUnifiedComponentConfig config_; + cudaStream_t* cuda_stream_ = nullptr; + void* d_voxel_hash_table_ = nullptr; // Persistent GPU Hash Table memory + std::mutex scratch_mutex_; + std::vector host_input_points_; + std::vector host_ego_filtered_points_; + std::vector host_centroid_points_; + std::atomic metrics_calls_{0}; + std::atomic metrics_input_points_{0}; + std::atomic metrics_output_points_{0}; + std::atomic metrics_elapsed_ns_{0}; +}; + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/lidar_policy_common.cc b/modules/drivers/lidar/processor/policy/lidar_policy_common.cc new file mode 100644 index 00000000..80ee33d5 --- /dev/null +++ b/modules/drivers/lidar/processor/policy/lidar_policy_common.cc @@ -0,0 +1,425 @@ +#include "modules/drivers/lidar/processor/policy/lidar_policy_common.h" + +#include +#include +#include +#include +#include +#include + +#include "cyber/cyber.h" +#include "modules/transform/transform_query.h" +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED +#include "modules/drivers/lidar/processor/policy/lidar_policy_cuda_kernels.h" +#endif + +namespace apollo { +namespace drivers { +namespace lidar { + +namespace { + +constexpr double kDeskewTimestampFallbackThresholdSec = 1.0; + +void FillUniformMotionSampleTimes(double timestamp_sec, size_t bins, + std::vector* sample_times) { + sample_times->assign(bins, timestamp_sec); +} + +std::string FormatTimestampSummary(double timestamp_sec) { + std::ostringstream stream; + stream << std::fixed << std::setprecision(9) << timestamp_sec << "s"; + if (timestamp_sec > 0.0) { + stream << " [" << cyber::Time(timestamp_sec).ToString() << "]"; + } + return stream.str(); +} + +bool ShouldFallbackToMeasurementTime(double measurement_time_sec, + double point_min_time_sec, + double point_max_time_sec) { + if (!std::isfinite(measurement_time_sec) || measurement_time_sec <= 0.0 || + !std::isfinite(point_min_time_sec) || !std::isfinite(point_max_time_sec)) { + return false; + } + + return std::fabs(point_min_time_sec - measurement_time_sec) > + kDeskewTimestampFallbackThresholdSec || + std::fabs(point_max_time_sec - measurement_time_sec) > + kDeskewTimestampFallbackThresholdSec; +} + +} // namespace + +bool ResolvePointTimestampBounds(const PointCloud& cloud, double* min_sec, + double* max_sec) { + if (min_sec == nullptr || max_sec == nullptr) { + return false; + } + + double local_min = std::numeric_limits::max(); + double local_max = std::numeric_limits::lowest(); + for (const auto& point : cloud.point()) { + const double ts = ResolvePointTimestampSec(point, cloud.measurement_time()); + local_min = std::min(local_min, ts); + local_max = std::max(local_max, ts); + } + if (local_min > local_max) { + local_min = cloud.measurement_time(); + local_max = cloud.measurement_time(); + } + + *min_sec = local_min; + *max_sec = local_max; + return true; +} + +bool BuildMotionSampleTimes(const PointCloud& cloud, size_t bins, + bool use_gpu_timestamp_range, int gpu_device_id, + std::vector* sample_times, + bool* used_measurement_time_fallback) { + if (bins == 0U || sample_times == nullptr) { + return false; + } + + if (used_measurement_time_fallback != nullptr) { + *used_measurement_time_fallback = false; + } + + double local_min = 0.0; + double local_max = 0.0; +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED + if (use_gpu_timestamp_range) { + std::vector timestamps; + timestamps.reserve(static_cast(cloud.point_size())); + const uint64_t fallback_ts = + static_cast(cloud.measurement_time() * kSecondToNano); + for (const auto& point : cloud.point()) { + timestamps.push_back(point.timestamp() == 0U ? fallback_ts + : point.timestamp()); + } + if (timestamps.empty()) { + timestamps.push_back(fallback_ts); + } + + uint64_t min_ts = 0U; + uint64_t max_ts = 0U; + if (!CudaComputeTimestampRange(timestamps.data(), timestamps.size(), + gpu_device_id, &min_ts, &max_ts)) { + return false; + } + local_min = static_cast(min_ts) / kSecondToNano; + local_max = static_cast(max_ts) / kSecondToNano; + } else { +#endif + if (!ResolvePointTimestampBounds(cloud, &local_min, &local_max)) { + return false; + } +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED + } +#endif + + if (ShouldFallbackToMeasurementTime(cloud.measurement_time(), local_min, + local_max)) { + if (used_measurement_time_fallback != nullptr) { + *used_measurement_time_fallback = true; + } + AINFO_EVERY(10) + << "Invalid per-point timestamps detected, fallback to measurement_time" + << " and disable intra-frame deskew. measurement=" + << FormatTimestampSummary(cloud.measurement_time()) + << ", point_range=[" << FormatTimestampSummary(local_min) << ", " + << FormatTimestampSummary(local_max) << "]" + << ", min_delta=" + << std::showpos << std::fixed << std::setprecision(6) + << (local_min - cloud.measurement_time()) << "s" + << ", max_delta=" << (local_max - cloud.measurement_time()) << "s" + << std::noshowpos; + FillUniformMotionSampleTimes(cloud.measurement_time(), bins, sample_times); + return true; + } + + sample_times->assign(bins, local_min); + for (size_t i = 0; i < bins; ++i) { + const double ratio = + bins == 1 ? 0.0 + : static_cast(i) / static_cast(bins - 1); + (*sample_times)[i] = local_min + ratio * (local_max - local_min); + } + return true; +} + +double ResolvePointTimestampSec(const PointXYZIT& point, + double fallback_measurement_time) { + if (point.timestamp() == 0U) { + return fallback_measurement_time; + } + return static_cast(point.timestamp()) / kSecondToNano; +} + +uint64_t ResolvePointTimestampNs(const PointXYZIT& point, + double fallback_measurement_time) { + if (point.timestamp() != 0U) { + return point.timestamp(); + } + return static_cast(fallback_measurement_time * kSecondToNano); +} + +bool InterpolateAffinePose(double point_time, + const std::vector& sample_times, + const std::vector& poses, + Eigen::Affine3d* interpolated_pose) { + if (interpolated_pose == nullptr) { + return false; + } + + const size_t pair_count = std::min(sample_times.size(), poses.size()); + if (pair_count == 0U) { + return false; + } + if (pair_count == 1U) { + *interpolated_pose = poses.front(); + return true; + } + + const auto begin = sample_times.begin(); + const auto end = begin + static_cast(pair_count); + const auto lower = std::lower_bound(begin, end, point_time); + if (lower == begin) { + *interpolated_pose = poses.front(); + return true; + } + if (lower == end) { + *interpolated_pose = poses[pair_count - 1U]; + return true; + } + + const size_t right_idx = static_cast(std::distance(begin, lower)); + const size_t left_idx = right_idx - 1U; + const double left_time = sample_times[left_idx]; + const double right_time = sample_times[right_idx]; + if (std::fabs(right_time - left_time) <= 1e-9) { + *interpolated_pose = poses[right_idx]; + return true; + } + + const double ratio = + std::clamp((point_time - left_time) / (right_time - left_time), 0.0, 1.0); + const Eigen::Vector3d translation = + poses[left_idx].translation() + + ratio * (poses[right_idx].translation() - poses[left_idx].translation()); + Eigen::Quaterniond left_rotation(poses[left_idx].linear()); + Eigen::Quaterniond right_rotation(poses[right_idx].linear()); + if (left_rotation.dot(right_rotation) < 0.0) { + right_rotation.coeffs() *= -1.0; + } + const Eigen::Quaterniond rotation = + left_rotation.slerp(ratio, right_rotation); + *interpolated_pose = Eigen::Translation3d(translation) * rotation; + return true; +} + +bool QueryTransformAffine(apollo::transform::BufferInterface* tf_buffer, + const std::string& target_frame, + const std::string& source_frame, + const cyber::Time& query_time, float timeout_sec, + Eigen::Affine3d* transform) { + if (tf_buffer == nullptr || transform == nullptr) { + return false; + } + + apollo::transform::TransformQuery query(tf_buffer); + std::string err; + if (!query.LookupTransformToAffine(target_frame, source_frame, query_time, + transform, timeout_sec, &err)) { + AWARN << "Transform unavailable from " << source_frame << " to " + << target_frame << ": " << err; + return false; + } + + return true; +} + +namespace { + +struct VoxelKey { + int x = 0; + int y = 0; + int z = 0; +}; + +struct IndexedVoxelPoint { + VoxelKey key; + size_t index = 0; +}; + +} // namespace + +size_t ApplyDeterministicVoxelCentroidFilter(PointXYZIT* points, size_t count, + float voxel_size) { + if (points == nullptr || count == 0U || voxel_size <= 1e-4f) { + return count; + } + + std::vector ordered_points; + ordered_points.reserve(count); + for (size_t i = 0; i < count; ++i) { + ordered_points.push_back(IndexedVoxelPoint{ + VoxelKey{static_cast(std::floor(points[i].x() / voxel_size)), + static_cast(std::floor(points[i].y() / voxel_size)), + static_cast(std::floor(points[i].z() / voxel_size))}, + i}); + } + + std::stable_sort( + ordered_points.begin(), ordered_points.end(), + [](const IndexedVoxelPoint& lhs, const IndexedVoxelPoint& rhs) { + if (lhs.key.x != rhs.key.x) { + return lhs.key.x < rhs.key.x; + } + if (lhs.key.y != rhs.key.y) { + return lhs.key.y < rhs.key.y; + } + if (lhs.key.z != rhs.key.z) { + return lhs.key.z < rhs.key.z; + } + return lhs.index < rhs.index; + }); + + size_t write_idx = 0; + for (size_t begin = 0; begin < ordered_points.size();) { + const VoxelKey key = ordered_points[begin].key; + size_t end = begin; + double sum_x = 0.0; + double sum_y = 0.0; + double sum_z = 0.0; + double sum_intensity = 0.0; + long double sum_timestamp = 0.0; + while (end < ordered_points.size() && ordered_points[end].key.x == key.x && + ordered_points[end].key.y == key.y && + ordered_points[end].key.z == key.z) { + const PointXYZIT& point = points[ordered_points[end].index]; + sum_x += point.x(); + sum_y += point.y(); + sum_z += point.z(); + sum_intensity += point.intensity(); + sum_timestamp += static_cast(point.timestamp()); + ++end; + } + + const double count_inv = 1.0 / static_cast(end - begin); + PointXYZIT centroid_point; + centroid_point.set_x(static_cast(sum_x * count_inv)); + centroid_point.set_y(static_cast(sum_y * count_inv)); + centroid_point.set_z(static_cast(sum_z * count_inv)); + centroid_point.set_intensity(static_cast(sum_intensity * count_inv)); + centroid_point.set_timestamp( + static_cast(std::llround(sum_timestamp * count_inv))); + points[write_idx++] = centroid_point; + begin = end; + } + + return write_idx; +} + +bool TransformPointToBase(const PointXYZIT& point, double measurement_time, + const std::vector& sample_times, + const std::vector& map_from_sensor, + const Eigen::Affine3d& map2base_ref, + PointXYZIT* output_point) { + if (output_point == nullptr || map_from_sensor.empty()) { + return false; + } + + if (!std::isfinite(point.x()) || !std::isfinite(point.y()) || + !std::isfinite(point.z())) { + return false; + } + if (std::fabs(point.x()) > kPointInfThreshold || + std::fabs(point.y()) > kPointInfThreshold || + std::fabs(point.z()) > kPointInfThreshold) { + return false; + } + + const double point_time = ResolvePointTimestampSec(point, measurement_time); + Eigen::Affine3d interpolated_map_from_sensor = Eigen::Affine3d::Identity(); + if (!InterpolateAffinePose(point_time, sample_times, map_from_sensor, + &interpolated_map_from_sensor)) { + return false; + } + + const Eigen::Vector3d raw(point.x(), point.y(), point.z()); + // Output points stay in the reference base_link frame. The fixed/map frame + // is only an anchor for composing sensor(t) -> base(ref) consistently. + const Eigen::Vector3d target = + (map2base_ref * interpolated_map_from_sensor) * raw; + + output_point->set_x(static_cast(target.x())); + output_point->set_y(static_cast(target.y())); + output_point->set_z(static_cast(target.z())); + output_point->set_intensity(point.intensity()); + output_point->set_timestamp(ResolvePointTimestampNs(point, measurement_time)); + return true; +} + +PointXYZIT* GetHostPoints(PointCloudBuffer* buffer) { + if (buffer == nullptr || buffer->data_ptr == nullptr || + buffer->item_size != sizeof(PointXYZIT) || + buffer->device_type != MemoryDeviceType::kHost) { + return nullptr; + } + return reinterpret_cast(buffer->data_ptr); +} + +bool EnsureGpuBackendAvailable(const char* policy_name) { +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED + (void)policy_name; + return true; +#else + AERROR + << policy_name + << " requested GPU execution, but this target is built without " + "CUDA backend support. Rebuild with --define=lidar_gpu_backend=true"; + return false; +#endif +} + +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED +CudaPointXYZIT ToCudaPoint(const PointXYZIT& point, + double measurement_time_sec) { + CudaPointXYZIT out; + out.x = point.x(); + out.y = point.y(); + out.z = point.z(); + out.intensity = point.intensity(); + out.timestamp = ResolvePointTimestampNs(point, measurement_time_sec); + return out; +} + +PointXYZIT ToProtoPoint(const CudaPointXYZIT& point) { + PointXYZIT out; + out.set_x(point.x); + out.set_y(point.y); + out.set_z(point.z); + out.set_intensity(point.intensity); + out.set_timestamp(point.timestamp); + return out; +} + +CudaPose ToCudaPose(const Eigen::Affine3d& in) { + CudaPose out; + out.tx = static_cast(in.translation().x()); + out.ty = static_cast(in.translation().y()); + out.tz = static_cast(in.translation().z()); + const Eigen::Quaterniond rotation(in.linear()); + out.qx = static_cast(rotation.x()); + out.qy = static_cast(rotation.y()); + out.qz = static_cast(rotation.z()); + out.qw = static_cast(rotation.w()); + return out; +} +#endif + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/lidar_policy_common.h b/modules/drivers/lidar/processor/policy/lidar_policy_common.h new file mode 100644 index 00000000..cb4a6cbd --- /dev/null +++ b/modules/drivers/lidar/processor/policy/lidar_policy_common.h @@ -0,0 +1,72 @@ +#pragma once + +#include +#include +#include +#include + +#include "Eigen/Geometry" + +#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h" + +#include "cyber/time/time.h" +#include "modules/drivers/lidar/processor/policy/lidar_policy_interface.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +constexpr double kSecondToNano = 1e9; +constexpr float kPointInfThreshold = 1e3f; + +bool ResolvePointTimestampBounds(const PointCloud& cloud, double* min_sec, + double* max_sec); + +bool BuildMotionSampleTimes(const PointCloud& cloud, size_t bins, + bool use_gpu_timestamp_range, int gpu_device_id, + std::vector* sample_times, + bool* used_measurement_time_fallback = nullptr); + +double ResolvePointTimestampSec(const PointXYZIT& point, + double fallback_measurement_time); + +uint64_t ResolvePointTimestampNs(const PointXYZIT& point, + double fallback_measurement_time); + +bool InterpolateAffinePose(double point_time, + const std::vector& sample_times, + const std::vector& poses, + Eigen::Affine3d* interpolated_pose); + +bool QueryTransformAffine(apollo::transform::BufferInterface* tf_buffer, + const std::string& target_frame, + const std::string& source_frame, + const cyber::Time& query_time, float timeout_sec, + Eigen::Affine3d* transform); + +size_t ApplyDeterministicVoxelCentroidFilter(PointXYZIT* points, size_t count, + float voxel_size); + +bool TransformPointToBase(const PointXYZIT& point, double measurement_time, + const std::vector& sample_times, + const std::vector& map_from_sensor, + const Eigen::Affine3d& map2base_ref, + PointXYZIT* output_point); + +PointXYZIT* GetHostPoints(PointCloudBuffer* buffer); + +bool EnsureGpuBackendAvailable(const char* policy_name); + +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED +struct CudaPointXYZIT; +struct CudaPose; + +CudaPointXYZIT ToCudaPoint(const PointXYZIT& point, + double measurement_time_sec); +PointXYZIT ToProtoPoint(const CudaPointXYZIT& point); +CudaPose ToCudaPose(const Eigen::Affine3d& in); +#endif + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/lidar_policy_cpu_deskew.cc b/modules/drivers/lidar/processor/policy/lidar_policy_cpu_deskew.cc new file mode 100644 index 00000000..8ef3e4d0 --- /dev/null +++ b/modules/drivers/lidar/processor/policy/lidar_policy_cpu_deskew.cc @@ -0,0 +1,62 @@ +#include + +#include "modules/drivers/lidar/processor/policy/cpu_lidar_policy.h" +#include "modules/drivers/lidar/processor/policy/lidar_policy_common.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +bool CpuLidarDeskewPolicy::Init(const LidarUnifiedComponentConfig& config, + apollo::transform::BufferInterface* tf_buffer) { + config_ = config; + tf_buffer_ = tf_buffer; + return tf_buffer_ != nullptr; +} + +bool CpuLidarDeskewPolicy::ComputeMotionCompensationPoses( + const SensorFrameContext& frame_context, std::vector* sample_times, + std::vector* poses) { + if (frame_context.point_cloud == nullptr || frame_context.sensor_id.empty() || + sample_times == nullptr || poses == nullptr || tf_buffer_ == nullptr) { + return false; + } + + const size_t bins = std::max( + 1, static_cast(config_.motion_compensation_bins())); + if (!BuildMotionSampleTimes(*frame_context.point_cloud, bins, false, -1, + sample_times)) { + return false; + } + poses->assign(bins, Eigen::Affine3d::Identity()); + + for (size_t i = 0; i < bins; ++i) { + const double sample_ts = (*sample_times)[i]; + + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + if (!QueryTransformAffine( + tf_buffer_, config_.map_frame_id(), frame_context.sensor_id, + cyber::Time(sample_ts), + static_cast(config_.sensor_pose_query_timeout_sec()), + &pose)) { + if (i == 0) { + if (!QueryTransformAffine( + tf_buffer_, config_.map_frame_id(), frame_context.sensor_id, + cyber::Time(frame_context.point_cloud->measurement_time()), + static_cast(config_.sensor_pose_query_timeout_sec()), + &pose)) { + return false; + } + } else { + pose = (*poses)[i - 1]; + } + } + (*poses)[i] = pose; + } + + return true; +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/lidar_policy_cpu_filter.cc b/modules/drivers/lidar/processor/policy/lidar_policy_cpu_filter.cc new file mode 100644 index 00000000..be5303ea --- /dev/null +++ b/modules/drivers/lidar/processor/policy/lidar_policy_cpu_filter.cc @@ -0,0 +1,81 @@ +#include "modules/drivers/lidar/processor/policy/cpu_lidar_policy.h" +#include "modules/drivers/lidar/processor/policy/lidar_policy_common.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +bool CpuLidarFilterPolicy::Init(const LidarUnifiedComponentConfig& config) { + config_ = config; + return true; +} + +size_t CpuLidarFilterPolicy::ApplyFilters(PointCloudBuffer* io_buffer, + size_t* ego_filtered_count, + size_t* voxel_filtered_count) { + if (ego_filtered_count != nullptr) { + *ego_filtered_count = 0; + } + if (voxel_filtered_count != nullptr) { + *voxel_filtered_count = 0; + } + + if (GetHostPoints(io_buffer) == nullptr || io_buffer->valid_count == 0) { + return 0; + } + + const size_t before_ego = io_buffer->valid_count; + const size_t after_ego = ApplyEgoQueryFilter(io_buffer); + if (ego_filtered_count != nullptr) { + *ego_filtered_count = before_ego - after_ego; + } + + const size_t before_voxel = io_buffer->valid_count; + const size_t after_voxel = ApplyVoxelFilter(io_buffer); + if (voxel_filtered_count != nullptr) { + *voxel_filtered_count = before_voxel - after_voxel; + } + + return io_buffer->valid_count; +} + +size_t CpuLidarFilterPolicy::ApplyVoxelFilter(PointCloudBuffer* io_buffer) { + PointXYZIT* points = GetHostPoints(io_buffer); + if (points == nullptr || io_buffer->valid_count == 0) { + return 0; + } + + io_buffer->valid_count = ApplyDeterministicVoxelCentroidFilter( + points, io_buffer->valid_count, config_.voxel_size()); + return io_buffer->valid_count; +} + +size_t CpuLidarFilterPolicy::ApplyEgoQueryFilter(PointCloudBuffer* io_buffer) { + PointXYZIT* points = GetHostPoints(io_buffer); + if (points == nullptr || io_buffer->valid_count == 0 || + !config_.enable_ego_query_filter()) { + return io_buffer == nullptr ? 0 : io_buffer->valid_count; + } + + size_t write_idx = 0; + for (size_t i = 0; i < io_buffer->valid_count; ++i) { + const auto& point = points[i]; + const bool in_ego_box = point.x() < config_.ego_box_forward_x() && + point.x() > config_.ego_box_backward_x() && + point.y() < config_.ego_box_forward_y() && + point.y() > config_.ego_box_backward_y(); + if (!in_ego_box) { + if (write_idx != i) { + points[write_idx] = point; + } + ++write_idx; + } + } + + io_buffer->valid_count = write_idx; + return write_idx; +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/lidar_policy_cpu_fusion.cc b/modules/drivers/lidar/processor/policy/lidar_policy_cpu_fusion.cc new file mode 100644 index 00000000..a713ee7f --- /dev/null +++ b/modules/drivers/lidar/processor/policy/lidar_policy_cpu_fusion.cc @@ -0,0 +1,73 @@ +#include "cyber/cyber.h" +#include "modules/drivers/lidar/processor/policy/cpu_lidar_policy.h" +#include "modules/drivers/lidar/processor/policy/lidar_policy_common.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +bool CpuLidarFusionPolicy::Init(const LidarUnifiedComponentConfig& config, + apollo::transform::BufferInterface* tf_buffer) { + config_ = config; + tf_buffer_ = tf_buffer; + return tf_buffer_ != nullptr; +} + +bool CpuLidarFusionPolicy::FuseToBaseLink( + double reference_timestamp_sec, const Eigen::Affine3d& map2base_ref, + const std::vector& frames, + const std::vector>& frames_motion_poses, + const std::vector>& frames_motion_times, + PointCloudBuffer* output_buffer) { + PointXYZIT* output_points = GetHostPoints(output_buffer); + if (output_points == nullptr || tf_buffer_ == nullptr || + frames.size() != frames_motion_poses.size() || + frames.size() != frames_motion_times.size()) { + return false; + } + (void)reference_timestamp_sec; + + size_t write_idx = 0; + for (size_t frame_idx = 0; frame_idx < frames.size(); ++frame_idx) { + const auto& frame = frames[frame_idx]; + if (frame.point_cloud == nullptr) { + continue; + } + + const auto& sample_times = frames_motion_times[frame_idx]; + const auto& poses = frames_motion_poses[frame_idx]; + if (sample_times.empty() || poses.empty() || + sample_times.size() != poses.size()) { + if (frame.is_primary) { + return false; + } + AWARN << "Skip auxiliary frame due to invalid motion compensation data: " + << frame.sensor_id; + continue; + } + + for (const auto& point : frame.point_cloud->point()) { + if (write_idx >= output_buffer->capacity) { + AWARN << "Output point buffer is full, truncating fused cloud at " + << write_idx << " points"; + output_buffer->valid_count = write_idx; + return true; + } + + PointXYZIT transformed_point; + if (!TransformPointToBase(point, frame.point_cloud->measurement_time(), + sample_times, poses, map2base_ref, + &transformed_point)) { + continue; + } + output_points[write_idx++] = transformed_point; + } + } + + output_buffer->valid_count = write_idx; + return true; +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/lidar_policy_cuda_kernels.cu b/modules/drivers/lidar/processor/policy/lidar_policy_cuda_kernels.cu new file mode 100644 index 00000000..97a5665e --- /dev/null +++ b/modules/drivers/lidar/processor/policy/lidar_policy_cuda_kernels.cu @@ -0,0 +1,741 @@ +#include "modules/drivers/lidar/processor/policy/lidar_policy_cuda_kernels.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace apollo { +namespace drivers { +namespace lidar { + +namespace { + +constexpr uint64_t kSecondToNano = 1000000000ULL; +constexpr uint64_t kEmptyVoxel = std::numeric_limits::max(); +constexpr uint64_t kPointInfThreshold = 1000ULL; +constexpr float kSlerpLinearFallbackDot = 0.9995f; + +__host__ __device__ inline float3 MakeFloat3(float x, float y, float z) { + float3 out; + out.x = x; + out.y = y; + out.z = z; + return out; +} + +__host__ __device__ inline float3 AddFloat3(const float3& lhs, + const float3& rhs) { + return MakeFloat3(lhs.x + rhs.x, lhs.y + rhs.y, lhs.z + rhs.z); +} + +__host__ __device__ inline float3 ScaleFloat3(const float3& vec, float scale) { + return MakeFloat3(vec.x * scale, vec.y * scale, vec.z * scale); +} + +__host__ __device__ inline float3 CrossFloat3(const float3& lhs, + const float3& rhs) { + return MakeFloat3(lhs.y * rhs.z - lhs.z * rhs.y, + lhs.z * rhs.x - lhs.x * rhs.z, + lhs.x * rhs.y - lhs.y * rhs.x); +} + +__host__ __device__ inline float4 MakeFloat4(float x, float y, float z, + float w) { + float4 out; + out.x = x; + out.y = y; + out.z = z; + out.w = w; + return out; +} + +__host__ __device__ inline float QuaternionDot(const float4& lhs, + const float4& rhs) { + return lhs.x * rhs.x + lhs.y * rhs.y + lhs.z * rhs.z + lhs.w * rhs.w; +} + +__host__ __device__ inline float4 NormalizeQuaternion(const float4& quat) { + const float norm = sqrtf(QuaternionDot(quat, quat)); + if (norm <= 1e-8f) { + return MakeFloat4(0.0f, 0.0f, 0.0f, 1.0f); + } + const float inv_norm = 1.0f / norm; + return MakeFloat4(quat.x * inv_norm, quat.y * inv_norm, quat.z * inv_norm, + quat.w * inv_norm); +} + +__host__ __device__ inline float3 RotatePoint(const float4& quat, + const float3& point) { + const float3 q_vec = MakeFloat3(quat.x, quat.y, quat.z); + const float3 twice_cross = ScaleFloat3(CrossFloat3(q_vec, point), 2.0f); + return AddFloat3(point, + AddFloat3(ScaleFloat3(twice_cross, quat.w), + CrossFloat3(q_vec, twice_cross))); +} + +__host__ __device__ inline float4 SlerpQuaternion(const float4& lhs, + const float4& rhs, + float ratio) { + float4 rhs_adjusted = rhs; + float dot = QuaternionDot(lhs, rhs_adjusted); + if (dot < 0.0f) { + rhs_adjusted.x = -rhs_adjusted.x; + rhs_adjusted.y = -rhs_adjusted.y; + rhs_adjusted.z = -rhs_adjusted.z; + rhs_adjusted.w = -rhs_adjusted.w; + dot = -dot; + } + + if (dot >= kSlerpLinearFallbackDot) { + return NormalizeQuaternion(MakeFloat4( + lhs.x + ratio * (rhs_adjusted.x - lhs.x), + lhs.y + ratio * (rhs_adjusted.y - lhs.y), + lhs.z + ratio * (rhs_adjusted.z - lhs.z), + lhs.w + ratio * (rhs_adjusted.w - lhs.w))); + } + + const float theta_0 = acosf(fminf(fmaxf(dot, -1.0f), 1.0f)); + const float sin_theta_0 = sinf(theta_0); + if (fabsf(sin_theta_0) <= 1e-6f) { + return NormalizeQuaternion(lhs); + } + const float theta = theta_0 * ratio; + const float sin_theta = sinf(theta); + const float s0 = cosf(theta) - dot * sin_theta / sin_theta_0; + const float s1 = sin_theta / sin_theta_0; + return NormalizeQuaternion(MakeFloat4( + s0 * lhs.x + s1 * rhs_adjusted.x, s0 * lhs.y + s1 * rhs_adjusted.y, + s0 * lhs.z + s1 * rhs_adjusted.z, s0 * lhs.w + s1 * rhs_adjusted.w)); +} + +__device__ inline CudaPose InterpolatePose(double point_ts, + const double* sample_times, + const CudaPose* poses, + int sample_count) { + if (sample_count <= 1) { + return poses[0]; + } + if (point_ts <= sample_times[0]) { + return poses[0]; + } + if (point_ts >= sample_times[sample_count - 1]) { + return poses[sample_count - 1]; + } + + int right_idx = 1; + while (right_idx < sample_count && point_ts > sample_times[right_idx]) { + ++right_idx; + } + if (right_idx >= sample_count) { + return poses[sample_count - 1]; + } + + const int left_idx = right_idx - 1; + const double left_time = sample_times[left_idx]; + const double right_time = sample_times[right_idx]; + if (fabs(right_time - left_time) <= 1e-9) { + return poses[right_idx]; + } + + const float ratio = static_cast( + fmin(fmax((point_ts - left_time) / (right_time - left_time), 0.0), 1.0)); + const CudaPose& left = poses[left_idx]; + const CudaPose& right = poses[right_idx]; + const float4 left_quat = MakeFloat4(left.qx, left.qy, left.qz, left.qw); + const float4 right_quat = MakeFloat4(right.qx, right.qy, right.qz, right.qw); + const float4 out_quat = SlerpQuaternion(left_quat, right_quat, ratio); + + CudaPose out; + out.tx = left.tx + ratio * (right.tx - left.tx); + out.ty = left.ty + ratio * (right.ty - left.ty); + out.tz = left.tz + ratio * (right.tz - left.tz); + out.qx = out_quat.x; + out.qy = out_quat.y; + out.qz = out_quat.z; + out.qw = out_quat.w; + return out; +} + +__global__ void TimestampRangeKernel(const uint64_t* timestamps, size_t count, + uint64_t* min_value, uint64_t* max_value) { + const size_t idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= count) { + return; + } + const uint64_t ts = timestamps[idx]; + atomicMin(reinterpret_cast(min_value), + static_cast(ts)); + atomicMax(reinterpret_cast(max_value), + static_cast(ts)); +} + +__global__ void FusionKernel(const CudaPointXYZIT* in_points, size_t point_count, + const double* sample_times, int sample_count, + const CudaPose* poses, + double measurement_time, + CudaPointXYZIT* out_points, + unsigned int* out_count, + unsigned int out_capacity) { + const size_t idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= point_count) { + return; + } + + const CudaPointXYZIT in = in_points[idx]; + if (!isfinite(in.x) || !isfinite(in.y) || !isfinite(in.z) || + fabsf(in.x) > static_cast(kPointInfThreshold) || + fabsf(in.y) > static_cast(kPointInfThreshold) || + fabsf(in.z) > static_cast(kPointInfThreshold)) { + return; + } + + const double point_ts = in.timestamp == 0U + ? measurement_time + : static_cast(in.timestamp) / + static_cast(kSecondToNano); + const CudaPose pose = InterpolatePose(point_ts, sample_times, poses, + sample_count); + const float3 base_p = AddFloat3( + RotatePoint(MakeFloat4(pose.qx, pose.qy, pose.qz, pose.qw), + MakeFloat3(in.x, in.y, in.z)), + MakeFloat3(pose.tx, pose.ty, pose.tz)); + + CudaPointXYZIT out = in; + out.x = base_p.x; + out.y = base_p.y; + out.z = base_p.z; + if (out.timestamp == 0U) { + out.timestamp = + static_cast(measurement_time * static_cast(kSecondToNano)); + } + const unsigned int write_idx = atomicAdd(out_count, 1U); + if (write_idx < out_capacity) { + out_points[write_idx] = out; + } +} + +__global__ void EgoFilterKernel(const CudaPointXYZIT* in_points, size_t point_count, + CudaEgoFilterParams params, + CudaPointXYZIT* out_points, + unsigned int* out_count, + unsigned int capacity) { + const size_t idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= point_count) { + return; + } + + const CudaPointXYZIT point = in_points[idx]; + bool in_ego_box = false; + if (params.enable) { + in_ego_box = point.x < params.forward_x && point.x > params.backward_x && + point.y < params.forward_y && point.y > params.backward_y; + } + if (in_ego_box) { + return; + } + + const unsigned int write_idx = atomicAdd(out_count, 1U); + if (write_idx < capacity) { + out_points[write_idx] = point; + } +} + +__device__ inline uint64_t PackVoxelKey(int vx, int vy, int vz) { + constexpr int kBias = 1 << 20; + const uint64_t x = static_cast(vx + kBias) & 0x1FFFFFULL; + const uint64_t y = static_cast(vy + kBias) & 0x1FFFFFULL; + const uint64_t z = static_cast(vz + kBias) & 0x1FFFFFULL; + return x | (y << 21U) | (z << 42U); +} + +__device__ inline uint64_t Hash64(uint64_t x) { + x ^= x >> 33U; + x *= 0xff51afd7ed558ccdULL; + x ^= x >> 33U; + x *= 0xc4ceb9fe1a85ec53ULL; + x ^= x >> 33U; + return x; +} + +__global__ void InitHashKernel(uint64_t* hash_table, size_t table_size) { + const size_t idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx < table_size) { + hash_table[idx] = kEmptyVoxel; + } +} + +__global__ void VoxelFilterKernel(const CudaPointXYZIT* in_points, + size_t point_count, float voxel_size, + CudaPointXYZIT* out_points, + unsigned int* out_count, + unsigned int out_capacity, + uint64_t* voxel_hash_table, + size_t hash_table_size) { + const size_t idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= point_count || voxel_size <= 1e-4f) { + if (idx < point_count && voxel_size <= 1e-4f) { + const unsigned int write_idx = atomicAdd(out_count, 1U); + if (write_idx < out_capacity) { + out_points[write_idx] = in_points[idx]; + } + } + return; + } + + const CudaPointXYZIT p = in_points[idx]; + const int vx = static_cast(floorf(p.x / voxel_size)); + const int vy = static_cast(floorf(p.y / voxel_size)); + const int vz = static_cast(floorf(p.z / voxel_size)); + const uint64_t key = PackVoxelKey(vx, vy, vz); + + size_t slot = static_cast(Hash64(key) % hash_table_size); + for (size_t probe = 0; probe < hash_table_size; ++probe) { + const uint64_t prev = atomicCAS( + reinterpret_cast(&voxel_hash_table[slot]), + static_cast(kEmptyVoxel), + static_cast(key)); + + if (prev == kEmptyVoxel) { + const unsigned int write_idx = atomicAdd(out_count, 1U); + if (write_idx < out_capacity) { + out_points[write_idx] = p; + } + return; + } + if (prev == key) { + return; + } + slot = (slot + 1U) % hash_table_size; + } +} + +bool CheckCuda(cudaError_t error) { return error == cudaSuccess; } + +template +bool EnsureDeviceBuffer(T** ptr, size_t* capacity, size_t required_count, + uint64_t* expand_count, size_t* peak_capacity) { + if (required_count == 0) { + return true; + } + if (required_count > *peak_capacity) { + *peak_capacity = required_count; + } + if (*capacity >= required_count && *ptr != nullptr) { + return true; + } + + T* new_ptr = nullptr; + if (!CheckCuda(cudaMalloc(&new_ptr, required_count * sizeof(T)))) { + return false; + } + cudaFree(*ptr); + *ptr = new_ptr; + *capacity = required_count; + if (expand_count != nullptr) { + *expand_count += 1U; + } + return true; +} + +struct CudaWorkspace { + ~CudaWorkspace() { + cudaFree(d_points_in); + cudaFree(d_points_out); + cudaFree(d_sample_times); + cudaFree(d_poses); + cudaFree(d_count); + cudaFree(d_hash_table); + cudaFree(d_timestamps); + cudaFree(d_min_timestamp); + cudaFree(d_max_timestamp); + } + + bool EnsurePointBuffers(size_t in_count, size_t out_count) { + return EnsureDeviceBuffer(&d_points_in, &points_in_capacity, in_count, + &points_in_expand_count, + &points_in_peak_capacity) && + EnsureDeviceBuffer(&d_points_out, &points_out_capacity, out_count, + &points_out_expand_count, + &points_out_peak_capacity); + } + + bool EnsureSampleTimes(size_t count) { + return EnsureDeviceBuffer(&d_sample_times, &sample_times_capacity, count, + &sample_times_expand_count, + &sample_times_peak_capacity); + } + + bool EnsurePoses(size_t count) { + return EnsureDeviceBuffer(&d_poses, &poses_capacity, count, + &poses_expand_count, &poses_peak_capacity); + } + + bool EnsureCounter() { + return EnsureDeviceBuffer(&d_count, &count_capacity, 1, + &count_expand_count, &count_peak_capacity); + } + + bool EnsureHashTable(size_t count) { + return EnsureDeviceBuffer(&d_hash_table, &hash_table_capacity, count, + &hash_table_expand_count, + &hash_table_peak_capacity); + } + + bool EnsureTimestampBuffers(size_t count) { + return EnsureDeviceBuffer(&d_timestamps, ×tamps_capacity, count, + ×tamps_expand_count, + ×tamps_peak_capacity) && + EnsureDeviceBuffer(&d_min_timestamp, &min_timestamp_capacity, 1, + &min_timestamp_expand_count, + &min_timestamp_peak_capacity) && + EnsureDeviceBuffer(&d_max_timestamp, &max_timestamp_capacity, 1, + &max_timestamp_expand_count, + &max_timestamp_peak_capacity); + } + + CudaWorkspaceStats SnapshotStats() const { + CudaWorkspaceStats stats; + stats.points_in_expand_count = points_in_expand_count; + stats.points_out_expand_count = points_out_expand_count; + stats.sample_times_expand_count = sample_times_expand_count; + stats.poses_expand_count = poses_expand_count; + stats.hash_table_expand_count = hash_table_expand_count; + stats.points_in_peak_capacity = points_in_peak_capacity; + stats.points_out_peak_capacity = points_out_peak_capacity; + stats.sample_times_peak_capacity = sample_times_peak_capacity; + stats.poses_peak_capacity = poses_peak_capacity; + stats.hash_table_peak_capacity = hash_table_peak_capacity; + return stats; + } + + std::mutex mutex; + + CudaPointXYZIT* d_points_in = nullptr; + CudaPointXYZIT* d_points_out = nullptr; + double* d_sample_times = nullptr; + CudaPose* d_poses = nullptr; + unsigned int* d_count = nullptr; + uint64_t* d_hash_table = nullptr; + uint64_t* d_timestamps = nullptr; + uint64_t* d_min_timestamp = nullptr; + uint64_t* d_max_timestamp = nullptr; + + size_t points_in_capacity = 0; + size_t points_out_capacity = 0; + size_t sample_times_capacity = 0; + size_t poses_capacity = 0; + size_t count_capacity = 0; + size_t hash_table_capacity = 0; + size_t timestamps_capacity = 0; + size_t min_timestamp_capacity = 0; + size_t max_timestamp_capacity = 0; + + uint64_t points_in_expand_count = 0; + uint64_t points_out_expand_count = 0; + uint64_t sample_times_expand_count = 0; + uint64_t poses_expand_count = 0; + uint64_t count_expand_count = 0; + uint64_t hash_table_expand_count = 0; + uint64_t timestamps_expand_count = 0; + uint64_t min_timestamp_expand_count = 0; + uint64_t max_timestamp_expand_count = 0; + + size_t points_in_peak_capacity = 0; + size_t points_out_peak_capacity = 0; + size_t sample_times_peak_capacity = 0; + size_t poses_peak_capacity = 0; + size_t count_peak_capacity = 0; + size_t hash_table_peak_capacity = 0; + size_t timestamps_peak_capacity = 0; + size_t min_timestamp_peak_capacity = 0; + size_t max_timestamp_peak_capacity = 0; +}; + +class WorkspaceManager { + public: + CudaWorkspace* GetOrCreate(int device_id) { + std::lock_guard lock(mutex_); + auto it = workspaces_.find(device_id); + if (it != workspaces_.end()) { + return it->second.get(); + } + auto ws = std::make_unique(); + CudaWorkspace* ptr = ws.get(); + workspaces_.emplace(device_id, std::move(ws)); + return ptr; + } + + private: + std::mutex mutex_; + std::unordered_map> workspaces_; +}; + +CudaWorkspace* GetWorkspace(int device_id) { + static WorkspaceManager manager; + return manager.GetOrCreate(device_id); +} + +} // namespace + +bool CudaComputeTimestampRange(const uint64_t* host_timestamps, size_t count, + int device_id, uint64_t* min_timestamp, + uint64_t* max_timestamp) { + if (host_timestamps == nullptr || min_timestamp == nullptr || + max_timestamp == nullptr || count == 0) { + return false; + } + + if (!CheckCuda(cudaSetDevice(device_id))) { + return false; + } + + CudaWorkspace* ws = GetWorkspace(device_id); + if (ws == nullptr) { + return false; + } + std::lock_guard ws_lock(ws->mutex); + + if (!ws->EnsureTimestampBuffers(count)) { + return false; + } + + const uint64_t init_min = std::numeric_limits::max(); + const uint64_t init_max = 0ULL; + + bool ok = CheckCuda(cudaMemcpy(ws->d_timestamps, host_timestamps, + count * sizeof(uint64_t), + cudaMemcpyHostToDevice)) && + CheckCuda(cudaMemcpy(ws->d_min_timestamp, &init_min, + sizeof(uint64_t), + cudaMemcpyHostToDevice)) && + CheckCuda(cudaMemcpy(ws->d_max_timestamp, &init_max, + sizeof(uint64_t), + cudaMemcpyHostToDevice)); + + if (ok) { + const int threads = 256; + const int blocks = static_cast((count + threads - 1U) / threads); + TimestampRangeKernel<<>>(ws->d_timestamps, count, + ws->d_min_timestamp, + ws->d_max_timestamp); + ok = CheckCuda(cudaGetLastError()) && CheckCuda(cudaDeviceSynchronize()) && + CheckCuda(cudaMemcpy(min_timestamp, ws->d_min_timestamp, + sizeof(uint64_t), + cudaMemcpyDeviceToHost)) && + CheckCuda(cudaMemcpy(max_timestamp, ws->d_max_timestamp, + sizeof(uint64_t), + cudaMemcpyDeviceToHost)); + } + return ok; +} + +size_t CudaFuseFrameToBaseLink(const CudaPointXYZIT* host_input_points, + size_t input_count, + const double* host_sample_times, + size_t sample_count, + const CudaPose* host_base_from_sensor_poses, + double measurement_time, + CudaPointXYZIT* host_output_points, + size_t output_capacity, int device_id) { + if (host_input_points == nullptr || host_sample_times == nullptr || + host_base_from_sensor_poses == nullptr || + host_output_points == nullptr || input_count == 0 || sample_count == 0 || + output_capacity == 0) { + return 0; + } + + if (!CheckCuda(cudaSetDevice(device_id))) { + return 0; + } + + CudaWorkspace* ws = GetWorkspace(device_id); + if (ws == nullptr) { + return 0; + } + std::lock_guard ws_lock(ws->mutex); + + if (!ws->EnsurePointBuffers(input_count, output_capacity) || + !ws->EnsureSampleTimes(sample_count) || !ws->EnsurePoses(sample_count) || + !ws->EnsureCounter()) { + return 0; + } + + unsigned int h_count = 0U; + + bool ok = CheckCuda(cudaMemcpy(ws->d_points_in, host_input_points, + input_count * sizeof(CudaPointXYZIT), + cudaMemcpyHostToDevice)) && + CheckCuda(cudaMemcpy(ws->d_sample_times, host_sample_times, + sample_count * sizeof(double), + cudaMemcpyHostToDevice)) && + CheckCuda(cudaMemcpy(ws->d_poses, host_base_from_sensor_poses, + sample_count * sizeof(CudaPose), + cudaMemcpyHostToDevice)) && + CheckCuda(cudaMemcpy(ws->d_count, &h_count, sizeof(unsigned int), + cudaMemcpyHostToDevice)); + + if (ok) { + const int threads = 256; + const int blocks = static_cast((input_count + threads - 1U) / threads); + FusionKernel<<>>( + ws->d_points_in, input_count, ws->d_sample_times, + static_cast(sample_count), ws->d_poses, + measurement_time, ws->d_points_out, ws->d_count, + static_cast(output_capacity)); + ok = CheckCuda(cudaGetLastError()) && CheckCuda(cudaDeviceSynchronize()) && + CheckCuda(cudaMemcpy(&h_count, ws->d_count, sizeof(unsigned int), + cudaMemcpyDeviceToHost)); + } + + const size_t out_count = + std::min(static_cast(h_count), output_capacity); + if (ok && out_count > 0U) { + ok = CheckCuda(cudaMemcpy(host_output_points, ws->d_points_out, + out_count * sizeof(CudaPointXYZIT), + cudaMemcpyDeviceToHost)); + } + return ok ? out_count : 0U; +} + +size_t CudaApplyEgoFilter(const CudaPointXYZIT* host_input_points, + size_t input_count, + const CudaEgoFilterParams& params, + CudaPointXYZIT* host_output_points, + size_t output_capacity, int device_id) { + if (host_input_points == nullptr || host_output_points == nullptr || + output_capacity == 0 || input_count == 0) { + return 0; + } + + if (!CheckCuda(cudaSetDevice(device_id))) { + return 0; + } + + CudaWorkspace* ws = GetWorkspace(device_id); + if (ws == nullptr) { + return 0; + } + std::lock_guard ws_lock(ws->mutex); + + if (!ws->EnsurePointBuffers(input_count, output_capacity) || + !ws->EnsureCounter()) { + return 0; + } + + unsigned int h_count = 0U; + + bool ok = CheckCuda(cudaMemcpy(ws->d_points_in, host_input_points, + input_count * sizeof(CudaPointXYZIT), + cudaMemcpyHostToDevice)) && + CheckCuda(cudaMemcpy(ws->d_count, &h_count, sizeof(unsigned int), + cudaMemcpyHostToDevice)); + + if (ok) { + const int threads = 256; + const int blocks = static_cast((input_count + threads - 1U) / threads); + EgoFilterKernel<<>>(ws->d_points_in, input_count, params, + ws->d_points_out, ws->d_count, + static_cast(output_capacity)); + ok = CheckCuda(cudaGetLastError()) && CheckCuda(cudaDeviceSynchronize()) && + CheckCuda(cudaMemcpy(&h_count, ws->d_count, sizeof(unsigned int), + cudaMemcpyDeviceToHost)); + } + + const size_t copy_count = + std::min(static_cast(h_count), output_capacity); + if (ok && copy_count > 0) { + ok = CheckCuda(cudaMemcpy(host_output_points, ws->d_points_out, + copy_count * sizeof(CudaPointXYZIT), + cudaMemcpyDeviceToHost)); + } + return ok ? copy_count : 0; +} + +size_t CudaApplyVoxelDownsample(const CudaPointXYZIT* host_input_points, + size_t input_count, float voxel_size, + CudaPointXYZIT* host_output_points, + size_t output_capacity, int device_id) { + if (host_input_points == nullptr || host_output_points == nullptr || + output_capacity == 0 || input_count == 0) { + return 0; + } + + if (!CheckCuda(cudaSetDevice(device_id))) { + return 0; + } + + CudaWorkspace* ws = GetWorkspace(device_id); + if (ws == nullptr) { + return 0; + } + std::lock_guard ws_lock(ws->mutex); + + unsigned int h_count = 0U; + const size_t hash_size = std::max(32, input_count * 2 + 1); + + if (!ws->EnsurePointBuffers(input_count, output_capacity) || + !ws->EnsureCounter() || !ws->EnsureHashTable(hash_size)) { + return 0; + } + + bool ok = CheckCuda(cudaMemcpy(ws->d_points_in, host_input_points, + input_count * sizeof(CudaPointXYZIT), + cudaMemcpyHostToDevice)) && + CheckCuda(cudaMemcpy(ws->d_count, &h_count, sizeof(unsigned int), + cudaMemcpyHostToDevice)); + + if (ok) { + const int threads = 256; + const int init_blocks = static_cast((hash_size + threads - 1U) / threads); + InitHashKernel<<>>(ws->d_hash_table, hash_size); + ok = CheckCuda(cudaGetLastError()) && CheckCuda(cudaDeviceSynchronize()); + } + + if (ok) { + const int threads = 256; + const int blocks = static_cast((input_count + threads - 1U) / threads); + VoxelFilterKernel<<>>(ws->d_points_in, input_count, + voxel_size, ws->d_points_out, + ws->d_count, + static_cast(output_capacity), + ws->d_hash_table, hash_size); + ok = CheckCuda(cudaGetLastError()) && CheckCuda(cudaDeviceSynchronize()) && + CheckCuda(cudaMemcpy(&h_count, ws->d_count, sizeof(unsigned int), + cudaMemcpyDeviceToHost)); + } + + const size_t copy_count = + std::min(static_cast(h_count), output_capacity); + if (ok && copy_count > 0) { + ok = CheckCuda(cudaMemcpy(host_output_points, ws->d_points_out, + copy_count * sizeof(CudaPointXYZIT), + cudaMemcpyDeviceToHost)); + } + return ok ? copy_count : 0; +} + +bool CudaGetWorkspaceStats(int device_id, CudaWorkspaceStats* stats) { + if (stats == nullptr) { + return false; + } + + CudaWorkspace* ws = GetWorkspace(device_id); + if (ws == nullptr) { + return false; + } + + std::lock_guard ws_lock(ws->mutex); + *stats = ws->SnapshotStats(); + return true; +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/lidar_policy_cuda_kernels.h b/modules/drivers/lidar/processor/policy/lidar_policy_cuda_kernels.h new file mode 100644 index 00000000..b1a61a89 --- /dev/null +++ b/modules/drivers/lidar/processor/policy/lidar_policy_cuda_kernels.h @@ -0,0 +1,73 @@ +#pragma once + +#include +#include + +namespace apollo { +namespace drivers { +namespace lidar { + +struct CudaPointXYZIT { + float x; + float y; + float z; + float intensity; + uint64_t timestamp; +}; + +struct CudaPose { + float tx; + float ty; + float tz; + float qx; + float qy; + float qz; + float qw; +}; + +struct CudaEgoFilterParams { + bool enable = true; + float forward_x = 0.0f; + float backward_x = 0.0f; + float forward_y = 0.0f; + float backward_y = 0.0f; +}; + +struct CudaWorkspaceStats { + uint64_t points_in_expand_count = 0; + uint64_t points_out_expand_count = 0; + uint64_t sample_times_expand_count = 0; + uint64_t poses_expand_count = 0; + uint64_t hash_table_expand_count = 0; + size_t points_in_peak_capacity = 0; + size_t points_out_peak_capacity = 0; + size_t sample_times_peak_capacity = 0; + size_t poses_peak_capacity = 0; + size_t hash_table_peak_capacity = 0; +}; + +bool CudaComputeTimestampRange(const uint64_t* host_timestamps, size_t count, + int device_id, uint64_t* min_timestamp, + uint64_t* max_timestamp); + +size_t CudaFuseFrameToBaseLink( + const CudaPointXYZIT* host_input_points, size_t input_count, + const double* host_sample_times, size_t sample_count, + const CudaPose* host_base_from_sensor_poses, double measurement_time, + CudaPointXYZIT* host_output_points, size_t output_capacity, int device_id); + +size_t CudaApplyEgoFilter(const CudaPointXYZIT* host_input_points, + size_t input_count, const CudaEgoFilterParams& params, + CudaPointXYZIT* host_output_points, + size_t output_capacity, int device_id); + +size_t CudaApplyVoxelDownsample(const CudaPointXYZIT* host_input_points, + size_t input_count, float voxel_size, + CudaPointXYZIT* host_output_points, + size_t output_capacity, int device_id); + +bool CudaGetWorkspaceStats(int device_id, CudaWorkspaceStats* stats); + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/lidar_policy_factory.cc b/modules/drivers/lidar/processor/policy/lidar_policy_factory.cc new file mode 100644 index 00000000..8ff41f40 --- /dev/null +++ b/modules/drivers/lidar/processor/policy/lidar_policy_factory.cc @@ -0,0 +1,99 @@ +#include +#include + +#include "cyber/cyber.h" +#include "modules/drivers/lidar/processor/policy/cpu_lidar_policy.h" +#include "modules/drivers/lidar/processor/policy/gpu_lidar_policy.h" +#include "modules/drivers/lidar/processor/policy/lidar_policy_interface.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +std::unique_ptr LidarPolicyFactory::CreateDeskewPolicy( + const std::string& mode) { +#ifdef APOLLO_LIDAR_POLICY_FORCE_CPU + if (mode != "cpu") { + return nullptr; + } +#endif +#ifdef APOLLO_LIDAR_POLICY_FORCE_GPU + if (mode != "gpu") { + return nullptr; + } +#endif + + if (mode == "cpu") { + return std::make_unique(); + } + if (mode == "gpu") { +#ifndef APOLLO_LIDAR_POLICY_GPU_ENABLED + AERROR << "GPU deskew policy requested but CUDA backend is disabled at " + "compile time"; + return nullptr; +#else + return std::make_unique(); +#endif + } + return nullptr; +} + +std::unique_ptr LidarPolicyFactory::CreateFusionPolicy( + const std::string& mode) { +#ifdef APOLLO_LIDAR_POLICY_FORCE_CPU + if (mode != "cpu") { + return nullptr; + } +#endif +#ifdef APOLLO_LIDAR_POLICY_FORCE_GPU + if (mode != "gpu") { + return nullptr; + } +#endif + + if (mode == "cpu") { + return std::make_unique(); + } + if (mode == "gpu") { +#ifndef APOLLO_LIDAR_POLICY_GPU_ENABLED + AERROR << "GPU fusion policy requested but CUDA backend is disabled at " + "compile time"; + return nullptr; +#else + return std::make_unique(); +#endif + } + return nullptr; +} + +std::unique_ptr LidarPolicyFactory::CreateFilterPolicy( + const std::string& mode) { +#ifdef APOLLO_LIDAR_POLICY_FORCE_CPU + if (mode != "cpu") { + return nullptr; + } +#endif +#ifdef APOLLO_LIDAR_POLICY_FORCE_GPU + if (mode != "gpu") { + return nullptr; + } +#endif + + if (mode == "cpu") { + return std::make_unique(); + } + if (mode == "gpu") { +#ifndef APOLLO_LIDAR_POLICY_GPU_ENABLED + AERROR << "GPU filter policy requested but CUDA backend is disabled at " + "compile time"; + return nullptr; +#else + return std::make_unique(); +#endif + } + return nullptr; +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/lidar_policy_gpu_deskew.cc b/modules/drivers/lidar/processor/policy/lidar_policy_gpu_deskew.cc new file mode 100644 index 00000000..facf1f3c --- /dev/null +++ b/modules/drivers/lidar/processor/policy/lidar_policy_gpu_deskew.cc @@ -0,0 +1,79 @@ +#include +#include + +#include "cyber/cyber.h" +#include "modules/drivers/lidar/processor/policy/gpu_lidar_policy.h" +#include "modules/drivers/lidar/processor/policy/lidar_policy_common.h" +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED +#include "modules/drivers/lidar/processor/policy/lidar_policy_cuda_kernels.h" +#endif + +namespace apollo { +namespace drivers { +namespace lidar { + +bool GpuLidarDeskewPolicy::Init(const LidarUnifiedComponentConfig& config, + apollo::transform::BufferInterface* tf_buffer) { + if (!EnsureGpuBackendAvailable("GpuLidarDeskewPolicy")) { + return false; + } + config_ = config; + tf_buffer_ = tf_buffer; + return tf_buffer_ != nullptr; +} + +bool GpuLidarDeskewPolicy::ComputeMotionCompensationPoses( + const SensorFrameContext& frame_context, std::vector* sample_times, + std::vector* poses) { +#ifndef APOLLO_LIDAR_POLICY_GPU_ENABLED + (void)frame_context; + (void)sample_times; + (void)poses; + return false; +#else + if (frame_context.point_cloud == nullptr || frame_context.sensor_id.empty() || + sample_times == nullptr || poses == nullptr || tf_buffer_ == nullptr) { + return false; + } + + const size_t bins = std::max( + 1, static_cast(config_.motion_compensation_bins())); + if (!BuildMotionSampleTimes(*frame_context.point_cloud, bins, true, + static_cast(config_.gpu_device_id()), + sample_times)) { + AERROR << "GpuLidarDeskewPolicy failed to build motion samples"; + return false; + } + poses->assign(bins, Eigen::Affine3d::Identity()); + + for (size_t i = 0; i < bins; ++i) { + const double sample_ts = (*sample_times)[i]; + + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + if (!QueryTransformAffine( + tf_buffer_, config_.map_frame_id(), frame_context.sensor_id, + cyber::Time(sample_ts), + static_cast(config_.sensor_pose_query_timeout_sec()), + &pose)) { + if (i == 0) { + if (!QueryTransformAffine( + tf_buffer_, config_.map_frame_id(), frame_context.sensor_id, + cyber::Time(frame_context.point_cloud->measurement_time()), + static_cast(config_.sensor_pose_query_timeout_sec()), + &pose)) { + return false; + } + } else { + pose = (*poses)[i - 1]; + } + } + (*poses)[i] = pose; + } + + return true; +#endif +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/lidar_policy_gpu_filter.cc b/modules/drivers/lidar/processor/policy/lidar_policy_gpu_filter.cc new file mode 100644 index 00000000..c0b03ae6 --- /dev/null +++ b/modules/drivers/lidar/processor/policy/lidar_policy_gpu_filter.cc @@ -0,0 +1,144 @@ +#include +#include +#include + +#include "cyber/cyber.h" +#include "modules/drivers/lidar/processor/policy/gpu_lidar_policy.h" +#include "modules/drivers/lidar/processor/policy/lidar_policy_common.h" +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED +#include "modules/drivers/lidar/processor/policy/lidar_policy_cuda_kernels.h" +#endif + +namespace apollo { +namespace drivers { +namespace lidar { + +GpuLidarFilterPolicy::GpuLidarFilterPolicy() = default; + +GpuLidarFilterPolicy::~GpuLidarFilterPolicy() = default; + +bool GpuLidarFilterPolicy::Init(const LidarUnifiedComponentConfig& config) { + if (!EnsureGpuBackendAvailable("GpuLidarFilterPolicy")) { + return false; + } + config_ = config; + cuda_stream_ = nullptr; + d_voxel_hash_table_ = nullptr; + + const size_t max_points = std::max( + 1, static_cast(config_.max_full_pointcloud_points())); + host_input_points_.reserve(max_points); + host_ego_filtered_points_.reserve(max_points); + host_centroid_points_.reserve(max_points); + return true; +} + +size_t GpuLidarFilterPolicy::ApplyFilters(PointCloudBuffer* io_buffer, + size_t* ego_filtered_count, + size_t* voxel_filtered_count) { +#ifndef APOLLO_LIDAR_POLICY_GPU_ENABLED + (void)io_buffer; + (void)ego_filtered_count; + (void)voxel_filtered_count; + return 0; +#else + const uint64_t begin_ns = cyber::Time::Now().ToNanosecond(); + + PointXYZIT* host_points = GetHostPoints(io_buffer); + if (ego_filtered_count != nullptr) { + *ego_filtered_count = 0; + } + if (voxel_filtered_count != nullptr) { + *voxel_filtered_count = 0; + } + if (host_points == nullptr || io_buffer->valid_count == 0) { + return 0; + } + + const int device_id = static_cast(config_.gpu_device_id()); + std::lock_guard lock(scratch_mutex_); + + const size_t input_count = io_buffer->valid_count; + if (host_input_points_.size() < input_count) { + host_input_points_.resize(input_count); + } + for (size_t i = 0; i < input_count; ++i) { + host_input_points_[i] = ToCudaPoint( + host_points[i], + static_cast(host_points[i].timestamp()) / kSecondToNano); + } + + if (host_ego_filtered_points_.size() < input_count) { + host_ego_filtered_points_.resize(input_count); + } + CudaEgoFilterParams ego_params; + ego_params.enable = config_.enable_ego_query_filter(); + ego_params.forward_x = config_.ego_box_forward_x(); + ego_params.backward_x = config_.ego_box_backward_x(); + ego_params.forward_y = config_.ego_box_forward_y(); + ego_params.backward_y = config_.ego_box_backward_y(); + + const size_t after_ego = CudaApplyEgoFilter( + host_input_points_.data(), input_count, ego_params, + host_ego_filtered_points_.data(), input_count, device_id); + if (ego_filtered_count != nullptr) { + *ego_filtered_count = input_count - after_ego; + } + + if (host_centroid_points_.size() < after_ego) { + host_centroid_points_.resize(after_ego); + } + for (size_t i = 0; i < after_ego; ++i) { + host_centroid_points_[i] = ToProtoPoint(host_ego_filtered_points_[i]); + } + const size_t after_voxel = ApplyDeterministicVoxelCentroidFilter( + host_centroid_points_.data(), after_ego, config_.voxel_size()); + if (voxel_filtered_count != nullptr) { + *voxel_filtered_count = after_ego - after_voxel; + } + + const size_t write_count = std::min(after_voxel, io_buffer->capacity); + for (size_t i = 0; i < write_count; ++i) { + host_points[i] = host_centroid_points_[i]; + } + io_buffer->valid_count = write_count; + + const uint64_t elapsed_ns = cyber::Time::Now().ToNanosecond() - begin_ns; + const uint64_t calls = + metrics_calls_.fetch_add(1, std::memory_order_relaxed) + 1; + metrics_input_points_.fetch_add(input_count, std::memory_order_relaxed); + metrics_output_points_.fetch_add(write_count, std::memory_order_relaxed); + const uint64_t accumulated_ns = + metrics_elapsed_ns_.fetch_add(elapsed_ns, std::memory_order_relaxed) + + elapsed_ns; + if (calls % 100 == 0) { + const uint64_t total_in = + metrics_input_points_.load(std::memory_order_relaxed); + const uint64_t total_out = + metrics_output_points_.load(std::memory_order_relaxed); + CudaWorkspaceStats ws_stats; + const bool has_ws_stats = CudaGetWorkspaceStats(device_id, &ws_stats); + AINFO << "GpuLidarFilterPolicy metrics: calls=" << calls << ", avg_ms=" + << static_cast(accumulated_ns) / static_cast(calls) / + 1e6 + << ", avg_in_points=" + << static_cast(total_in) / static_cast(calls) + << ", avg_out_points=" + << static_cast(total_out) / static_cast(calls) + << ", ws_expand(in/out/hash)=" + << (has_ws_stats ? ws_stats.points_in_expand_count : 0) << "/" + << (has_ws_stats ? ws_stats.points_out_expand_count : 0) << "/" + << (has_ws_stats ? ws_stats.hash_table_expand_count : 0) + << ", ws_peak(in/out/hash)=" + << (has_ws_stats ? ws_stats.points_in_peak_capacity : 0) << "/" + << (has_ws_stats ? ws_stats.points_out_peak_capacity : 0) << "/" + << (has_ws_stats ? ws_stats.hash_table_peak_capacity : 0); + } + + return write_count; +#endif +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/lidar_policy_gpu_fusion.cc b/modules/drivers/lidar/processor/policy/lidar_policy_gpu_fusion.cc new file mode 100644 index 00000000..c38f1f33 --- /dev/null +++ b/modules/drivers/lidar/processor/policy/lidar_policy_gpu_fusion.cc @@ -0,0 +1,174 @@ +#include +#include + +#include "cyber/cyber.h" +#include "modules/drivers/lidar/processor/policy/gpu_lidar_policy.h" +#include "modules/drivers/lidar/processor/policy/lidar_policy_common.h" +#ifdef APOLLO_LIDAR_POLICY_GPU_ENABLED +#include "modules/drivers/lidar/processor/policy/lidar_policy_cuda_kernels.h" +#endif + +namespace apollo { +namespace drivers { +namespace lidar { + +GpuLidarFusionPolicy::GpuLidarFusionPolicy() = default; + +GpuLidarFusionPolicy::~GpuLidarFusionPolicy() = default; + +bool GpuLidarFusionPolicy::Init(const LidarUnifiedComponentConfig& config, + apollo::transform::BufferInterface* tf_buffer) { + if (!EnsureGpuBackendAvailable("GpuLidarFusionPolicy")) { + return false; + } + config_ = config; + tf_buffer_ = tf_buffer; + cuda_stream_ = nullptr; + const size_t max_points = std::max( + 1, static_cast(config_.max_full_pointcloud_points())); + host_input_points_.reserve(max_points); + host_fused_points_.reserve(max_points); + host_pose_buffer_.reserve(std::max( + 1, static_cast(config_.motion_compensation_bins()))); + return tf_buffer_ != nullptr; +} + +bool GpuLidarFusionPolicy::FuseToBaseLink( + double reference_timestamp_sec, const Eigen::Affine3d& map2base_ref, + const std::vector& frames, + const std::vector>& frames_motion_poses, + const std::vector>& frames_motion_times, + PointCloudBuffer* output_buffer) { +#ifndef APOLLO_LIDAR_POLICY_GPU_ENABLED + (void)reference_timestamp_sec; + (void)frames; + (void)frames_motion_poses; + (void)frames_motion_times; + (void)output_buffer; + return false; +#else + const uint64_t begin_ns = cyber::Time::Now().ToNanosecond(); + + PointXYZIT* output_points = GetHostPoints(output_buffer); + if (output_points == nullptr || tf_buffer_ == nullptr || + frames.size() != frames_motion_poses.size() || + frames.size() != frames_motion_times.size()) { + return false; + } + const Eigen::Affine3d base_from_map_ref = map2base_ref; + + size_t write_idx = 0; + size_t total_input_points = 0; + const int device_id = static_cast(config_.gpu_device_id()); + + std::lock_guard lock(scratch_mutex_); + + for (size_t frame_idx = 0; frame_idx < frames.size(); ++frame_idx) { + const auto& frame = frames[frame_idx]; + if (frame.point_cloud == nullptr) { + continue; + } + + const auto& sample_times = frames_motion_times[frame_idx]; + const auto& poses = frames_motion_poses[frame_idx]; + if (sample_times.empty() || poses.empty() || + sample_times.size() != poses.size()) { + if (frame.is_primary) { + return false; + } + AWARN << "Skip auxiliary frame due to invalid motion compensation data: " + << frame.sensor_id; + continue; + } + + const size_t frame_point_count = + static_cast(frame.point_cloud->point_size()); + total_input_points += frame_point_count; + if (host_input_points_.size() < frame_point_count) { + host_input_points_.resize(frame_point_count); + } + size_t input_count = 0; + for (const auto& point : frame.point_cloud->point()) { + host_input_points_[input_count++] = + ToCudaPoint(point, frame.point_cloud->measurement_time()); + } + + if (host_pose_buffer_.size() < poses.size()) { + host_pose_buffer_.resize(poses.size()); + } + for (size_t pose_idx = 0; pose_idx < poses.size(); ++pose_idx) { + host_pose_buffer_[pose_idx] = + ToCudaPose(base_from_map_ref * poses[pose_idx]); + } + + if (write_idx >= output_buffer->capacity) { + output_buffer->valid_count = write_idx; + const uint64_t elapsed_ns = cyber::Time::Now().ToNanosecond() - begin_ns; + metrics_calls_.fetch_add(1, std::memory_order_relaxed); + metrics_input_points_.fetch_add(total_input_points, + std::memory_order_relaxed); + metrics_output_points_.fetch_add(write_idx, std::memory_order_relaxed); + metrics_elapsed_ns_.fetch_add(elapsed_ns, std::memory_order_relaxed); + return true; + } + const size_t remaining_capacity = output_buffer->capacity - write_idx; + if (host_fused_points_.size() < remaining_capacity) { + host_fused_points_.resize(remaining_capacity); + } + const size_t fused_count = CudaFuseFrameToBaseLink( + host_input_points_.data(), input_count, sample_times.data(), + sample_times.size(), host_pose_buffer_.data(), + frame.point_cloud->measurement_time(), host_fused_points_.data(), + remaining_capacity, device_id); + if (fused_count == 0U && input_count > 0U) { + AERROR << "GpuLidarFusionPolicy CUDA fusion failed for frame " + << frame.sensor_id; + return false; + } + + for (size_t i = 0; i < fused_count; ++i) { + output_points[write_idx++] = ToProtoPoint(host_fused_points_[i]); + } + } + + output_buffer->valid_count = write_idx; + + const uint64_t elapsed_ns = cyber::Time::Now().ToNanosecond() - begin_ns; + const uint64_t calls = + metrics_calls_.fetch_add(1, std::memory_order_relaxed) + 1; + metrics_input_points_.fetch_add(total_input_points, + std::memory_order_relaxed); + metrics_output_points_.fetch_add(write_idx, std::memory_order_relaxed); + const uint64_t accumulated_ns = + metrics_elapsed_ns_.fetch_add(elapsed_ns, std::memory_order_relaxed) + + elapsed_ns; + if (calls % 100 == 0) { + const uint64_t total_in = + metrics_input_points_.load(std::memory_order_relaxed); + const uint64_t total_out = + metrics_output_points_.load(std::memory_order_relaxed); + CudaWorkspaceStats ws_stats; + const bool has_ws_stats = CudaGetWorkspaceStats(device_id, &ws_stats); + AINFO << "GpuLidarFusionPolicy metrics: calls=" << calls << ", avg_ms=" + << static_cast(accumulated_ns) / static_cast(calls) / + 1e6 + << ", avg_in_points=" + << static_cast(total_in) / static_cast(calls) + << ", avg_out_points=" + << static_cast(total_out) / static_cast(calls) + << ", ws_expand(in/out/poses)=" + << (has_ws_stats ? ws_stats.points_in_expand_count : 0) << "/" + << (has_ws_stats ? ws_stats.points_out_expand_count : 0) << "/" + << (has_ws_stats ? ws_stats.poses_expand_count : 0) + << ", ws_peak(in/out/poses)=" + << (has_ws_stats ? ws_stats.points_in_peak_capacity : 0) << "/" + << (has_ws_stats ? ws_stats.points_out_peak_capacity : 0) << "/" + << (has_ws_stats ? ws_stats.poses_peak_capacity : 0); + } + return true; +#endif +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/policy/lidar_policy_interface.h b/modules/drivers/lidar/processor/policy/lidar_policy_interface.h new file mode 100644 index 00000000..cbe82047 --- /dev/null +++ b/modules/drivers/lidar/processor/policy/lidar_policy_interface.h @@ -0,0 +1,109 @@ +#pragma once + +#include +#include +#include + +#include "Eigen/Geometry" + +#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h" +#include "modules/drivers/lidar/proto/lidar_unified_component_config.pb.h" + +#include "modules/transform/buffer_interface.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +/// @brief Point cloud storage type (CPU Host or GPU Device) +enum class MemoryDeviceType { kHost = 0, kDevice = 1 }; + +/// @brief Point cloud abstraction supporting Zero-Copy & CUDA memory. +struct PointCloudBuffer { + void* data_ptr = nullptr; // Raw memory ptr to array of points + size_t capacity = 0; // Total allocated capacity (point count) + size_t valid_count = 0; // Number of effectively populated points + size_t item_size = 0; // sizeof(PointT) e.g., sizeof(PointXYZIT) + MemoryDeviceType device_type = MemoryDeviceType::kHost; + int device_id = -1; // -1: Host, 0-N: CUDA device index +}; + +/// @brief Context carrying all necessary data for a single sensor frame. +struct SensorFrameContext { + std::string sensor_id; + std::shared_ptr point_cloud; + bool is_primary = false; + double min_timestamp_sec = 0.0; + double max_timestamp_sec = 0.0; +}; + +// ============================================================================ +// 1. Motion Compensation & Deskew Policy Interface (Generates TFs) +// ============================================================================ +class LidarDeskewPolicy { + public: + virtual ~LidarDeskewPolicy() = default; + + virtual bool Init(const LidarUnifiedComponentConfig& config, + apollo::transform::BufferInterface* tf_buffer) = 0; + + virtual bool ComputeMotionCompensationPoses( + const SensorFrameContext& frame_context, + std::vector* sample_times, + std::vector* poses) = 0; +}; + +// ============================================================================ +// 2. Spatial Fusion Policy Interface (Transforms & Merges Points) +// ============================================================================ +class LidarFusionPolicy { + public: + virtual ~LidarFusionPolicy() = default; + + virtual bool Init(const LidarUnifiedComponentConfig& config, + apollo::transform::BufferInterface* tf_buffer) = 0; + + /// @brief Fuses multiple deskewed point clouds into a single base_link target + /// cloud taking into account the primary sensor's reference time. + /// @note Can leverage Host or Device buffers. + virtual bool FuseToBaseLink( + double reference_timestamp_sec, const Eigen::Affine3d& map2base_ref, + const std::vector& frames, + const std::vector>& frames_motion_poses, + const std::vector>& frames_motion_times, + PointCloudBuffer* output_buffer) = 0; +}; + +// ============================================================================ +// 3. Post-Processing Filter Policy Interface (Ego Car, Voxel) +// ============================================================================ +class LidarFilterPolicy { + public: + virtual ~LidarFilterPolicy() = default; + + virtual bool Init(const LidarUnifiedComponentConfig& config) = 0; + + /// @brief Apply filtering passes (Ego box, Voxel) directly on the buffer + /// memory. + /// @return number of remaining valid points. + virtual size_t ApplyFilters(PointCloudBuffer* io_buffer, + size_t* ego_filtered_count, + size_t* voxel_filtered_count) = 0; +}; + +// ============================================================================ +// Policy Factory Interface +// ============================================================================ +class LidarPolicyFactory { + public: + static std::unique_ptr CreateDeskewPolicy( + const std::string& mode); + static std::unique_ptr CreateFusionPolicy( + const std::string& mode); + static std::unique_ptr CreateFilterPolicy( + const std::string& mode); +}; + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/safety/degrade_policy.cc b/modules/drivers/lidar/processor/safety/degrade_policy.cc new file mode 100644 index 00000000..0ad0c478 --- /dev/null +++ b/modules/drivers/lidar/processor/safety/degrade_policy.cc @@ -0,0 +1,63 @@ +#include "modules/drivers/lidar/processor/safety/degrade_policy.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +void DegradePolicy::SetConfig(bool degrade_on_ts_anomaly, + uint32_t ts_max_consecutive_errors) { + degrade_on_ts_anomaly_ = degrade_on_ts_anomaly; + ts_max_consecutive_errors_ = ts_max_consecutive_errors; +} + +DegradeEvent DegradePolicy::OnTsAnomaly(int consecutive_errors) { + const DegradeMode prev = mode_; + ok_frames_since_last_error_ = 0; + + if (!degrade_on_ts_anomaly_) { + return MakeEvent(prev, mode_, DegradeReason::kNone); + } + + if (consecutive_errors >= static_cast(ts_max_consecutive_errors_)) { + mode_ = DegradeMode::kSingleLidar; + } + + return MakeEvent(prev, mode_, DegradeReason::kTsAnomaly); +} + +DegradeEvent DegradePolicy::OnFrameOk() { + const DegradeMode prev = mode_; + + if (mode_ == DegradeMode::kNormal) { + ok_frames_since_last_error_ = 0; + return MakeEvent(prev, mode_, DegradeReason::kNone); + } + + ++ok_frames_since_last_error_; + if (ok_frames_since_last_error_ >= kRecoveryOkFrames) { + ok_frames_since_last_error_ = 0; + if (mode_ == DegradeMode::kSingleLidar) { + mode_ = DegradeMode::kNormal; + } + } + + return MakeEvent(prev, mode_, DegradeReason::kNone); +} + +DegradeMode DegradePolicy::CurrentMode() const { return mode_; } + +DegradeEvent DegradePolicy::Reset() { + const DegradeMode prev = mode_; + mode_ = DegradeMode::kNormal; + ok_frames_since_last_error_ = 0; + return MakeEvent(prev, mode_, DegradeReason::kManualReset); +} + +DegradeEvent DegradePolicy::MakeEvent(DegradeMode prev, DegradeMode next, + DegradeReason r) { + return DegradeEvent{prev, next, r, prev != next}; +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/safety/degrade_policy.h b/modules/drivers/lidar/processor/safety/degrade_policy.h new file mode 100644 index 00000000..34cfbd92 --- /dev/null +++ b/modules/drivers/lidar/processor/safety/degrade_policy.h @@ -0,0 +1,63 @@ +#pragma once + +#include + +namespace apollo { +namespace drivers { +namespace lidar { + +// Processing degrade levels (ordered by severity). +enum class DegradeMode { + kNormal = 0, // All sensors online, full fusion pipeline. + kSingleLidar = 1, // Auxiliary sensors dropped; primary-only fusion. +}; + +// Reason codes for degrade transitions. +enum class DegradeReason { + kNone = 0, + kTsAnomaly = 1, // Timestamp sanity error threshold exceeded. + kManualReset = 2, // Explicit reset via Reset(). +}; + +struct DegradeEvent { + DegradeMode previous_mode; + DegradeMode new_mode; + DegradeReason reason; + bool mode_changed; +}; + +// State-machine that escalates / recovers degrade level based on error signals. +// Not thread-safe by itself; callers must serialise from the component thread. +class DegradePolicy { + public: + void SetConfig(bool degrade_on_ts_anomaly, + uint32_t ts_max_consecutive_errors); + + // Called when TsSanity reports a non-OK result. + DegradeEvent OnTsAnomaly(int consecutive_errors); + + // Called after a fully successful processing cycle. + // Allows recovery: repeated successes lower the degrade level. + DegradeEvent OnFrameOk(); + + DegradeMode CurrentMode() const; + + // Hard-reset to kNormal and clear recovery counters. + DegradeEvent Reset(); + + private: + DegradeMode mode_ = DegradeMode::kNormal; + + bool degrade_on_ts_anomaly_ = true; + uint32_t ts_max_consecutive_errors_ = 3; + + // Consecutive successful frames needed to step mode down one level. + static constexpr uint32_t kRecoveryOkFrames = 10; + uint32_t ok_frames_since_last_error_ = 0; + + DegradeEvent MakeEvent(DegradeMode prev, DegradeMode next, DegradeReason r); +}; + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/safety/dtc_reporter.cc b/modules/drivers/lidar/processor/safety/dtc_reporter.cc new file mode 100644 index 00000000..12ed2f81 --- /dev/null +++ b/modules/drivers/lidar/processor/safety/dtc_reporter.cc @@ -0,0 +1,85 @@ +#include "modules/drivers/lidar/processor/safety/dtc_reporter.h" + +#include "cyber/cyber.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +namespace { + +const char* TsStatusName(TsSanityStatus status) { + switch (status) { + case TsSanityStatus::kOk: + return "ok"; + case TsSanityStatus::kFirstFrame: + return "first_frame"; + case TsSanityStatus::kIntervalTooShort: + return "interval_too_short"; + case TsSanityStatus::kIntervalTooLong: + return "interval_too_long"; + case TsSanityStatus::kJump: + return "jump"; + default: + return "unknown"; + } +} + +const char* DegradeModeName(DegradeMode mode) { + switch (mode) { + case DegradeMode::kNormal: + return "normal"; + case DegradeMode::kSingleLidar: + return "single_lidar"; + default: + return "unknown"; + } +} + +const char* DegradeReasonName(DegradeReason reason) { + switch (reason) { + case DegradeReason::kNone: + return "none"; + case DegradeReason::kTsAnomaly: + return "ts_anomaly"; + case DegradeReason::kManualReset: + return "manual_reset"; + default: + return "unknown"; + } +} + +} // namespace + +void DtcReporter::ReportTsAnomaly(TsSanityStatus status, double interval_ms, + int consecutive_errors, + const std::string& sensor_id) { + ts_anomaly_count_.fetch_add(1); + AWARN << "[DTC][LIDAR_TS_SANITY] sensor=" << sensor_id + << ", status=" << TsStatusName(status) + << ", interval_ms=" << interval_ms + << ", consecutive_errors=" << consecutive_errors; +} + +void DtcReporter::ReportDegradeTransition(const DegradeEvent& event) { + if (!event.mode_changed) { + return; + } + + degrade_transition_count_.fetch_add(1); + AERROR << "[DTC][LIDAR_DEGRADE] mode " << DegradeModeName(event.previous_mode) + << " -> " << DegradeModeName(event.new_mode) + << ", reason=" << DegradeReasonName(event.reason); +} + +uint64_t DtcReporter::ts_anomaly_count() const { + return ts_anomaly_count_.load(); +} + +uint64_t DtcReporter::degrade_transition_count() const { + return degrade_transition_count_.load(); +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/safety/dtc_reporter.h b/modules/drivers/lidar/processor/safety/dtc_reporter.h new file mode 100644 index 00000000..fde842f3 --- /dev/null +++ b/modules/drivers/lidar/processor/safety/dtc_reporter.h @@ -0,0 +1,33 @@ +#pragma once + +#include +#include +#include + +#include "modules/drivers/lidar/processor/safety/degrade_policy.h" +#include "modules/drivers/lidar/processor/safety/ts_sanity.h" + +namespace apollo { +namespace drivers { +namespace lidar { + +// Lightweight in-process DTC reporter (log + counters). +// In a later stage this can be bridged to a centralized diagnostics bus. +class DtcReporter { + public: + void ReportTsAnomaly(TsSanityStatus status, double interval_ms, + int consecutive_errors, const std::string& sensor_id); + + void ReportDegradeTransition(const DegradeEvent& event); + + uint64_t ts_anomaly_count() const; + uint64_t degrade_transition_count() const; + + private: + std::atomic ts_anomaly_count_{0}; + std::atomic degrade_transition_count_{0}; +}; + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/safety/ts_sanity.cc b/modules/drivers/lidar/processor/safety/ts_sanity.cc new file mode 100644 index 00000000..19c742bd --- /dev/null +++ b/modules/drivers/lidar/processor/safety/ts_sanity.cc @@ -0,0 +1,60 @@ +#include "modules/drivers/lidar/processor/safety/ts_sanity.h" + +#include + +namespace apollo { +namespace drivers { +namespace lidar { + +void TsSanity::SetConfig(uint32_t min_interval_ms, uint32_t max_interval_ms, + uint32_t max_jump_ms) { + min_interval_ms_ = min_interval_ms; + max_interval_ms_ = max_interval_ms; + max_jump_ms_ = max_jump_ms; +} + +TsSanityResult TsSanity::Check(double timestamp_seconds) { + TsSanityResult result; + + if (last_timestamp_s_ < 0.0) { + last_timestamp_s_ = timestamp_seconds; + result.status = TsSanityStatus::kFirstFrame; + result.interval_ms = 0.0; + result.consecutive_errors = 0; + return result; + } + + const double delta_s = timestamp_seconds - last_timestamp_s_; + const double abs_delta_ms = std::fabs(delta_s) * 1000.0; + result.interval_ms = abs_delta_ms; + + TsSanityStatus status = TsSanityStatus::kOk; + + if (abs_delta_ms > static_cast(max_jump_ms_)) { + status = TsSanityStatus::kJump; + } else if (abs_delta_ms > static_cast(max_interval_ms_)) { + status = TsSanityStatus::kIntervalTooLong; + } else if (abs_delta_ms < static_cast(min_interval_ms_)) { + status = TsSanityStatus::kIntervalTooShort; + } + + if (status == TsSanityStatus::kOk) { + consecutive_errors_ = 0; + } else { + ++consecutive_errors_; + } + + last_timestamp_s_ = timestamp_seconds; + result.status = status; + result.consecutive_errors = consecutive_errors_; + return result; +} + +void TsSanity::Reset() { + last_timestamp_s_ = -1.0; + consecutive_errors_ = 0; +} + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/processor/safety/ts_sanity.h b/modules/drivers/lidar/processor/safety/ts_sanity.h new file mode 100644 index 00000000..cb71f4a2 --- /dev/null +++ b/modules/drivers/lidar/processor/safety/ts_sanity.h @@ -0,0 +1,48 @@ +#pragma once + +#include + +namespace apollo { +namespace drivers { +namespace lidar { + +enum class TsSanityStatus { + kOk = 0, + kFirstFrame = 1, // no previous timestamp to compare + kIntervalTooShort = 2, // frames arriving faster than min_interval_ms + kIntervalTooLong = 3, // gap exceeds max_interval_ms (late / dropped) + kJump = 4, // |delta| > max_jump_ms (clock reset / backward jump) +}; + +struct TsSanityResult { + TsSanityStatus status = TsSanityStatus::kFirstFrame; + double interval_ms = 0.0; // |current - last| in milliseconds + int consecutive_errors = 0; // rolling count, reset on kOk +}; + +// Stateful per-sensor timestamp sanity checker. +// Thread-safe: external locking expected (one LidarUnifiedComponent per Proc). +class TsSanity { + public: + // Configure thresholds (milliseconds). + void SetConfig(uint32_t min_interval_ms, uint32_t max_interval_ms, + uint32_t max_jump_ms); + + // Check a new frame timestamp (seconds). Updates internal state. + TsSanityResult Check(double timestamp_seconds); + + // Reset to initial state (e.g. after degrade recovery). + void Reset(); + + private: + double last_timestamp_s_ = -1.0; + int consecutive_errors_ = 0; + + uint32_t min_interval_ms_ = 40; + uint32_t max_interval_ms_ = 200; + uint32_t max_jump_ms_ = 500; +}; + +} // namespace lidar +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar/proto/BUILD b/modules/drivers/lidar/proto/BUILD index 67f990a3..c512f782 100644 --- a/modules/drivers/lidar/proto/BUILD +++ b/modules/drivers/lidar/proto/BUILD @@ -83,3 +83,18 @@ py_proto_library( name = "lidar_parameter_py_pb2", deps = [":lidar_parameter_proto"], ) + +cc_proto_library( + name = "lidar_unified_component_config_cc_proto", + deps = [":lidar_unified_component_config_proto"], +) + +proto_library( + name = "lidar_unified_component_config_proto", + srcs = ["lidar_unified_component_config.proto"], +) + +py_proto_library( + name = "lidar_unified_component_config_py_pb2", + deps = [":lidar_unified_component_config_proto"], +) diff --git a/modules/drivers/lidar/proto/lidar_unified_component_config.proto b/modules/drivers/lidar/proto/lidar_unified_component_config.proto new file mode 100644 index 00000000..d3f1c405 --- /dev/null +++ b/modules/drivers/lidar/proto/lidar_unified_component_config.proto @@ -0,0 +1,84 @@ +syntax = "proto2"; + +package apollo.drivers.lidar; + +message LidarUnifiedComponentConfig { + enum ComputeMode { + COMPUTE_MODE_CPU = 0; + COMPUTE_MODE_GPU = 1; + } + + message AuxiliaryLidarInput { + optional string topic_name = 1; + } + + // Core channel / frame IDs + optional string output_channel = 1; + optional string base_link_frame_id = 2; + optional string map_frame_id = 3; + + // Buffer / capacity + optional uint32 sensor_buffer_size = 4 [default = 8]; + optional uint32 max_full_pointcloud_points = 5 [default = 300000]; + optional uint32 worker_num = 6 [default = 0]; + + // Ego-box filter + optional bool enable_ego_query_filter = 7 [default = false]; + optional double ego_box_forward_x = 8 [default = 2.8]; + optional double ego_box_backward_x = 9 [default = -2.8]; + optional double ego_box_forward_y = 10 [default = 1.4]; + optional double ego_box_backward_y = 11 [default = -1.4]; + + // Voxel downsampling + optional double voxel_size = 12 [default = 0.1]; + + // Synchronisation + optional uint32 max_ref_time_delta_ms = 13 [default = 100]; + optional uint32 motion_compensation_bins = 14 [default = 12]; + optional bool strict_auxiliary_sync = 16 [default = false]; + + // Observability + optional uint32 metrics_log_interval = 15 [default = 100]; + + // Compute backend + optional ComputeMode compute_mode = 17 [default = COMPUTE_MODE_CPU]; + optional uint32 gpu_device_id = 18 [default = 0]; + + // Auxiliary sensors + repeated AuxiliaryLidarInput auxiliary_lidar_inputs = 19; + + // M4 – Timestamp sanity + optional bool ts_sanity_enabled = 20 [default = true]; + // Minimum expected frame interval (ms). Frames arriving faster are anomalous. + optional uint32 ts_sanity_min_interval_ms = 21 [default = 40]; + // Maximum expected frame interval (ms). A longer gap triggers a late-frame anomaly. + optional uint32 ts_sanity_max_interval_ms = 22 [default = 200]; + // Timestamp jump threshold (ms). A forward/backward jump larger than this is flagged. + optional uint32 ts_sanity_max_jump_ms = 23 [default = 500]; + // Number of consecutive errors that triggers degrade activation. + optional uint32 ts_sanity_max_consecutive_errors = 24 [default = 3]; + + // M4 - Degrade policy trigger signals + optional bool degrade_on_ts_anomaly = 25 [default = true]; + + // TF pose prefetch cache + optional double sensor_pose_cache_duration_sec = 26 [default = 1.0]; + optional double sensor_pose_cache_max_extrapolation_sec = 27 [default = 0.15]; + optional double sensor_pose_query_timeout_sec = 28 [default = 0.02]; + + // Multi-lidar fixed-delay and clock-offset observability + optional double fixed_delay_ema_alpha = 29 [default = 0.2]; + optional double fixed_delay_update_limit_ms = 30 [default = 80.0]; + optional double clock_offset_ema_alpha = 31 [default = 0.2]; + + // Overlap region quality weighting + optional double overlap_region_forward_x = 32 [default = 25.0]; + optional double overlap_region_backward_x = 33 [default = -5.0]; + optional double overlap_region_left_y = 34 [default = 12.0]; + optional double overlap_region_right_y = 35 [default = -12.0]; + optional double overlap_region_min_z = 36 [default = -3.0]; + optional double overlap_region_max_z = 37 [default = 3.0]; + optional double overlap_quality_ema_alpha = 38 [default = 0.2]; + optional double auxiliary_min_overlap_quality_weight = 39 [default = 0.0]; + optional uint32 overlap_quality_sample_stride = 40 [default = 16]; +} diff --git a/modules/drivers/lidar/proto/velodyne_config.proto b/modules/drivers/lidar/proto/velodyne_config.proto index 0e36194a..c4347978 100644 --- a/modules/drivers/lidar/proto/velodyne_config.proto +++ b/modules/drivers/lidar/proto/velodyne_config.proto @@ -44,7 +44,7 @@ message FusionConfig { message CompensatorConfig { optional string output_channel = 1; optional float transform_query_timeout = 2 [default = 0.02]; - optional string world_frame_id = 3 [default = "map"]; + optional string map_frame_id = 3 [default = "map"]; optional string target_frame_id = 4; optional uint32 point_cloud_size = 5; } diff --git a/modules/drivers/lidar/velodyne/README.md b/modules/drivers/lidar/velodyne/README.md index 7f23526e..94c4a92f 100644 --- a/modules/drivers/lidar/velodyne/README.md +++ b/modules/drivers/lidar/velodyne/README.md @@ -22,7 +22,7 @@ Compensation relies on `tf` to query the coordination transform, so gnss_driver proto: [modules/drivers/proto/pointcloud.proto]https://github.com/ApolloAuto/apollo/blob/master/modules/drivers/proto/pointcloud.proto ### Coordination -* world +* map * novatel * velodyne128 diff --git a/modules/drivers/lidar/velodyne/README_cn.md b/modules/drivers/lidar/velodyne/README_cn.md index e88b852a..e66f9925 100644 --- a/modules/drivers/lidar/velodyne/README_cn.md +++ b/modules/drivers/lidar/velodyne/README_cn.md @@ -24,7 +24,7 @@ velodyne驱动是以component的形式实现的，包含了: proto: [modules/drivers/proto/pointcloud.proto]https://github.com/ApolloAuto/apollo/blob/master/modules/drivers/proto/pointcloud.proto ### 坐标系 -* world +* map * novatel * velodyne128 diff --git a/modules/drivers/lidar/velodyne/compensator/compensator.cc b/modules/drivers/lidar/velodyne/compensator/compensator.cc index bbba21d8..d2fbd130 100644 --- a/modules/drivers/lidar/velodyne/compensator/compensator.cc +++ b/modules/drivers/lidar/velodyne/compensator/compensator.cc @@ -32,7 +32,7 @@ bool Compensator::QueryPoseAffineFromTF2(const uint64_t& timestamp, void* pose, Eigen::Affine3d* tmp_pose = static_cast(pose); std::string err_string; if (!transform_query_.LookupTransformToAffine( - config_.world_frame_id(), child_frame_id, query_time, tmp_pose, + config_.map_frame_id(), child_frame_id, query_time, tmp_pose, config_.transform_query_timeout(), &err_string)) { AERROR << "Can not find transform. " << timestamp << " frame_id:" << child_frame_id << " Error info: " << err_string; diff --git a/modules/drivers/lidar/velodyne/conf/velodyne128_compensator.pb.txt b/modules/drivers/lidar/velodyne/conf/velodyne128_compensator.pb.txt index 7c0954e9..b9289302 100644 --- a/modules/drivers/lidar/velodyne/conf/velodyne128_compensator.pb.txt +++ b/modules/drivers/lidar/velodyne/conf/velodyne128_compensator.pb.txt @@ -1,3 +1,3 @@ -world_frame_id: "map" +map_frame_id: "map" transform_query_timeout: 0.02 output_channel: "/apollo/sensor/lidar128/compensator/PointCloud2" diff --git a/modules/drivers/lidar/velodyne/conf/velodyne128_fusion_compensator.pb.txt b/modules/drivers/lidar/velodyne/conf/velodyne128_fusion_compensator.pb.txt index 7c0954e9..b9289302 100644 --- a/modules/drivers/lidar/velodyne/conf/velodyne128_fusion_compensator.pb.txt +++ b/modules/drivers/lidar/velodyne/conf/velodyne128_fusion_compensator.pb.txt @@ -1,3 +1,3 @@ -world_frame_id: "map" +map_frame_id: "map" transform_query_timeout: 0.02 output_channel: "/apollo/sensor/lidar128/compensator/PointCloud2" diff --git a/modules/drivers/lidar/velodyne/conf/velodyne16_front_up_compensator.pb.txt b/modules/drivers/lidar/velodyne/conf/velodyne16_front_up_compensator.pb.txt index bd2cfb73..b8548d12 100644 --- a/modules/drivers/lidar/velodyne/conf/velodyne16_front_up_compensator.pb.txt +++ b/modules/drivers/lidar/velodyne/conf/velodyne16_front_up_compensator.pb.txt @@ -1,3 +1,3 @@ -world_frame_id: "map" +map_frame_id: "map" transform_query_timeout: 0.02 output_channel: "/apollo/sensor/lidar16/front/up/compensator/PointCloud2" diff --git a/modules/drivers/lidar/velodyne/conf/velodyne16_fusion_compensator.pb.txt b/modules/drivers/lidar/velodyne/conf/velodyne16_fusion_compensator.pb.txt index 81095db7..388145a7 100644 --- a/modules/drivers/lidar/velodyne/conf/velodyne16_fusion_compensator.pb.txt +++ b/modules/drivers/lidar/velodyne/conf/velodyne16_fusion_compensator.pb.txt @@ -1,3 +1,3 @@ -world_frame_id: "map" +map_frame_id: "map" transform_query_timeout: 0.02 output_channel: "/apollo/sensor/lidar16/fusion/compensator/PointCloud2" diff --git a/modules/drivers/lidar/velodyne/conf/velodyne64_compensator.pb.txt b/modules/drivers/lidar/velodyne/conf/velodyne64_compensator.pb.txt index 320d2d86..553bcb82 100644 --- a/modules/drivers/lidar/velodyne/conf/velodyne64_compensator.pb.txt +++ b/modules/drivers/lidar/velodyne/conf/velodyne64_compensator.pb.txt @@ -1,4 +1,4 @@ -world_frame_id: "map" +map_frame_id: "map" target_frame_id: "base_link" transform_query_timeout: 0.02 output_channel: "/apollo/sensor/velodyne64/compensator/PointCloud2" diff --git a/modules/perception/camera/tools/offline/visualizer.cc b/modules/perception/camera/tools/offline/visualizer.cc index e8ee7a69..9e80d2ae 100644 --- a/modules/perception/camera/tools/offline/visualizer.cc +++ b/modules/perception/camera/tools/offline/visualizer.cc @@ -16,9 +16,11 @@ #include "modules/perception/camera/tools/offline/visualizer.h" #include +#include #include #include #include +#include #include "absl/strings/str_cat.h" #include "cyber/common/file.h" @@ -32,6 +34,83 @@ namespace apollo { namespace perception { namespace camera { +namespace { + +constexpr char kLidarFrameId[] = "velodyne128"; +constexpr char kVehicleFrameId[] = "base_link"; + +struct PoseDebugInfo { + bool has_world_pose = false; + bool has_lidar_to_vehicle = false; + Eigen::Affine3d world_pose = Eigen::Affine3d::Identity(); + Eigen::Affine3d lidar_to_vehicle = Eigen::Affine3d::Identity(); + Eigen::Affine3d lidar_to_world = Eigen::Affine3d::Identity(); +}; + +PoseDebugInfo QueryPoseDebugInfo(TransformServer* tf_server, + double timestamp) { + PoseDebugInfo pose_info; + if (tf_server != nullptr) { + pose_info.has_world_pose = + tf_server->QueryPos(timestamp, &pose_info.world_pose); + pose_info.has_lidar_to_vehicle = tf_server->QueryTransform( + kLidarFrameId, kVehicleFrameId, &pose_info.lidar_to_vehicle); + } + if (!pose_info.has_world_pose) { + pose_info.world_pose.setIdentity(); + } + if (!pose_info.has_lidar_to_vehicle) { + pose_info.lidar_to_vehicle.setIdentity(); + } + pose_info.lidar_to_world = + pose_info.world_pose * pose_info.lidar_to_vehicle; + return pose_info; +} + +std::string FormatLidarPoseText(const Eigen::Affine3d& pose) { + std::ostringstream stream; + stream << std::fixed << std::setprecision(2) + << "lidar xyz: (" << pose.translation()(0) << ", " + << pose.translation()(1) << ", " << pose.translation()(2) << ")" + << " yaw(deg): " + << atan2(pose.linear()(1, 0), pose.linear()(0, 0)) * 180.0 / M_PI; + return stream.str(); +} + +std::string FormatTimestampText(double timestamp) { + std::ostringstream stream; + stream << std::fixed << std::setprecision(6) + << "timestamp: " << timestamp; + return stream.str(); +} + +void DrawOverlayLine(cv::Mat* image, int* line_pos, const std::string& text, + double font_scale = 0.85, int thickness = 2) { + *line_pos += 36; + cv::putText(*image, text, cv::Point(10, *line_pos), + cv::FONT_HERSHEY_DUPLEX, font_scale, red_color, thickness); +} + +void DrawPoseOverlay(cv::Mat* image, int* line_pos, double timestamp, + const PoseDebugInfo& pose_info, + std::size_t tracked_object_count) { + DrawOverlayLine(image, line_pos, FormatTimestampText(timestamp)); + DrawOverlayLine(image, line_pos, + absl::StrCat("tracked objects: ", tracked_object_count)); + DrawOverlayLine(image, line_pos, + pose_info.has_world_pose + ? "world pose source: tf_server" + : "world pose source: identity fallback"); + DrawOverlayLine(image, line_pos, + pose_info.has_lidar_to_vehicle + ? "lidar extrinsic: velodyne128 -> base_link" + : "lidar extrinsic: missing tf"); + DrawOverlayLine(image, line_pos, + FormatLidarPoseText(pose_info.lidar_to_world), 0.75, 2); +} + +} // namespace + std::vector colorlistobj = {magenta_color, // last digit 0 purple_color, // last digit 1 teal_color, // last digit 2 @@ -888,17 +967,12 @@ bool Visualizer::DrawTrajectories(const base::ObjectPtr &object, void Visualizer::Draw2Dand3D(const cv::Mat &img, const CameraFrame &frame) { cv::Mat image = img.clone(); - Eigen::Affine3d pose; - if (!tf_server_->QueryPos(frame.timestamp, &pose)) { - pose.setIdentity(); - } - Eigen::Affine3d lidar2vehicle; - if (!tf_server_->QueryTransform("velodyne128", "base_link", - &lidar2vehicle)) { + const PoseDebugInfo pose_info = QueryPoseDebugInfo(tf_server_, frame.timestamp); + if (!pose_info.has_lidar_to_vehicle) { AWARN << "Failed to query transform from lidar to ground."; return; } - Eigen::Affine3d lidar2world = pose * lidar2vehicle; + Eigen::Affine3d lidar2world = pose_info.lidar_to_world; Eigen::Affine3d world2lidar = lidar2world.inverse(); for (const auto &object : frame.tracked_objects) { base::RectF rect(object->camera_supplement.box); @@ -1012,10 +1086,15 @@ void Visualizer::ShowResult(const cv::Mat &img, const CameraFrame &frame) { draw_range_circle(); } + int line_pos = 100; cv::putText(image, camera_name, cv::Point(10, 50), cv::FONT_HERSHEY_DUPLEX, 1.3, red_color, 3); cv::putText(image, absl::StrCat("frame #: ", frame.frame_id), - cv::Point(10, 100), cv::FONT_HERSHEY_DUPLEX, 1.3, red_color, 3); + cv::Point(10, line_pos), cv::FONT_HERSHEY_DUPLEX, 1.3, + red_color, 3); + const PoseDebugInfo pose_info = QueryPoseDebugInfo(tf_server_, frame.timestamp); + DrawPoseOverlay(&image, &line_pos, frame.timestamp, pose_info, + frame.tracked_objects.size()); Draw2Dand3D(image, frame); } @@ -1405,6 +1484,9 @@ void Visualizer::ShowResult_all_info_single_camera( cv::putText(image, absl::StrCat("frame id: ", frame.frame_id), cv::Point(10, line_pos), cv::FONT_HERSHEY_DUPLEX, 1.3, red_color, 3); + const PoseDebugInfo pose_info = QueryPoseDebugInfo(tf_server_, frame.timestamp); + DrawPoseOverlay(&image, &line_pos, frame.timestamp, pose_info, + frame.tracked_objects.size()); line_pos += 50; if (motion_buffer != nullptr) { cv::putText( diff --git a/modules/perception/lidar/lib/detector/graph_segmentation/BUILD b/modules/perception/lidar/lib/detector/graph_segmentation/BUILD index b8bcf973..f9668501 100644 --- a/modules/perception/lidar/lib/detector/graph_segmentation/BUILD +++ b/modules/perception/lidar/lib/detector/graph_segmentation/BUILD @@ -29,3 +29,13 @@ cc_library( ) cpplint() + +cc_test( + name = "graph_segmentation_test", + size = "small", + srcs = ["graph_segmentation_test.cc"], + deps = [ + ":graph_segmentation", + "@com_google_googletest//:gtest_main", + ], +) diff --git a/modules/perception/lidar/lib/detector/graph_segmentation/graph_segmentation_test.cc b/modules/perception/lidar/lib/detector/graph_segmentation/graph_segmentation_test.cc new file mode 100644 index 00000000..8fa3ffd2 --- /dev/null +++ b/modules/perception/lidar/lib/detector/graph_segmentation/graph_segmentation_test.cc @@ -0,0 +1,129 @@ +// Copyright 2026 WheelOS. All Rights Reserved. +// +// 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. + +// Created Date: 2026-04-11 +// Author: daohu527 + +#include "modules/perception/lidar/lib/detector/graph_segmentation/graph_segmentation.h" + +#include +#include +#include + +#include "gtest/gtest.h" + +#include "modules/perception/base/point_cloud.h" + +namespace apollo { +namespace perception { +namespace lidar { + +namespace { + +pipeline::StageConfig MakeStageConfig() { + pipeline::StageConfig stage_config; + stage_config.set_stage_type(pipeline::StageType::GRAPH_CLUSTER_SEGMENTER); + stage_config.set_enabled(true); + stage_config.set_type("GraphClusterSegmenter"); + + auto* config = stage_config.mutable_graph_cluster_segmenter_config(); + config->set_resolution(0.6f); + config->set_threshold(2.0f); + config->set_min_pt_number(5); + config->set_search_radius(2); + config->set_height_threshold(2.0f); + config->set_xmin(-20.0f); + config->set_xmax(20.0f); + config->set_ymin(-20.0f); + config->set_ymax(20.0f); + config->set_semantic_cost(1.0f); + config->set_car_xmin(-0.35f); + config->set_car_xmax(0.35f); + config->set_car_ymin(-0.45f); + config->set_car_ymax(0.45f); + config->set_car_zmax(0.56f); + config->set_min_radius(0.5f); + config->set_z_min_from_ground(0.05f); + config->set_split_aspect_ratio(15.0f); + config->set_split_distance(5.0f); + return stage_config; +} + +void AddPoint(LidarFrame* frame, float x, float y, float z) { + base::PointF local_point; + local_point.x = x; + local_point.y = y; + local_point.z = z; + local_point.intensity = 1.0f; + frame->cloud->push_back(local_point, 0.0); + + base::PointD world_point; + world_point.x = x; + world_point.y = y; + world_point.z = z; + world_point.intensity = 1.0f; + frame->world_cloud->push_back(world_point, 0.0); +} + +} // namespace + +class GraphSegmentationTest : public ::testing::Test { + protected: + void SetUp() override { segmentor_.reset(new GraphSegmentation()); } + + void EmitMetric(const std::string& key, double value) { + ::testing::Test::RecordProperty(key, std::to_string(value)); + } + + std::unique_ptr segmentor_; +}; + +TEST_F(GraphSegmentationTest, ClusterTest) { + LidarFrame frame; + frame.cloud = base::PointFCloudPool::Instance().Get(); + frame.world_cloud = base::PointDCloudPool::Instance().Get(); + frame.lidar2vehicle_extrinsics = Eigen::Affine3d::Identity(); + + // Create two distinct clusters + for (int i = 0; i < 100; ++i) { + AddPoint(&frame, static_cast(i % 10) * 0.1f, + static_cast(i / 10) * 0.1f, 1.0f); + } + + for (int i = 0; i < 100; ++i) { + AddPoint(&frame, 10.0f + static_cast(i % 10) * 0.1f, + 10.0f + static_cast(i / 10) * 0.1f, 1.0f); + } + + frame.cloud->mutable_points_height()->assign( + frame.cloud->size(), std::numeric_limits::max()); + frame.world_cloud->mutable_points_height()->assign( + frame.world_cloud->size(), std::numeric_limits::max()); + frame.non_ground_indices.indices.resize(frame.cloud->size()); + std::iota(frame.non_ground_indices.indices.begin(), + frame.non_ground_indices.indices.end(), 0); + + auto stage_config = MakeStageConfig(); + EXPECT_TRUE(segmentor_->Init(stage_config)); + + LidarDetectorOptions options; + EXPECT_TRUE(segmentor_->Detect(options, &frame)); + + EmitMetric("generated_cluster_count", frame.segmented_objects.size()); + EXPECT_GE(frame.segmented_objects.size(), 2u); +} + +} // namespace lidar +} // namespace perception +} // namespace apollo diff --git a/modules/perception/lidar/lib/ground_detector/spatio_temporal_ground_detector/spatio_temporal_ground_detector_test.cc b/modules/perception/lidar/lib/ground_detector/spatio_temporal_ground_detector/spatio_temporal_ground_detector_test.cc index 2b95f0d0..3e617ab1 100644 --- a/modules/perception/lidar/lib/ground_detector/spatio_temporal_ground_detector/spatio_temporal_ground_detector_test.cc +++ b/modules/perception/lidar/lib/ground_detector/spatio_temporal_ground_detector/spatio_temporal_ground_detector_test.cc @@ -1,143 +1,149 @@ -/****************************************************************************** - * Copyright 2018 The Apollo Authors. All Rights Reserved. - * - * 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. - *****************************************************************************/ +// Copyright 2026 WheelOS. All Rights Reserved. +// +// 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. + +// Created Date: 2026-04-11 +// Author: daohu527 #include "modules/perception/lidar/lib/ground_detector/spatio_temporal_ground_detector/spatio_temporal_ground_detector.h" -#include "gflags/gflags.h" +#include +#include + #include "gtest/gtest.h" -#include "pcl/io/pcd_io.h" +#include "modules/perception/base/point_cloud.h" +#include "modules/perception/common/point_cloud_processing/common.h" +#include "modules/perception/lidar/common/lidar_point_label.h" namespace apollo { namespace perception { -namespace lib { +namespace lidar { -DECLARE_string(work_root); -DECLARE_string(config_manager_path); +namespace { + +pipeline::StageConfig MakeStageConfig() { + pipeline::StageConfig stage_config; + stage_config.set_stage_type(pipeline::StageType::GROUND_SEGMENTER); + stage_config.set_enabled(true); + stage_config.set_type("SpatioTemporalGroundDetector"); + + auto* config = stage_config.mutable_ground_segmenter_config(); + config->set_grid_size(32); + config->set_ground_thres(0.25f); + config->set_roi_rad_x(120.0f); + config->set_roi_rad_y(120.0f); + config->set_roi_rad_z(100.0f); + config->set_nr_smooth_iter(5); + config->set_use_roi(false); + config->set_use_ground_service(false); + config->set_sample_region_z_lower(-3.0f); + config->set_sample_region_z_upper(-1.0f); + config->set_roi_near_rad(32.0f); + config->set_planefit_orien_threshold(5.0f); + config->set_big_grid_size(256); + config->set_small_grid_size(32); + config->set_z_compare_thres(0.1f); + config->set_smooth_z_thres(1.0f); + config->set_planefit_dist_thres_near(0.1f); + config->set_planefit_dist_thres_far(0.2f); + config->set_inliers_min_threshold(6); + config->set_near_range_dist(10.0f); + config->set_near_range_ground_thres(0.25f); + config->set_middle_range_dist(20.0f); + config->set_middle_range_ground_thres(0.25f); + return stage_config; +} -} // namespace lib +} // namespace -namespace lidar { +class SpatioTemporalGroundDetectorTest : public ::testing::Test { + protected: + void SetUp() override { detector_.reset(new SpatioTemporalGroundDetector()); } -bool LoadPCDFile(const std::string& file_path, base::PointFCloudPtr cloud_out) { - int ret = 0; - pcl::PointCloud org_cloud; - if ((ret = pcl::io::loadPCDFile(file_path, org_cloud)) < 0) { - AERROR << "Failed to load pcd file: " << file_path << " " << ret; - return false; + void EmitMetric(const std::string& key, double value) { + ::testing::Test::RecordProperty(key, std::to_string(value)); } - cloud_out->resize(org_cloud.size()); - int pid = 0; - for (size_t i = 0; i < org_cloud.size(); ++i) { - if (std::isnan(org_cloud.at(i).x) || std::isnan(org_cloud.at(i).y) || - std::isnan(org_cloud.at(i).z)) { - continue; - } - base::PointF& pt = cloud_out->at(pid++); - pt.x = org_cloud.at(i).x; - pt.y = org_cloud.at(i).y; - pt.z = org_cloud.at(i).z; - pt.intensity = org_cloud.at(i).intensity; + std::unique_ptr detector_; +}; + +TEST_F(SpatioTemporalGroundDetectorTest, FlatGroundTest) { + LidarFrame frame; + frame.cloud = base::PointFCloudPool::Instance().Get(); + frame.world_cloud = base::PointDCloudPool::Instance().Get(); + + // Synthetic planar ground + int total_ground = 500; + for (int i = 0; i < total_ground; ++i) { + base::PointF pt; + pt.x = static_cast(i % 20); + pt.y = static_cast(i / 20); + // Add small noise + pt.z = 0.05f * ((i % 3) - 1); + frame.cloud->push_back(pt, 0.0); } - cloud_out->resize(pid); - return true; -} + // Non-ground points + int total_non_ground = 50; + for (int i = 0; i < total_non_ground; ++i) { + base::PointF pt; + pt.x = static_cast(i); + pt.y = static_cast(i); + pt.z = 2.0f; + frame.cloud->push_back(pt, 0.0); + } + + // Need to populate world cloud to avoid crash if it depends on it + for (size_t i = 0; i < frame.cloud->size(); ++i) { + base::PointD ptd; + ptd.x = frame.cloud->at(i).x; + ptd.y = frame.cloud->at(i).y; + ptd.z = frame.cloud->at(i).z; + frame.world_cloud->push_back(ptd, 0.0); + } + + frame.cloud->mutable_points_height()->assign( + frame.cloud->size(), std::numeric_limits::max()); + frame.world_cloud->mutable_points_height()->assign( + frame.world_cloud->size(), std::numeric_limits::max()); + frame.cloud->mutable_points_label()->assign(frame.cloud->size(), 0u); + frame.world_cloud->mutable_points_label()->assign(frame.world_cloud->size(), + 0u); + frame.lidar2vehicle_extrinsics = Eigen::Affine3d::Identity(); + frame.lidar2world_pose = Eigen::Affine3d::Identity(); + + auto stage_config = MakeStageConfig(); + EXPECT_TRUE(detector_->Init(stage_config)); + + GroundDetectorOptions options; + EXPECT_TRUE(detector_->Detect(options, &frame)); + + int ground_count = 0; + int non_ground_count = 0; + for (size_t i = 0; i < frame.cloud->size(); ++i) { + if (frame.cloud->points_label(i) == + static_cast(LidarPointLabel::GROUND)) { + ground_count++; + } else { + non_ground_count++; + } + } -TEST(SpatioTemporalGroundDetectorTest, test_spatio_temporal_ground_detector) { - // char cyber_path[100] = "CYBER_PATH="; - // putenv(cyber_path); - // char module_path[100] = "MODULE_PATH="; - // putenv(module_path); - // EXPECT_TRUE(SceneManager::Instance().Init()); - - // // load pcd data - // base::PointFCloudPtr pc_ptr; - // pc_ptr.reset(new base::PointFCloud); - // std::string filename = - // "modules/perception/testdata/lidar/lib/" - // "ground_detector/spatio_temporal_ground_detector/" - // "pcd_data/QB9178_3_1461752790_1461753090_36701.pcd"; - // bool ret = LoadPCDFile(filename, pc_ptr); - // ACHECK(ret) << "Failed to load " << filename; - - // // test init - // std::shared_ptr detector( - // new SpatioTemporalGroundDetector); - // EXPECT_TRUE(detector->Init()); - // EXPECT_STREQ("SpatioTemporalGroundDetector", detector->Name().c_str()); - - // // test input - // GroundDetectorOptions options; - // std::shared_ptr frame_data = nullptr; - // EXPECT_FALSE(detector->Detect(options, frame_data.get())); - // frame_data = std::shared_ptr(new LidarFrame); - // EXPECT_FALSE(detector->Detect(options, frame_data.get())); - // frame_data->cloud = base::PointFCloudPool::Instance().Get(); - // frame_data->world_cloud = base::PointDCloudPool::Instance().Get(); - // EXPECT_FALSE(detector->Detect(options, frame_data.get())); - - // // test use_roi_ = false - // frame_data->cloud = pc_ptr; - // frame_data->lidar2world_pose = Eigen::Affine3d::Identity(); - // base::PointD temp; - // for (size_t i = 0; i < frame_data->cloud->size(); ++i) { - // const auto& pt = frame_data->cloud->at(i); - // temp.x = pt.x; - // temp.y = pt.y; - // temp.z = pt.z; - // frame_data->world_cloud->push_back(temp); - // } - // EXPECT_TRUE(detector->Detect(options, frame_data.get())); - // EXPECT_GT(frame_data->non_ground_indices.indices.size(), 0); - - // // test use_roi_ = true && default_point_size_ = 100 - // detector->use_roi_ = true; - // detector->default_point_size_ = 10; - // size_t roi_num = static_cast(0.75 * pc_ptr->size()); - // std::vector roi_indices(roi_num); - // std::iota(roi_indices.begin(), roi_indices.end(), 0); - // frame_data->roi_indices.indices = roi_indices; - // EXPECT_TRUE(detector->Detect(options, frame_data.get())); - // EXPECT_EQ(roi_num * 2, detector->default_point_size_); - // EXPECT_EQ(roi_num * 2, detector->point_indices_temp_.size()); - // EXPECT_EQ(roi_num * 6, detector->data_.size()); - // EXPECT_GT(frame_data->non_ground_indices.indices.size(), 0); - - // detector->use_roi_ = false; - // EXPECT_TRUE(detector->Detect(options, frame_data.get())); - // EXPECT_GT(frame_data->non_ground_indices.indices.size(), 0); - // auto ground_service = SceneManager::Instance().Service("GroundService"); - // GroundServicePtr ground_service_cast = - // std::dynamic_pointer_cast(ground_service); - - // Eigen::Vector3d world_point(0.0, 0.0, 0.0); - // float out_query = 0.0f; - // float out_detected = 0.0f; - // for (size_t i = 0; i < 10; ++i) { - // const auto& index = frame_data->non_ground_indices.indices[i]; - // const auto& pt = frame_data->world_cloud->at(index); - // out_detected = frame_data->world_cloud->points_height(index); - // world_point(0) = pt.x; - // world_point(1) = pt.y; - // world_point(2) = pt.z; - // out_query = ground_service_cast->QueryPointToGroundDistance(world_point); - // EXPECT_NEAR(out_query, out_detected, 1e-6); - // } + EmitMetric("ground_points_count", ground_count); + EmitMetric("non_ground_points_count", non_ground_count); + EmitMetric("total_input_points", frame.cloud->size()); + EXPECT_GT(ground_count, 0); } } // namespace lidar diff --git a/modules/perception/lidar/lib/object_builder/BUILD b/modules/perception/lidar/lib/object_builder/BUILD index 77dd161d..f0f011be 100644 --- a/modules/perception/lidar/lib/object_builder/BUILD +++ b/modules/perception/lidar/lib/object_builder/BUILD @@ -1,4 +1,4 @@ -load("@rules_cc//cc:defs.bzl", "cc_library") +load("@rules_cc//cc:defs.bzl", "cc_library", "cc_test") load("//tools:cpplint.bzl", "cpplint") package(default_visibility = ["//visibility:public"]) @@ -18,4 +18,14 @@ cc_library( ], ) +cc_test( + name = "object_builder_test", + size = "small", + srcs = ["object_builder_test.cc"], + deps = [ + ":object_builder", + "@com_google_googletest//:gtest_main", + ], +) + cpplint() diff --git a/modules/perception/lidar/lib/object_builder/object_builder_test.cc b/modules/perception/lidar/lib/object_builder/object_builder_test.cc new file mode 100644 index 00000000..43a1c53f --- /dev/null +++ b/modules/perception/lidar/lib/object_builder/object_builder_test.cc @@ -0,0 +1,135 @@ +// Copyright 2026 WheelOS. All Rights Reserved. +// +// 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. + +// Created Date: 2026-03-06 +// Author: daohu527 + +#include "modules/perception/lidar/lib/object_builder/object_builder.h" + +#include +#include +#include +#include + +#include "gtest/gtest.h" + +namespace apollo { +namespace perception { +namespace lidar { + +namespace { + +template +void EmitMetric(const std::string& name, const T& value) { + std::ostringstream stream; + stream << value; + std::cout << "METRIC " << name << "=" << stream.str() << std::endl; + ::testing::Test::RecordProperty(name, stream.str()); +} + +pipeline::StageConfig MakeStageConfig() { + pipeline::StageConfig stage_config; + stage_config.set_stage_type(pipeline::StageType::OBJECT_GEOMETRY_BUILDER); + stage_config.set_enabled(true); + stage_config.set_type("ObjectGeometryBuilder"); + stage_config.mutable_object_geometry_builder_config(); + return stage_config; +} + +void AddObjectPoint(base::ObjectPtr object, float x, float y, float z, + double timestamp) { + base::PointF point; + point.x = x; + point.y = y; + point.z = z; + object->lidar_supplement.cloud.push_back(point, timestamp); +} + +base::ObjectPtr MakeBoxLikeObject() { + auto object = std::make_shared(); + AddObjectPoint(object, 0.0f, 0.0f, 0.0f, 0.1); + AddObjectPoint(object, 4.0f, 0.0f, 0.0f, 0.2); + AddObjectPoint(object, 4.0f, 2.0f, 1.0f, 0.3); + AddObjectPoint(object, 0.0f, 2.0f, 1.0f, 0.4); + return object; +} + +base::ObjectPtr MakeDegenerateObject() { + auto object = std::make_shared(); + AddObjectPoint(object, 10.0f, 0.0f, 0.0f, 0.5); + AddObjectPoint(object, 10.0f, 1.0f, 0.0f, 0.7); + return object; +} + +} // namespace + +TEST(ObjectGeometryBuilderStageTest, process_stage_metrics) { + ObjectGeometryBuilder builder; + auto stage_config = MakeStageConfig(); + EXPECT_TRUE(builder.Init(stage_config)); + EXPECT_EQ(builder.Name(), "ObjectGeometryBuilder"); + + LidarFrame frame; + frame.segmented_objects.push_back(MakeBoxLikeObject()); + frame.segmented_objects.push_back(MakeDegenerateObject()); + + pipeline::DataFrame data_frame{}; + data_frame.lidar_frame = &frame; + + EXPECT_FALSE(builder.Process(nullptr)); + EXPECT_TRUE(builder.Process(&data_frame)); + + ASSERT_EQ(frame.segmented_objects.size(), 2u); + + const auto& primary_object = frame.segmented_objects[0]; + ASSERT_NE(primary_object, nullptr); + EXPECT_EQ(primary_object->id, 0); + EXPECT_EQ(primary_object->polygon.size(), 4u); + EXPECT_NEAR(primary_object->center.x(), 2.0, 1e-3); + EXPECT_NEAR(primary_object->center.y(), 1.0, 1e-3); + EXPECT_NEAR(primary_object->center.z(), 0.0, 1e-3); + EXPECT_NEAR(primary_object->size(0), 4.0f, 1e-3f); + EXPECT_NEAR(primary_object->size(1), 2.0f, 1e-3f); + EXPECT_NEAR(primary_object->size(2), 1.0f, 1e-3f); + EXPECT_NEAR(primary_object->theta, 0.0f, 1e-6f); + EXPECT_NEAR(primary_object->latest_tracked_time, 0.25, 1e-6); + + const auto& degenerate_object = frame.segmented_objects[1]; + ASSERT_NE(degenerate_object, nullptr); + EXPECT_EQ(degenerate_object->id, 1); + EXPECT_EQ(degenerate_object->polygon.size(), 4u); + EXPECT_NEAR(degenerate_object->center.x(), 10.0, 1e-3); + EXPECT_NEAR(degenerate_object->center.y(), 0.5, 1e-3); + EXPECT_NEAR(degenerate_object->center.z(), 0.0, 1e-3); + EXPECT_GE(degenerate_object->size(0), 1.0f); + EXPECT_GE(degenerate_object->size(1), 0.01f); + EXPECT_GE(degenerate_object->size(2), 0.01f); + EXPECT_NEAR(degenerate_object->latest_tracked_time, 0.6, 1e-6); + + EmitMetric("object_count", frame.segmented_objects.size()); + EmitMetric("primary_polygon_vertices", primary_object->polygon.size()); + EmitMetric("degenerate_polygon_vertices", degenerate_object->polygon.size()); + EmitMetric("primary_length", primary_object->size(0)); + EmitMetric("primary_width", primary_object->size(1)); + EmitMetric("primary_height", primary_object->size(2)); + EmitMetric("primary_latest_tracked_time", + primary_object->latest_tracked_time); + EmitMetric("degenerate_length", degenerate_object->size(0)); + EmitMetric("degenerate_width", degenerate_object->size(1)); + EmitMetric("degenerate_height", degenerate_object->size(2)); +} + +} // namespace lidar +} // namespace perception +} // namespace apollo diff --git a/modules/perception/lidar/lib/object_filter_bank/BUILD b/modules/perception/lidar/lib/object_filter_bank/BUILD index f18679db..86051f4b 100644 --- a/modules/perception/lidar/lib/object_filter_bank/BUILD +++ b/modules/perception/lidar/lib/object_filter_bank/BUILD @@ -21,16 +21,15 @@ cc_library( ], ) -# todo(zero): need fix -# cc_test( -# name = "object_filter_bank_test", -# size = "small", -# srcs = ["object_filter_bank_test.cc"], -# deps = [ -# ":object_filter_bank", -# "@com_github_gflags_gflags//:gflags", -# "@com_google_googletest//:gtest_main", -# ], -# ) +cc_test( + name = "object_filter_bank_test", + size = "small", + srcs = ["object_filter_bank_test.cc"], + deps = [ + ":object_filter_bank", + "@com_github_gflags_gflags//:gflags", + "@com_google_googletest//:gtest_main", + ], +) cpplint() diff --git a/modules/perception/lidar/lib/object_filter_bank/object_filter_bank_test.cc b/modules/perception/lidar/lib/object_filter_bank/object_filter_bank_test.cc index 13ccfaaa..b122a7b5 100644 --- a/modules/perception/lidar/lib/object_filter_bank/object_filter_bank_test.cc +++ b/modules/perception/lidar/lib/object_filter_bank/object_filter_bank_test.cc @@ -1,87 +1,89 @@ -/****************************************************************************** - * Copyright 2018 The Apollo Authors. All Rights Reserved. - * - * 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 "gflags/gflags.h" -#include "gtest/gtest.h" +// Copyright 2026 WheelOS. All Rights Reserved. +// +// 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. + +// Created Date: 2026-04-11 +// Author: daohu527 #include "modules/perception/lidar/lib/object_filter_bank/object_filter_bank.h" -DECLARE_string(work_root); -DECLARE_string(config_manager_path); +#include + +#include "gtest/gtest.h" + +#include "modules/perception/base/object_pool_types.h" +#include "modules/perception/base/point_cloud.h" namespace apollo { namespace perception { namespace lidar { -class MockObjectFilter1 : public BaseObjectFilter { - public: - MockObjectFilter1() = default; +namespace { - virtual ~MockObjectFilter1() = default; +pipeline::StageConfig MakeStageConfig() { + pipeline::StageConfig stage_config; + stage_config.set_stage_type(pipeline::StageType::OBJECT_POST_FILTER_BANK); + stage_config.set_enabled(true); + stage_config.set_type("ObjectPostFilterBank"); + return stage_config; +} - bool Init(const ObjectFilterInitOptions& options = - ObjectFilterInitOptions()) override { - return true; - } +} // namespace + +class ObjectFilterBankTest : public ::testing::Test { + protected: + void SetUp() override { filter_bank_.reset(new ObjectFilterBank()); } - bool Filter(const ObjectFilterOptions& options, LidarFrame* frame) override { - return false; + void EmitMetric(const std::string& key, double value) { + ::testing::Test::RecordProperty(key, std::to_string(value)); } - std::string Name() const override { return "MockObjectFilter1"; } -}; // class MockObjectFilter1 -PERCEPTION_REGISTER_OBJECTFILTER(MockObjectFilter1); + std::unique_ptr filter_bank_; +}; -class MockObjectFilter2 : public BaseObjectFilter { - public: - MockObjectFilter2() = default; +TEST_F(ObjectFilterBankTest, EmptyPluginBankKeepsObjectsStable) { + LidarFrame frame; + frame.cloud = base::PointFCloudPool::Instance().Get(); + frame.world_cloud = base::PointDCloudPool::Instance().Get(); - virtual ~MockObjectFilter2() = default; + // Create one valid object and one invalid (too small/noise) + auto obj1 = base::ObjectPool::Instance().Get(); + obj1->id = 1; + obj1->size = Eigen::Vector3f(2.0, 1.0, 1.0); // Good size + obj1->center = Eigen::Vector3d(5.0, 5.0, 0.0); + frame.segmented_objects.push_back(obj1); - bool Init(const ObjectFilterInitOptions& options = - ObjectFilterInitOptions()) override { - return false; - } + auto obj2 = base::ObjectPool::Instance().Get(); + obj2->id = 2; + obj2->size = Eigen::Vector3f(0.01, 0.01, 0.01); // Too small + obj2->center = Eigen::Vector3d(10.0, 10.0, 0.0); + frame.segmented_objects.push_back(obj2); - bool Filter(const ObjectFilterOptions& options, LidarFrame* frame) override { - return false; - } + const size_t initial_count = frame.segmented_objects.size(); - std::string Name() const override { return "MockObjectFilter2"; } -}; // class MockObjectFilter -PERCEPTION_REGISTER_OBJECTFILTER(MockObjectFilter2); - -TEST(LidarLibObjectFilterBankTest, lidar_lib_object_filter_bank_test) { - // FIXME(perception): fix missing data files - return; - - char cyber_path[100] = "CYBER_PATH="; - putenv(cyber_path); - char module_path[100] = "MODULE_PATH="; - putenv(module_path); - FLAGS_work_root = - "/apollo/modules/perception/testdata/" - "lidar/lib/object_filter_bank/filter_bank"; - - ObjectFilterBank filter_bank; - EXPECT_EQ(filter_bank.Name(), "ObjectPostFilterBank"); - EXPECT_TRUE(filter_bank.Init()); - EXPECT_EQ(filter_bank.Size(), 3); - LidarFrame frame; - ObjectFilterOptions option; - EXPECT_TRUE(filter_bank.Filter(option, &frame)); + auto stage_config = MakeStageConfig(); + EXPECT_TRUE(filter_bank_->Init(stage_config)); + + pipeline::DataFrame data_frame{}; + data_frame.lidar_frame = &frame; + EXPECT_TRUE(filter_bank_->Process(&data_frame)); + + const size_t final_count = frame.segmented_objects.size(); + + EmitMetric("objects_pre_filter", initial_count); + EmitMetric("objects_post_filter", final_count); + EmitMetric("objects_filtered_out", initial_count - final_count); + EXPECT_EQ(final_count, initial_count); } } // namespace lidar diff --git a/modules/perception/lidar/lib/pointcloud_preprocessor/BUILD b/modules/perception/lidar/lib/pointcloud_preprocessor/BUILD index 323b7f8f..9cc708b7 100644 --- a/modules/perception/lidar/lib/pointcloud_preprocessor/BUILD +++ b/modules/perception/lidar/lib/pointcloud_preprocessor/BUILD @@ -1,4 +1,4 @@ -load("@rules_cc//cc:defs.bzl", "cc_library") +load("@rules_cc//cc:defs.bzl", "cc_library", "cc_test") load("//tools:cpplint.bzl", "cpplint") package(default_visibility = ["//visibility:public"]) @@ -24,4 +24,14 @@ cc_library( alwayslink = True, ) +cc_test( + name = "pointcloud_preprocessor_test", + size = "small", + srcs = ["pointcloud_preprocessor_test.cc"], + deps = [ + ":pointcloud_preprocessor", + "@com_google_googletest//:gtest_main", + ], +) + cpplint() diff --git a/modules/perception/lidar/lib/pointcloud_preprocessor/pointcloud_preprocessor_test.cc b/modules/perception/lidar/lib/pointcloud_preprocessor/pointcloud_preprocessor_test.cc index 342bd26a..5233c0bc 100644 --- a/modules/perception/lidar/lib/pointcloud_preprocessor/pointcloud_preprocessor_test.cc +++ b/modules/perception/lidar/lib/pointcloud_preprocessor/pointcloud_preprocessor_test.cc @@ -16,28 +16,52 @@ #include "modules/perception/lidar/lib/pointcloud_preprocessor/pointcloud_preprocessor.h" -DECLARE_string(work_root); +#include + +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "modules/perception/base/point_cloud.h" +#include "modules/perception/lidar/common/lidar_frame.h" namespace apollo { namespace perception { namespace lidar { -class PointCloudPreprocessorTest : public testing::Test { - protected: - void SetUp() { - char cyber_path[100] = "CYBER_PATH="; - putenv(cyber_path); - char module_path[100] = "MODULE_PATH="; - putenv(module_path); - FLAGS_work_root = - "/apollo/modules/perception/testdata/" - "lidar/lib/pointcloud_preprocessor"; - } - void TearDown() {} +namespace { - protected: - PointCloudPreprocessor preprocessor; -}; +template +void EmitMetric(const std::string& name, const T& value) { + std::ostringstream stream; + stream << value; + std::cout << "METRIC " << name << "=" << stream.str() << std::endl; + ::testing::Test::RecordProperty(name, stream.str()); +} + +pipeline::StageConfig MakeStageConfig() { + pipeline::StageConfig stage_config; + stage_config.set_stage_type(pipeline::StageType::POINTCLOUD_PREPROCESSOR); + stage_config.set_enabled(true); + stage_config.set_type("PointCloudPreprocessor"); + + auto* config = stage_config.mutable_pointcloud_preprocessor_config(); + config->set_filter_naninf_points(true); + config->set_filter_nearby_box_points(true); + config->set_box_forward_x(2.0); + config->set_box_backward_x(-2.0); + config->set_box_forward_y(2.0); + config->set_box_backward_y(-2.0); + config->set_filter_high_z_points(true); + config->set_z_threshold(5.0); + return stage_config; +} + +} // namespace void MockPointcloud(base::PointFCloud* cloud) { cloud->resize(10); @@ -60,73 +84,55 @@ void MockPointcloud(base::PointFCloud* cloud) { cloud->at(7).z = 10.f; // 5. two normal points } -#ifdef PERCEPTION_LIDAR_USE_COMMON_MESSAGE -void MockMessage(adu::common::sensor::PointCloud* message) { - message->set_measurement_time(0.0); - for (size_t i = 0; i < 10; ++i) { - message->add_point(); - message->mutable_point(i)->set_x(5.f * i); - message->mutable_point(i)->set_y(5.f * i); - message->mutable_point(i)->set_z(0.f); - } - // 1. three nan points - message->mutable_point(0)->set_x(std::numeric_limits::quiet_NaN()); - message->mutable_point(1)->set_y(std::numeric_limits::quiet_NaN()); - message->mutable_point(2)->set_z(std::numeric_limits::quiet_NaN()); - // 2. three inf points - message->mutable_point(3)->set_x(10000.f); - message->mutable_point(4)->set_y(10000.f); - message->mutable_point(5)->set_z(10000.f); - // 3. one box points - message->mutable_point(6)->set_x(0.f); - message->mutable_point(6)->set_y(0.f); - // 4. one large z points - message->mutable_point(7)->set_z(10.f); - // 5. two normal points -} -#endif -TEST_F(PointCloudPreprocessorTest, basic_test) { +TEST(PointCloudPreprocessorStageTest, process_stage_metrics) { + PointCloudPreprocessor preprocessor; + auto stage_config = MakeStageConfig(); + EXPECT_TRUE(preprocessor.Init(stage_config)); EXPECT_EQ(preprocessor.Name(), "PointCloudPreprocessor"); - EXPECT_TRUE(preprocessor.Init()); - PointCloudPreprocessorOptions option; - { - LidarFrame frame; - EXPECT_FALSE(preprocessor.Preprocess(option, nullptr)); - EXPECT_FALSE(preprocessor.Preprocess(option, &frame)); - frame.cloud = base::PointFCloudPool::Instance().Get(); - frame.lidar2world_pose = Eigen::Affine3d::Identity(); - option.sensor2vehicle_extrinsics = Eigen::Affine3d::Identity(); - MockPointcloud(frame.cloud.get()); - EXPECT_EQ(frame.cloud->size(), 10); - EXPECT_TRUE(preprocessor.Preprocess(option, &frame)); - EXPECT_EQ(frame.cloud->size(), 2); - EXPECT_EQ(frame.world_cloud->size(), 2); - for (size_t i = 0; i < frame.cloud->size(); ++i) { - auto& pt = frame.cloud->at(i); - auto& world_pt = frame.world_cloud->at(i); - EXPECT_EQ(pt.x, world_pt.x); - EXPECT_EQ(pt.y, world_pt.y); - EXPECT_EQ(pt.z, world_pt.z); - EXPECT_EQ(pt.intensity, world_pt.intensity); - EXPECT_EQ(frame.cloud->points_beam_id()[i], - frame.world_cloud->points_beam_id()[i]); - } - } -#ifdef PERCEPTION_LIDAR_USE_COMMON_MESSAGE - { - std::shared_ptr message( - new adu::common::sensor::PointCloud); - LidarFrame frame; - EXPECT_FALSE(preprocessor.Preprocess(option, message, nullptr)); - MockMessage(message.get()); - EXPECT_EQ(message->point_size(), 10); - frame.lidar2world_pose = Eigen::Affine3d::Identity(); - EXPECT_TRUE(preprocessor.Preprocess(option, message, &frame)); - EXPECT_EQ(frame.cloud->size(), 2); - EXPECT_EQ(frame.world_cloud->size(), 2); + + pipeline::DataFrame data_frame{}; + LidarFrame frame; + data_frame.lidar_frame = &frame; + + EXPECT_FALSE(preprocessor.Process(nullptr)); + EXPECT_FALSE(preprocessor.Process(&data_frame)); + + frame.cloud.reset(new base::PointFCloud); + frame.lidar2vehicle_extrinsics = Eigen::Affine3d::Identity(); + frame.lidar2world_pose = Eigen::Affine3d::Identity(); + frame.lidar2world_pose.pretranslate(Eigen::Vector3d(5.0, -2.0, 1.5)); + MockPointcloud(frame.cloud.get()); + + const size_t input_points = frame.cloud->size(); + EXPECT_EQ(input_points, 10u); + EXPECT_TRUE(preprocessor.Process(&data_frame)); + + ASSERT_NE(frame.world_cloud, nullptr); + EXPECT_EQ(frame.cloud->size(), 2u); + EXPECT_EQ(frame.world_cloud->size(), 2u); + + std::set> remaining_xy; + for (size_t i = 0; i < frame.cloud->size(); ++i) { + const auto& pt = frame.cloud->at(i); + const auto& world_pt = frame.world_cloud->at(i); + remaining_xy.emplace(static_cast(pt.x), static_cast(pt.y)); + EXPECT_NEAR(world_pt.x - pt.x, 5.0, 1e-6); + EXPECT_NEAR(world_pt.y - pt.y, -2.0, 1e-6); + EXPECT_NEAR(world_pt.z - pt.z, 1.5, 1e-6); + EXPECT_EQ(pt.intensity, world_pt.intensity); } -#endif + + std::set> expected_xy = {{40, 40}, {45, 45}}; + EXPECT_EQ(remaining_xy, expected_xy); + + EmitMetric("input_points", input_points); + EmitMetric("removed_naninf_or_inf_points", 6); + EmitMetric("removed_nearby_box_points", 1); + EmitMetric("removed_high_z_points", 1); + EmitMetric("output_points", frame.cloud->size()); + EmitMetric("output_ratio", static_cast(frame.cloud->size()) / + static_cast(input_points)); } } // namespace lidar diff --git a/modules/perception/lidar/lib/roi_filter/hdmap_roi_filter/BUILD b/modules/perception/lidar/lib/roi_filter/hdmap_roi_filter/BUILD index 93c5bb72..e3dc1b34 100644 --- a/modules/perception/lidar/lib/roi_filter/hdmap_roi_filter/BUILD +++ b/modules/perception/lidar/lib/roi_filter/hdmap_roi_filter/BUILD @@ -1,4 +1,4 @@ -load("@rules_cc//cc:defs.bzl", "cc_library") +load("@rules_cc//cc:defs.bzl", "cc_library", "cc_test") load("//tools:cpplint.bzl", "cpplint") package(default_visibility = ["//visibility:public"]) @@ -41,15 +41,17 @@ cc_library( alwayslink = True, ) -#cc_test( -# name = "hdmap_roi_filter_test", -# size = "small", -# srcs = ["hdmap_roi_filter_test.cc"], -# deps = [ -# ":hdmap_roi_filter", -# "@com_google_googletest//:gtest_main", -# ], -#) +cc_test( + name = "hdmap_roi_filter_stage_test", + size = "small", + srcs = ["hdmap_roi_filter_stage_test.cc"], + deps = [ + ":hdmap_roi_filter", + "//modules/perception/base:base_type", + "//modules/perception/lidar/common:lidar_point_label", + "@com_google_googletest//:gtest_main", + ], +) cc_library( name = "polygon_mask", diff --git a/modules/perception/lidar/lib/roi_filter/hdmap_roi_filter/hdmap_roi_filter_stage_test.cc b/modules/perception/lidar/lib/roi_filter/hdmap_roi_filter/hdmap_roi_filter_stage_test.cc new file mode 100644 index 00000000..d51d63dd --- /dev/null +++ b/modules/perception/lidar/lib/roi_filter/hdmap_roi_filter/hdmap_roi_filter_stage_test.cc @@ -0,0 +1,144 @@ +// Copyright 2026 WheelOS. All Rights Reserved. +// +// 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. + +// Created Date: 2026-03-06 +// Author: daohu527 + +#include +#include +#include + +#include "gtest/gtest.h" + +#include "modules/perception/base/hdmap_struct.h" +#include "modules/perception/lidar/common/lidar_point_label.h" +#include "modules/perception/lidar/lib/roi_filter/hdmap_roi_filter/hdmap_roi_filter.h" + +namespace apollo { +namespace perception { +namespace lidar { + +namespace { + +template +void EmitMetric(const std::string& name, const T& value) { + std::ostringstream stream; + stream << value; + std::cout << "METRIC " << name << "=" << stream.str() << std::endl; + ::testing::Test::RecordProperty(name, stream.str()); +} + +pipeline::StageConfig MakeStageConfig() { + pipeline::StageConfig stage_config; + stage_config.set_stage_type(pipeline::StageType::POINTCLOUD_ROI_FILTER); + stage_config.set_enabled(true); + stage_config.set_type("HdmapPointCloudRoiFilter"); + + auto* config = stage_config.mutable_pointcloud_roi_filter_config(); + config->set_range(10.0); + config->set_cell_size(0.25); + config->set_extend_dist(0.0); + config->set_no_edge_table(false); + config->set_set_roi_service(false); + return stage_config; +} + +void AddPoint(LidarFrame* frame, float x, float y, float z) { + base::PointF local_point; + local_point.x = x; + local_point.y = y; + local_point.z = z; + frame->cloud->push_back(local_point, 0.0); + + base::PointD world_point; + world_point.x = x; + world_point.y = y; + world_point.z = z; + frame->world_cloud->push_back(world_point, 0.0); +} + +base::PolygonDType MakeSquarePolygon(double min_x, double min_y, double max_x, + double max_y) { + base::PolygonDType polygon; + polygon.resize(4); + + polygon[0].x = min_x; + polygon[0].y = min_y; + polygon[0].z = 0.0; + polygon[1].x = max_x; + polygon[1].y = min_y; + polygon[1].z = 0.0; + polygon[2].x = max_x; + polygon[2].y = max_y; + polygon[2].z = 0.0; + polygon[3].x = min_x; + polygon[3].y = max_y; + polygon[3].z = 0.0; + return polygon; +} + +} // namespace + +TEST(HdmapPointCloudRoiFilterStageTest, process_stage_metrics) { + HdmapPointCloudRoiFilter filter; + auto stage_config = MakeStageConfig(); + EXPECT_TRUE(filter.Init(stage_config)); + EXPECT_EQ(filter.Name(), "HdmapPointCloudRoiFilter"); + + LidarFrame frame; + frame.cloud.reset(new base::PointFCloud); + frame.world_cloud.reset(new base::PointDCloud); + frame.hdmap_struct.reset(new base::HdmapStruct); + frame.lidar2world_pose = Eigen::Affine3d::Identity(); + frame.hdmap_struct->road_polygons.push_back( + MakeSquarePolygon(0.0, 0.0, 4.0, 4.0)); + + AddPoint(&frame, 1.0f, 1.0f, 0.0f); + AddPoint(&frame, 3.0f, 1.0f, 0.0f); + AddPoint(&frame, 2.0f, 3.0f, 0.0f); + AddPoint(&frame, -1.0f, 1.0f, 0.0f); + AddPoint(&frame, 5.0f, 5.0f, 0.0f); + + pipeline::DataFrame data_frame{}; + data_frame.lidar_frame = &frame; + + EXPECT_FALSE(filter.Process(nullptr)); + EXPECT_TRUE(filter.Process(&data_frame)); + + ASSERT_EQ(frame.roi_indices.indices.size(), 3u); + EXPECT_EQ(frame.roi_indices.indices[0], 0); + EXPECT_EQ(frame.roi_indices.indices[1], 1); + EXPECT_EQ(frame.roi_indices.indices[2], 2); + + const auto roi_label = static_cast(LidarPointLabel::ROI); + for (size_t i = 0; i < frame.cloud->size(); ++i) { + if (i < 3) { + EXPECT_EQ(frame.cloud->points_label(i), roi_label); + EXPECT_EQ(frame.world_cloud->points_label(i), roi_label); + } else { + EXPECT_EQ(frame.cloud->points_label(i), 0u); + EXPECT_EQ(frame.world_cloud->points_label(i), 0u); + } + } + + EmitMetric("input_points", frame.cloud->size()); + EmitMetric("roi_points", frame.roi_indices.indices.size()); + EmitMetric("roi_ratio", + static_cast(frame.roi_indices.indices.size()) / + static_cast(frame.cloud->size())); +} + +} // namespace lidar +} // namespace perception +} // namespace apollo diff --git a/modules/perception/lidar/tools/offline_lidar_benchmark_runner.md b/modules/perception/lidar/tools/offline_lidar_benchmark_runner.md index d47884b4..8996fcd9 100644 --- a/modules/perception/lidar/tools/offline_lidar_benchmark_runner.md +++ b/modules/perception/lidar/tools/offline_lidar_benchmark_runner.md @@ -160,10 +160,13 @@ http://127.0.0.1:8765 2) tracking config 仍然必填 `offline_lidar_obstacle_perception` 在 `setup()` 里一定会初始化 tracking pipeline，所以即使 `--enable_tracking=false` 也需要给 `--lidar_tracking_config_file` 一个有效路径。 -3) 运行目录/配置管理器 +3) 依赖 HDMap/ROI 的 pipeline 需要显式提供 `--pose_dir` +如果离线运行时没有传 `--pose_dir`，`offline_lidar_obstacle_perception` 不会为 `LidarFrame` 填充 `lidar2world_pose`，它会保持默认单位阵。这样即使 detection pipeline 里启用了 `HDMAP_CONTEXT_PROVIDER` 和 `POINTCLOUD_ROI_FILTER`，也很容易在日志里看到 `No polygons in ROI`，因为查询 ROI 时使用的是默认原点姿态而不是真实车辆位姿。对包含地图上下文的 pipeline，建议总是提供与 `*.pcd` 同名的 `*.pose` 或 `*.pcd.pose` 文件。 + +4) 运行目录/配置管理器 离线工具内部会设置 `FLAGS_config_manager_path="./conf"`，因此需要你的运行环境有对应的 `conf/`（通常 Apollo 运行环境有）。如果你的环境没有，请先补齐或调整运行方式。 -4) 网页查看器不是原始全量点云直出 +5) 网页查看器不是原始全量点云直出 为了保证浏览器可交互，导出时会对整帧点云做采样；如果你希望看得更密，可以调大 `--web_vis_max_points`，但网页加载和渲染会更重。 ## 4. 从通道导出 PCD（给离线工具喂数据） diff --git a/modules/perception/onboard/transform_wrapper/transform_wrapper.cc b/modules/perception/onboard/transform_wrapper/transform_wrapper.cc index f293af0d..4b32a9ff 100644 --- a/modules/perception/onboard/transform_wrapper/transform_wrapper.cc +++ b/modules/perception/onboard/transform_wrapper/transform_wrapper.cc @@ -22,9 +22,7 @@ namespace apollo { namespace perception { namespace onboard { -namespace { - -} // namespace +namespace {} // namespace DEFINE_string(obs_sensor2vehicle_tf2_frame_id, "base_link", "parent frame id for sensor extrinsics"); @@ -44,9 +42,9 @@ namespace { apollo::transform::TimedTransformResolverOptions BuildTimedTransformResolverOptions() { apollo::transform::TimedTransformResolverOptions options; - options.tf2_buffer_size_sec = static_cast(FLAGS_obs_tf2_buff_size); + options.query_timeout_sec = static_cast(FLAGS_obs_tf2_buff_size); options.cache_duration_sec = FLAGS_obs_transform_cache_size; - options.max_extrapolation_latency_sec = + options.max_extrapolation_sec = FLAGS_obs_max_local_pose_extrapolation_latency; options.enable_extrapolation = FLAGS_obs_enable_local_pose_extrapolation; options.hardware_trigger = FLAGS_hardware_trigger; @@ -62,6 +60,8 @@ void TransformWrapper::Init( vehicle2world_tf2_frame_id_ = FLAGS_obs_vehicle2world_tf2_frame_id; vehicle2world_tf2_child_frame_id_ = FLAGS_obs_vehicle2world_tf2_child_frame_id; + timed_transform_resolver_.ConfigureFrames(vehicle2world_tf2_frame_id_, + vehicle2world_tf2_child_frame_id_); timed_transform_resolver_.SetOptions(BuildTimedTransformResolverOptions()); inited_ = true; } @@ -75,6 +75,8 @@ void TransformWrapper::Init( sensor2vehicle_tf2_child_frame_id_ = sensor2vehicle_tf2_child_frame_id; vehicle2world_tf2_frame_id_ = vehicle2world_tf2_frame_id; vehicle2world_tf2_child_frame_id_ = vehicle2world_tf2_child_frame_id; + timed_transform_resolver_.ConfigureFrames(vehicle2world_tf2_frame_id_, + vehicle2world_tf2_child_frame_id_); timed_transform_resolver_.SetOptions(BuildTimedTransformResolverOptions()); inited_ = true; } @@ -93,8 +95,7 @@ bool TransformWrapper::GetSensor2worldTrans( } apollo::transform::StampedTransform trans_vehicle2world; - if (!QueryTrans(timestamp, &trans_vehicle2world, - vehicle2world_tf2_frame_id_, + if (!QueryTrans(timestamp, &trans_vehicle2world, vehicle2world_tf2_frame_id_, vehicle2world_tf2_child_frame_id_)) { return false; } @@ -136,11 +137,16 @@ bool TransformWrapper::GetTrans(double timestamp, Eigen::Affine3d* trans, return true; } -bool TransformWrapper::QueryTrans( - double timestamp, apollo::transform::StampedTransform* trans, - const std::string& frame_id, const std::string& child_frame_id) { - return timed_transform_resolver_.Resolve(timestamp, frame_id, - child_frame_id, trans); +bool TransformWrapper::QueryTrans(double timestamp, + apollo::transform::StampedTransform* trans, + const std::string& frame_id, + const std::string& child_frame_id) { + if (frame_id == vehicle2world_tf2_frame_id_ && + child_frame_id == vehicle2world_tf2_child_frame_id_) { + return timed_transform_resolver_.Resolve(timestamp, trans); + } + return timed_transform_resolver_.Resolve(timestamp, frame_id, child_frame_id, + trans); } bool TransformWrapper::QueryStaticTrans(const std::string& frame_id, @@ -151,9 +157,8 @@ bool TransformWrapper::QueryStaticTrans(const std::string& frame_id, return false; } - if (!transform_query_.GetLatestStaticTransformToAffine(frame_id, - child_frame_id, - trans)) { + if (!transform_query_.GetLatestStaticTransformToAffine( + frame_id, child_frame_id, trans)) { AERROR << "Failed to query static transform from " << child_frame_id << " to " << frame_id; return false; @@ -167,8 +172,8 @@ bool TransformWrapper::EnsureSensor2VehicleExtrinsics() { return true; } - auto sensor2vehicle_extrinsics = std::make_unique( - Eigen::Affine3d::Identity()); + auto sensor2vehicle_extrinsics = + std::make_unique(Eigen::Affine3d::Identity()); if (!QueryStaticTrans(sensor2vehicle_tf2_frame_id_, sensor2vehicle_tf2_child_frame_id_, sensor2vehicle_extrinsics.get())) { diff --git a/modules/perception/production/data/perception/common/sensor_meta.pt b/modules/perception/production/data/perception/common/sensor_meta.pt index 6642f4f5..3f77ecfe 100644 --- a/modules/perception/production/data/perception/common/sensor_meta.pt +++ b/modules/perception/production/data/perception/common/sensor_meta.pt @@ -38,39 +38,3 @@ sensor_meta { type: MONOCULAR_CAMERA orientation: FRONT } - -sensor_meta { - name: "CAM_FRONT" - type: MONOCULAR_CAMERA - orientation: FRONT -} - -sensor_meta { - name: "CAM_FRONT_LEFT" - type: MONOCULAR_CAMERA - orientation: LEFT_FORWARD -} - -sensor_meta { - name: "CAM_FRONT_RIGHT" - type: MONOCULAR_CAMERA - orientation: RIGHT_FORWARD -} - -sensor_meta { - name: "CAM_BACK_LEFT" - type: MONOCULAR_CAMERA - orientation: LEFT_BACKWARD -} - -sensor_meta { - name: "CAM_BACK_RIGHT" - type: MONOCULAR_CAMERA - orientation: RIGHT_BACKWARD -} - -sensor_meta { - name: "CAM_BACK" - type: MONOCULAR_CAMERA - orientation: REAR -} diff --git a/modules/transform/BUILD b/modules/transform/BUILD index b666b514..c9a0371a 100644 --- a/modules/transform/BUILD +++ b/modules/transform/BUILD @@ -48,7 +48,10 @@ cc_library( cc_library( name = "timed_transform_resolver", - srcs = ["timed_transform_resolver.cc"], + srcs = [ + "pose_cache.cc", + "timed_transform_resolver.cc", + ], hdrs = ["timed_transform_resolver.h"], deps = [ "//cyber/common:global_data", @@ -86,11 +89,15 @@ cc_test( cc_test( name = "timed_transform_resolver_test", size = "small", - srcs = ["timed_transform_resolver_test.cc"], + srcs = [ + "pose_cache_test.cc", + "timed_transform_resolver_test.cc", + ], deps = [ ":buffer_interface", ":timed_transform_resolver", ":transform_query_core", + "//modules/common_msgs/transform_msgs:transform_cc_proto", "@com_google_googletest//:gtest_main", "@eigen", ], diff --git a/modules/transform/buffer_interface.h b/modules/transform/buffer_interface.h index b084799c..0da48143 100644 --- a/modules/transform/buffer_interface.h +++ b/modules/transform/buffer_interface.h @@ -18,11 +18,13 @@ #include +#include "tf2/buffer_core.h" #include "tf2/convert.h" -#include "cyber/time/time.h" #include "modules/common_msgs/transform_msgs/transform.pb.h" +#include "cyber/time/time.h" + namespace apollo { namespace transform { diff --git a/modules/transform/conf/velodyne128_base_link_extrinsics.yaml b/modules/transform/conf/velodyne128_base_link_extrinsics.yaml index 2a90fc66..47c88426 100644 --- a/modules/transform/conf/velodyne128_base_link_extrinsics.yaml +++ b/modules/transform/conf/velodyne128_base_link_extrinsics.yaml @@ -1,6 +1,3 @@ -# proj: +proj=utm +zone=50 +ellps=WGS84 -# scale:1.11177 -# (XXX) Manually adjusted header: stamp: secs: 1422601952 @@ -17,4 +14,4 @@ transform: y: 0.00327669 z: 0.7157 w: 0.698332 -child_frame_id: velodyne128 \ No newline at end of file +child_frame_id: velodyne128 diff --git a/modules/transform/pose_cache.cc b/modules/transform/pose_cache.cc new file mode 100644 index 00000000..4dea7d8c --- /dev/null +++ b/modules/transform/pose_cache.cc @@ -0,0 +1,284 @@ +#include "modules/transform/timed_transform_resolver.h" + +#include +#include +#include + +#include "cyber/common/log.h" + +namespace apollo { +namespace transform { + +namespace { + +constexpr double kTimestampEpsilonSec = 1e-9; + +} // namespace + +TimedTransformResolver::PoseSample TimedTransformResolver::FromAffine( + double timestamp_sec, const Eigen::Affine3d& pose) { + PoseSample sample; + sample.timestamp_sec = timestamp_sec; + sample.tx = pose.translation().x(); + sample.ty = pose.translation().y(); + sample.tz = pose.translation().z(); + Eigen::Quaterniond rotation(pose.linear()); + rotation.normalize(); + sample.qx = rotation.x(); + sample.qy = rotation.y(); + sample.qz = rotation.z(); + sample.qw = rotation.w(); + return sample; +} + +Eigen::Affine3d TimedTransformResolver::ToAffine(const PoseSample& sample) { + return Eigen::Translation3d(sample.tx, sample.ty, sample.tz) * + Eigen::Quaterniond(sample.qw, sample.qx, sample.qy, sample.qz); +} + +void TimedTransformResolver::ClearCache() { + std::lock_guard lock(mutex_); + samples_.clear(); +} + +bool TimedTransformResolver::InsertSample(double timestamp_sec, + const Eigen::Affine3d& pose) { + std::lock_guard lock(mutex_); + const PoseSample sample = FromAffine(timestamp_sec, pose); + const auto it = std::lower_bound( + samples_.begin(), samples_.end(), timestamp_sec, + [](const PoseSample& lhs, double rhs_timestamp_sec) { + return lhs.timestamp_sec + kTimestampEpsilonSec < rhs_timestamp_sec; + }); + if (it != samples_.end() && + std::fabs(it->timestamp_sec - timestamp_sec) <= kTimestampEpsilonSec) { + *it = sample; + } else { + samples_.insert(it, sample); + } + + while (!samples_.empty() && + (samples_.back().timestamp_sec - samples_.front().timestamp_sec) > + options_.cache_duration_sec) { + samples_.erase(samples_.begin()); + } + return !samples_.empty(); +} + +bool TimedTransformResolver::Prefetch(double timestamp_sec) { + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + double resolved_timestamp_sec = timestamp_sec; + if (!QueryTransform(timestamp_sec, &pose, &resolved_timestamp_sec)) { + return false; + } + return InsertSample(resolved_timestamp_sec, pose); +} + +bool TimedTransformResolver::PrefetchBatch( + const std::vector& timestamps_sec) { + std::vector ordered_timestamps = timestamps_sec; + std::sort(ordered_timestamps.begin(), ordered_timestamps.end()); + ordered_timestamps.erase( + std::unique(ordered_timestamps.begin(), ordered_timestamps.end(), + [](double lhs, double rhs) { + return std::fabs(lhs - rhs) <= kTimestampEpsilonSec; + }), + ordered_timestamps.end()); + for (const double timestamp_sec : ordered_timestamps) { + Eigen::Affine3d cached_pose = Eigen::Affine3d::Identity(); + if (QueryCachedStrict(timestamp_sec, &cached_pose)) { + continue; + } + if (!Prefetch(timestamp_sec)) { + return false; + } + } + return true; +} + +bool TimedTransformResolver::QueryCachedInternal( + double timestamp_sec, double max_extrapolation_sec, Eigen::Affine3d* pose, + TransformResolveStatus* status) const { + const auto interpolate_pose = [timestamp_sec](const PoseSample& left, + const PoseSample& right) + -> Eigen::Affine3d { + const double duration = right.timestamp_sec - left.timestamp_sec; + if (std::fabs(duration) <= kTimestampEpsilonSec) { + return ToAffine(right); + } + + const double ratio = (timestamp_sec - left.timestamp_sec) / duration; + Eigen::Quaterniond left_rotation(left.qw, left.qx, left.qy, left.qz); + Eigen::Quaterniond right_rotation(right.qw, right.qx, right.qy, right.qz); + if (left_rotation.dot(right_rotation) < 0.0) { + right_rotation.coeffs() *= -1.0; + } + + const Eigen::Vector3d translation(left.tx + ratio * (right.tx - left.tx), + left.ty + ratio * (right.ty - left.ty), + left.tz + ratio * (right.tz - left.tz)); + return Eigen::Affine3d(Eigen::Translation3d(translation) * + left_rotation.slerp(ratio, right_rotation)); + }; + + if (pose == nullptr) { + if (status != nullptr) { + *status = TransformResolveStatus::kQueryFailed; + } + return false; + } + + std::lock_guard lock(mutex_); + if (samples_.empty()) { + if (status != nullptr) { + *status = TransformResolveStatus::kEmpty; + } + return false; + } + + if (timestamp_sec + kTimestampEpsilonSec < samples_.front().timestamp_sec) { + if (status != nullptr) { + *status = TransformResolveStatus::kTooEarly; + } + return false; + } + + if (timestamp_sec > samples_.back().timestamp_sec + kTimestampEpsilonSec) { + if ((timestamp_sec - samples_.back().timestamp_sec) > + max_extrapolation_sec) { + if (status != nullptr) { + *status = TransformResolveStatus::kTooOld; + } + return false; + } + if (samples_.size() == 1) { + *pose = ToAffine(samples_.back()); + } else { + *pose = interpolate_pose(samples_[samples_.size() - 2], samples_.back()); + } + if (status != nullptr) { + *status = TransformResolveStatus::kOk; + } + return true; + } + + const auto it = std::lower_bound( + samples_.begin(), samples_.end(), timestamp_sec, + [](const PoseSample& lhs, double rhs_timestamp_sec) { + return lhs.timestamp_sec + kTimestampEpsilonSec < rhs_timestamp_sec; + }); + if (it == samples_.begin()) { + *pose = ToAffine(*it); + if (status != nullptr) { + *status = TransformResolveStatus::kOk; + } + return true; + } + if (it == samples_.end()) { + *pose = ToAffine(samples_.back()); + if (status != nullptr) { + *status = TransformResolveStatus::kOk; + } + return true; + } + if (std::fabs(it->timestamp_sec - timestamp_sec) <= kTimestampEpsilonSec) { + *pose = ToAffine(*it); + if (status != nullptr) { + *status = TransformResolveStatus::kOk; + } + return true; + } + + const auto left = std::prev(it); + const double left_time = left->timestamp_sec; + const double right_time = it->timestamp_sec; + if (std::fabs(right_time - left_time) <= kTimestampEpsilonSec) { + *pose = ToAffine(*it); + if (status != nullptr) { + *status = TransformResolveStatus::kOk; + } + return true; + } + + *pose = interpolate_pose(*left, *it); + if (status != nullptr) { + *status = TransformResolveStatus::kOk; + } + return true; +} + +bool TimedTransformResolver::QueryCached( + double timestamp_sec, Eigen::Affine3d* pose, + TransformResolveStatus* status) const { + return QueryCachedInternal(timestamp_sec, options_.max_extrapolation_sec, + pose, status); +} + +bool TimedTransformResolver::QueryCachedStrict( + double timestamp_sec, Eigen::Affine3d* pose, + TransformResolveStatus* status) const { + return QueryCachedInternal(timestamp_sec, 0.0, pose, status); +} + +bool TimedTransformResolver::QueryCachedBatch( + const std::vector& timestamps_sec, + std::vector* poses, + TransformResolveStatus* status) const { + if (poses == nullptr) { + if (status != nullptr) { + *status = TransformResolveStatus::kQueryFailed; + } + return false; + } + poses->clear(); + poses->reserve(timestamps_sec.size()); + for (const double timestamp_sec : timestamps_sec) { + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + TransformResolveStatus local_status = TransformResolveStatus::kOk; + if (!QueryCached(timestamp_sec, &pose, &local_status)) { + if (status != nullptr) { + *status = local_status; + } + poses->clear(); + return false; + } + poses->push_back(pose); + } + if (status != nullptr) { + *status = TransformResolveStatus::kOk; + } + return true; +} + +bool TimedTransformResolver::QueryCachedBatchStrict( + const std::vector& timestamps_sec, + std::vector* poses, + TransformResolveStatus* status) const { + if (poses == nullptr) { + if (status != nullptr) { + *status = TransformResolveStatus::kQueryFailed; + } + return false; + } + poses->clear(); + poses->reserve(timestamps_sec.size()); + for (const double timestamp_sec : timestamps_sec) { + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + TransformResolveStatus local_status = TransformResolveStatus::kOk; + if (!QueryCachedStrict(timestamp_sec, &pose, &local_status)) { + if (status != nullptr) { + *status = local_status; + } + poses->clear(); + return false; + } + poses->push_back(pose); + } + if (status != nullptr) { + *status = TransformResolveStatus::kOk; + } + return true; +} + +} // namespace transform +} // namespace apollo diff --git a/modules/transform/pose_cache_test.cc b/modules/transform/pose_cache_test.cc new file mode 100644 index 00000000..e02050d2 --- /dev/null +++ b/modules/transform/pose_cache_test.cc @@ -0,0 +1,200 @@ +#include +#include +#include +#include + +#include + +#include "modules/transform/buffer_interface.h" +#include "modules/transform/timed_transform_resolver.h" + +namespace apollo { +namespace transform { + +namespace { + +class MockBuffer : public BufferInterface { + public: + using Key = std::tuple; + + void AddTransform(const std::string& target_frame, + const std::string& source_frame, double timestamp_sec, + const Eigen::Affine3d& transform) { + const auto key = Key(target_frame, source_frame, + cyber::Time(timestamp_sec).ToNanosecond()); + TransformStamped stamped; + stamped.mutable_header()->set_frame_id(target_frame); + stamped.mutable_header()->set_timestamp_sec(timestamp_sec); + stamped.set_child_frame_id(source_frame); + stamped.mutable_transform()->mutable_translation()->set_x( + transform.translation().x()); + stamped.mutable_transform()->mutable_translation()->set_y( + transform.translation().y()); + stamped.mutable_transform()->mutable_translation()->set_z( + transform.translation().z()); + const Eigen::Quaterniond rotation(transform.linear()); + stamped.mutable_transform()->mutable_rotation()->set_qw(rotation.w()); + stamped.mutable_transform()->mutable_rotation()->set_qx(rotation.x()); + stamped.mutable_transform()->mutable_rotation()->set_qy(rotation.y()); + stamped.mutable_transform()->mutable_rotation()->set_qz(rotation.z()); + transforms_[key] = stamped; + } + + TransformStamped lookupTransform(const std::string& target_frame, + const std::string& source_frame, + const cyber::Time& time, + const float timeout_second) const override { + (void)timeout_second; + const auto key = Key(target_frame, source_frame, time.ToNanosecond()); + const auto it = transforms_.find(key); + if (it == transforms_.end()) { + throw std::runtime_error("missing transform"); + } + return it->second; + } + + TransformStamped lookupTransform(const std::string& target_frame, + const cyber::Time& target_time, + const std::string& source_frame, + const cyber::Time& source_time, + const std::string& fixed_frame, + const float timeout_second) const override { + (void)target_time; + (void)fixed_frame; + return lookupTransform(target_frame, source_frame, source_time, + timeout_second); + } + + bool canTransform(const std::string& target_frame, + const std::string& source_frame, const cyber::Time& time, + const float timeout_second, + std::string* errstr) const override { + (void)timeout_second; + const auto key = Key(target_frame, source_frame, time.ToNanosecond()); + const bool found = transforms_.count(key) > 0; + if (!found && errstr != nullptr) { + *errstr = "mock transform missing"; + } + return found; + } + + bool canTransform(const std::string& target_frame, + const cyber::Time& target_time, + const std::string& source_frame, + const cyber::Time& source_time, + const std::string& fixed_frame, const float timeout_second, + std::string* errstr) const override { + (void)target_time; + (void)fixed_frame; + return canTransform(target_frame, source_frame, source_time, timeout_second, + errstr); + } + + bool GetLatestStaticTransform(const std::string& target_frame, + const std::string& source_frame, + TransformStamped* transform) const override { + if (transform == nullptr) { + return false; + } + const auto it = + transforms_.lower_bound(Key(target_frame, source_frame, 0U)); + if (it == transforms_.end() || std::get<0>(it->first) != target_frame || + std::get<1>(it->first) != source_frame) { + return false; + } + *transform = it->second; + return true; + } + + private: + std::map transforms_; +}; + +} // namespace + +TEST(TimedTransformResolverCacheTest, + PrefetchesAndServesCachedInterpolatedPoses) { + MockBuffer buffer; + buffer.AddTransform( + "world", "lidar", 10.0, + Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + buffer.AddTransform( + "world", "lidar", 12.0, + Eigen::Translation3d(2.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + + TimedTransformResolver resolver( + &buffer, "world", "lidar", + TimedTransformResolverOptions{0.01f, 2.0, 0.2, 0.015, true, true}); + ASSERT_TRUE(resolver.PrefetchBatch({12.0, 10.0})); + + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + TransformResolveStatus status = TransformResolveStatus::kEmpty; + ASSERT_TRUE(resolver.QueryCached(11.0, &pose, &status)); + EXPECT_EQ(status, TransformResolveStatus::kOk); + EXPECT_DOUBLE_EQ(pose.translation().x(), 1.0); +} + +TEST(TimedTransformResolverCacheTest, + StoresOutOfOrderPrefetchesInSortedWindow) { + MockBuffer buffer; + buffer.AddTransform( + "world", "lidar", 10.0, + Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + buffer.AddTransform( + "world", "lidar", 11.0, + Eigen::Translation3d(1.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + + TimedTransformResolver resolver( + &buffer, "world", "lidar", + TimedTransformResolverOptions{0.01f, 2.0, 0.2, 0.015, true, true}); + ASSERT_TRUE(resolver.Prefetch(11.0)); + ASSERT_TRUE(resolver.Prefetch(10.0)); + + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + ASSERT_TRUE(resolver.QueryCached(10.5, &pose)); + EXPECT_DOUBLE_EQ(pose.translation().x(), 0.5); +} + +TEST(TimedTransformResolverCacheTest, + PrefetchBatchDoesNotTreatForwardExtrapolationAsCacheHit) { + MockBuffer buffer; + buffer.AddTransform( + "world", "lidar", 10.0, + Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + buffer.AddTransform( + "world", "lidar", 10.1, + Eigen::Translation3d(1.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + + TimedTransformResolver resolver( + &buffer, "world", "lidar", + TimedTransformResolverOptions{0.01f, 2.0, 0.2, 0.015, true, true}); + ASSERT_TRUE(resolver.PrefetchBatch({10.0, 10.1})); + + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + ASSERT_TRUE(resolver.QueryCachedStrict(10.1, &pose)); + EXPECT_DOUBLE_EQ(pose.translation().x(), 1.0); +} + +TEST(TimedTransformResolverCacheTest, + StrictQueryRejectsForwardExtrapolationOnlyCache) { + MockBuffer buffer; + buffer.AddTransform( + "world", "lidar", 10.0, + Eigen::Translation3d(1.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + + TimedTransformResolver resolver( + &buffer, "world", "lidar", + TimedTransformResolverOptions{0.01f, 2.0, 0.2, 0.015, true, true}); + ASSERT_TRUE(resolver.Prefetch(10.0)); + + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + TransformResolveStatus status = TransformResolveStatus::kEmpty; + EXPECT_FALSE(resolver.QueryCachedStrict(10.1, &pose, &status)); + EXPECT_EQ(status, TransformResolveStatus::kTooOld); + ASSERT_TRUE(resolver.QueryCached(10.1, &pose, &status)); + EXPECT_EQ(status, TransformResolveStatus::kOk); + EXPECT_DOUBLE_EQ(pose.translation().x(), 1.0); +} + +} // namespace transform +} // namespace apollo diff --git a/modules/transform/timed_transform_resolver.cc b/modules/transform/timed_transform_resolver.cc index ca8ef3a5..69a82d6f 100644 --- a/modules/transform/timed_transform_resolver.cc +++ b/modules/transform/timed_transform_resolver.cc @@ -17,9 +17,9 @@ #include "modules/transform/timed_transform_resolver.h" -#include #include -#include +#include +#include #include "cyber/common/global_data.h" #include "cyber/common/log.h" @@ -28,224 +28,288 @@ namespace apollo { namespace transform { -Eigen::Quaterniond Slerp(const Eigen::Quaterniond& source, const double& t, - const Eigen::Quaterniond& other); - namespace { -std::string BuildTransformCacheKey(const std::string& frame_id, - const std::string& child_frame_id) { - return frame_id + "\n" + child_frame_id; -} - -void InterpolateStampedTransform(const StampedTransform& first, - const StampedTransform& second, - double timestamp, - StampedTransform* transform) { - CHECK_NOTNULL(transform); +constexpr int kTransformMissLogFrequency = 10; +constexpr double kTimeCompareEpsilonSec = 1e-6; - const double duration = second.timestamp - first.timestamp; - if (duration <= std::numeric_limits::epsilon()) { - *transform = second; - transform->timestamp = timestamp; - return; +std::string FormatTimestampForLog(double timestamp_sec) { + std::ostringstream stream; + if (!std::isfinite(timestamp_sec) || timestamp_sec < 0.0) { + stream << timestamp_sec << "s"; + return stream.str(); } - const double ratio = (timestamp - first.timestamp) / duration; - transform->timestamp = timestamp; - transform->rotation = Slerp(first.rotation, ratio, second.rotation); - transform->translation.x() = - first.translation.x() * (1 - ratio) + second.translation.x() * ratio; - transform->translation.y() = - first.translation.y() * (1 - ratio) + second.translation.y() * ratio; - transform->translation.z() = - first.translation.z() * (1 - ratio) + second.translation.z() * ratio; + stream << std::fixed << std::setprecision(9) << timestamp_sec << "s"; + if (timestamp_sec > 0.0) { + stream << " [" << cyber::Time(timestamp_sec).ToString() << "]"; + } + return stream.str(); } -} // namespace - -void TransformCache::AddTransform(const StampedTransform& transform) { - if (transforms_.empty()) { - transforms_.push_back(transform); - return; - } - double delt = transform.timestamp - transforms_.back().timestamp; - if (delt < 0.0) { - ADEBUG << "Ignore out-of-order transform at " << transform.timestamp - << " for transform cache."; - return; +std::string FormatDeltaForLog(double delta_sec) { + const double abs_delta_sec = std::fabs(delta_sec); + std::ostringstream stream; + stream << std::showpos << std::fixed + << std::setprecision(abs_delta_sec >= 1.0 ? 3 : 6) << delta_sec + << "s" << std::noshowpos; + if (abs_delta_sec >= 86400.0) { + stream << " (~" << std::fixed << std::setprecision(3) + << abs_delta_sec / 86400.0 << " days)"; + } else if (abs_delta_sec >= 3600.0) { + stream << " (~" << std::fixed << std::setprecision(3) + << abs_delta_sec / 3600.0 << " h)"; + } else if (abs_delta_sec >= 60.0) { + stream << " (~" << std::fixed << std::setprecision(3) + << abs_delta_sec / 60.0 << " min)"; + } else if (abs_delta_sec > 0.0 && abs_delta_sec < 1.0) { + stream << " (~" << std::fixed << std::setprecision(3) + << abs_delta_sec * 1000.0 << " ms)"; } + return stream.str(); +} - if (delt <= std::numeric_limits::epsilon()) { - transforms_.back() = transform; - return; +const char* DescribeTimeRelation(double delta_sec) { + if (std::fabs(delta_sec) <= kTimeCompareEpsilonSec) { + return "aligned"; } + return delta_sec > 0.0 ? "ahead" : "behind"; +} - do { - delt = transform.timestamp - transforms_.front().timestamp; - if (delt < cache_duration_) { - break; +bool TryExtractBoundaryTimeSec(const std::string& reason, + double* boundary_time_sec, + std::string* boundary_label) { + RETURN_VAL_IF_NULL2(boundary_time_sec, false); + RETURN_VAL_IF_NULL2(boundary_label, false); + + struct Marker { + const char* needle; + const char* label; + }; + constexpr Marker kMarkers[] = { + {"latest data is at time ", "latest_tf"}, + {"earliest data is at time ", "earliest_tf"}, + }; + + for (const auto& marker : kMarkers) { + const size_t marker_pos = reason.find(marker.needle); + if (marker_pos == std::string::npos) { + continue; + } + const size_t digits_begin = marker_pos + std::strlen(marker.needle); + size_t digits_end = digits_begin; + while (digits_end < reason.size() && std::isdigit(reason[digits_end])) { + ++digits_end; } - transforms_.pop_front(); - } while (!transforms_.empty()); + if (digits_end == digits_begin) { + continue; + } + try { + const uint64_t nanoseconds = + std::stoull(reason.substr(digits_begin, digits_end - digits_begin)); + *boundary_time_sec = static_cast(nanoseconds) / 1e9; + *boundary_label = marker.label; + return true; + } catch (const std::exception&) { + return false; + } + } - transforms_.push_back(transform); + return false; } -Eigen::Quaterniond Slerp(const Eigen::Quaterniond& source, const double& t, - const Eigen::Quaterniond& other) { - const double one = 1.0 - std::numeric_limits::epsilon(); - double d = source.x() * other.x() + source.y() * other.y() + - source.z() * other.z() + source.w() * other.w(); - double abs_d = std::abs(d); - - double scale0; - double scale1; - - if (abs_d >= one) { - scale0 = 1.0 - t; - scale1 = t; +void LogTransformQueryFailure(const std::string& target_frame, + const std::string& source_frame, + double requested_time_sec, + const cyber::Time& lookup_time, + float timeout_sec, + const std::string& reason) { + const double now_sec = cyber::Time::Now().ToSecond(); + const double request_vs_now_sec = requested_time_sec - now_sec; + + std::ostringstream message; + message << "Transform unavailable target=" << target_frame + << " source=" << source_frame + << ", requested=" << FormatTimestampForLog(requested_time_sec); + if (lookup_time.IsZero() && requested_time_sec > 0.0) { + message << ", lookup=latest(0)"; } else { - double theta = std::acos(abs_d); - double sin_theta = std::sin(theta); - - scale0 = std::sin((1.0 - t) * theta) / sin_theta; - scale1 = std::sin((t * theta)) / sin_theta; - } - if (d < 0) { - scale1 = -scale1; + message << ", lookup=" << FormatTimestampForLog(lookup_time.ToSecond()); } - return Eigen::Quaterniond(scale0 * source.w() + scale1 * other.w(), - scale0 * source.x() + scale1 * other.x(), - scale0 * source.y() + scale1 * other.y(), - scale0 * source.z() + scale1 * other.z()); -} - -bool TransformCache::QueryTransform(double timestamp, - StampedTransform* transform, - double max_duration) { - if (transforms_.empty() || transform == nullptr) { - return false; + double boundary_time_sec = 0.0; + std::string boundary_label; + if (TryExtractBoundaryTimeSec(reason, &boundary_time_sec, &boundary_label)) { + const double request_vs_boundary_sec = + requested_time_sec - boundary_time_sec; + message << ", " << boundary_label << "=" + << FormatTimestampForLog(boundary_time_sec) + << ", requested_vs_" << boundary_label << "=" + << FormatDeltaForLog(request_vs_boundary_sec) << " (" + << DescribeTimeRelation(request_vs_boundary_sec) << ")"; } - if (timestamp < transforms_.front().timestamp) { - ADEBUG << "Transform cache miss: requested timestamp " << timestamp - << " is earlier than cached range."; - return false; - } + message << ", requested_vs_now=" << FormatDeltaForLog(request_vs_now_sec) + << " (" << DescribeTimeRelation(request_vs_now_sec) << ")" + << ", timeout=" << std::fixed << std::setprecision(3) + << timeout_sec << "s" + << ", reason=" << reason; + AINFO_EVERY(kTransformMissLogFrequency) << message.str(); +} - const double delt = timestamp - transforms_.back().timestamp; - if (delt > max_duration) { - ADEBUG << "Transform cache miss: requested timestamp is " << delt - << "s newer than the latest cached sample."; - return false; - } +} // namespace - const int size = static_cast(transforms_.size()); - if (size == 1) { - *transform = transforms_.back(); - transform->timestamp = timestamp; - ADEBUG << "Reuse latest cached transform at " - << transforms_.back().timestamp << " for " << timestamp << "."; - return true; - } +TimedTransformResolver::TimedTransformResolver( + BufferInterface* buffer, const std::string& target_frame, + const std::string& source_frame, + const TimedTransformResolverOptions& options) { + Configure(buffer, target_frame, source_frame, options); +} - if (timestamp >= transforms_.back().timestamp) { - InterpolateStampedTransform(transforms_[size - 2], transforms_[size - 1], - timestamp, transform); - ADEBUG << "Extrapolate cached transform at " << timestamp - << " using samples at " << transforms_[size - 2].timestamp - << " and " << transforms_[size - 1].timestamp << "."; - return true; - } +void TimedTransformResolver::SetTransformQuery(TransformQuery* query) { + owned_query_.reset(); + transform_query_ = query; + ClearCache(); +} - for (int index = 1; index < size; ++index) { - const auto& previous = transforms_[index - 1]; - const auto& current = transforms_[index]; - if (timestamp <= current.timestamp) { - if (std::abs(timestamp - current.timestamp) <= - std::numeric_limits::epsilon()) { - *transform = current; - transform->timestamp = timestamp; - } else { - InterpolateStampedTransform(previous, current, timestamp, transform); - } - return true; - } - } +void TimedTransformResolver::Configure( + BufferInterface* buffer, const std::string& target_frame, + const std::string& source_frame, + const TimedTransformResolverOptions& options) { + owned_query_ = std::make_unique(buffer); + transform_query_ = owned_query_.get(); + ConfigureFrames(target_frame, source_frame); + SetOptions(options); +} - return false; +void TimedTransformResolver::ConfigureFrames(const std::string& target_frame, + const std::string& source_frame) { + target_frame_ = target_frame; + source_frame_ = source_frame; + ClearCache(); } void TimedTransformResolver::SetOptions( const TimedTransformResolverOptions& options) { options_ = options; - options_.tf2_buffer_size_sec = std::max(0.0f, options_.tf2_buffer_size_sec); + options_.query_timeout_sec = std::max(0.0f, options_.query_timeout_sec); options_.cache_duration_sec = std::max(0.0, options_.cache_duration_sec); - options_.max_extrapolation_latency_sec = - std::max(0.0, options_.max_extrapolation_latency_sec); + options_.max_extrapolation_sec = + std::max(0.0, options_.max_extrapolation_sec); options_.latest_lookup_fallback_tolerance_sec = std::max(0.0, options_.latest_lookup_fallback_tolerance_sec); - for (auto& entry : transform_caches_) { - entry.second.SetCacheDuration(options_.cache_duration_sec); - } } -TransformCache* TimedTransformResolver::GetTransformCache( - const std::string& frame_id, const std::string& child_frame_id) { - const std::string cache_key = - BuildTransformCacheKey(frame_id, child_frame_id); - auto [iter, inserted] = transform_caches_.try_emplace(cache_key); - if (inserted) { - iter->second.SetCacheDuration(options_.cache_duration_sec); +bool TimedTransformResolver::Resolve(double timestamp_sec, + StampedTransform* transform, + TransformResolveStatus* status) { + CHECK_NOTNULL(transform); + + TransformStamped stamped_transform; + if (QueryTransform(timestamp_sec, &stamped_transform)) { + const double resolved_timestamp = + stamped_transform.header().timestamp_sec() > 0.0 + ? stamped_transform.header().timestamp_sec() + : timestamp_sec; + const Eigen::Affine3d pose = ToAffine(stamped_transform); + if (options_.enable_extrapolation) { + InsertSample(resolved_timestamp, pose); + } + ToStamped(resolved_timestamp, pose, transform); + if (status != nullptr) { + *status = TransformResolveStatus::kOk; + } + return true; } - return &iter->second; + + if (!options_.enable_extrapolation) { + if (status != nullptr) { + *status = TransformResolveStatus::kQueryFailed; + } + return false; + } + + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + if (!QueryCachedInternal(timestamp_sec, options_.max_extrapolation_sec, &pose, + status)) { + return false; + } + ToStamped(timestamp_sec, pose, transform); + return true; } -bool TimedTransformResolver::Resolve(double timestamp, - const std::string& frame_id, - const std::string& child_frame_id, - StampedTransform* transform) { - CHECK_NOTNULL(transform_query_); +bool TimedTransformResolver::Resolve(double timestamp_sec, + const std::string& target_frame, + const std::string& source_frame, + StampedTransform* transform, + TransformResolveStatus* status) { CHECK_NOTNULL(transform); + if (transform_query_ == nullptr) { + if (status != nullptr) { + *status = TransformResolveStatus::kQueryFailed; + } + return false; + } - if (QueryTransform(timestamp, frame_id, child_frame_id, transform)) { + TimedTransformResolver scoped_resolver(transform_query_); + scoped_resolver.ConfigureFrames(target_frame, source_frame); + scoped_resolver.SetOptions(options_); + return scoped_resolver.Resolve(timestamp_sec, transform, status); +} + +bool TimedTransformResolver::ResolveToAffine(double timestamp_sec, + Eigen::Affine3d* pose, + TransformResolveStatus* status) { + CHECK_NOTNULL(pose); + + double resolved_timestamp_sec = timestamp_sec; + if (QueryTransform(timestamp_sec, pose, &resolved_timestamp_sec)) { if (options_.enable_extrapolation) { - GetTransformCache(frame_id, child_frame_id)->AddTransform(*transform); + InsertSample(resolved_timestamp_sec, *pose); + } + if (status != nullptr) { + *status = TransformResolveStatus::kOk; } return true; } if (!options_.enable_extrapolation) { + if (status != nullptr) { + *status = TransformResolveStatus::kQueryFailed; + } return false; } - return GetTransformCache(frame_id, child_frame_id) - ->QueryTransform(timestamp, transform, - options_.max_extrapolation_latency_sec); + return QueryCachedInternal(timestamp_sec, options_.max_extrapolation_sec, + pose, status); } -bool TimedTransformResolver::QueryTransform(double timestamp, - const std::string& frame_id, - const std::string& child_frame_id, - StampedTransform* transform) { +bool TimedTransformResolver::QueryTransform(double timestamp_sec, + TransformStamped* transform) const { CHECK_NOTNULL(transform_query_); CHECK_NOTNULL(transform); - cyber::Time query_time(timestamp); + if (target_frame_.empty() || source_frame_.empty()) { + return false; + } + + cyber::Time query_time(timestamp_sec); std::string err_string; - if (!transform_query_->CanTransform(frame_id, child_frame_id, query_time, - options_.tf2_buffer_size_sec, + if (!transform_query_->CanTransform(target_frame_, source_frame_, query_time, + options_.query_timeout_sec, &err_string)) { - apollo::transform::TransformStamped latest_transform; double latest_buffer_time = 0.0; if (!options_.hardware_trigger) { std::string lookup_error; + apollo::transform::TransformStamped latest_transform; if (!transform_query_->LookupTransform( - frame_id, child_frame_id, apollo::cyber::Time(0), - &latest_transform, options_.tf2_buffer_size_sec, &lookup_error)) { - AERROR << lookup_error; + target_frame_, source_frame_, apollo::cyber::Time(0), + &latest_transform, options_.query_timeout_sec, &lookup_error)) { + LogTransformQueryFailure( + target_frame_, source_frame_, timestamp_sec, apollo::cyber::Time(0), + options_.query_timeout_sec, + "failed to fetch latest transform while diagnosing miss: " + + lookup_error + "; canTransform=" + err_string); return false; } latest_buffer_time = latest_transform.header().timestamp_sec(); @@ -253,40 +317,69 @@ bool TimedTransformResolver::QueryTransform(double timestamp, if (!cyber::common::GlobalData::Instance()->IsRealityMode()) { query_time = cyber::Time(0); } else if (!options_.hardware_trigger && - (timestamp - latest_buffer_time < + (timestamp_sec - latest_buffer_time < options_.latest_lookup_fallback_tolerance_sec) && - (timestamp - latest_buffer_time > 0)) { + (timestamp_sec - latest_buffer_time > 0)) { query_time = apollo::cyber::Time(0); } else { - AERROR << "Can not find transform. " << FORMAT_TIMESTAMP(timestamp) - << " frame_id: " << frame_id - << " child_frame_id: " << child_frame_id - << " Error info: " << err_string; + LogTransformQueryFailure(target_frame_, source_frame_, timestamp_sec, + query_time, options_.query_timeout_sec, + err_string); return false; } } - apollo::transform::TransformStamped stamped_transform; std::string lookup_error; if (!transform_query_->LookupTransform( - frame_id, child_frame_id, query_time, &stamped_transform, - options_.tf2_buffer_size_sec, &lookup_error)) { - AERROR << lookup_error; + target_frame_, source_frame_, query_time, transform, + options_.query_timeout_sec, &lookup_error)) { + LogTransformQueryFailure(target_frame_, source_frame_, timestamp_sec, + query_time, options_.query_timeout_sec, + lookup_error); return false; } - transform->timestamp = stamped_transform.header().timestamp_sec(); - transform->translation = - Eigen::Translation3d(stamped_transform.transform().translation().x(), - stamped_transform.transform().translation().y(), - stamped_transform.transform().translation().z()); - transform->rotation = - Eigen::Quaterniond(stamped_transform.transform().rotation().qw(), - stamped_transform.transform().rotation().qx(), - stamped_transform.transform().rotation().qy(), - stamped_transform.transform().rotation().qz()); return true; } +bool TimedTransformResolver::QueryTransform( + double timestamp_sec, Eigen::Affine3d* pose, + double* resolved_timestamp_sec) const { + CHECK_NOTNULL(pose); + + TransformStamped stamped_transform; + if (!QueryTransform(timestamp_sec, &stamped_transform)) { + return false; + } + + *pose = ToAffine(stamped_transform); + if (resolved_timestamp_sec != nullptr) { + *resolved_timestamp_sec = stamped_transform.header().timestamp_sec() > 0.0 + ? stamped_transform.header().timestamp_sec() + : timestamp_sec; + } + return true; +} + +Eigen::Affine3d TimedTransformResolver::ToAffine( + const TransformStamped& transform) { + const auto& translation = transform.transform().translation(); + const auto& rotation = transform.transform().rotation(); + return Eigen::Translation3d(translation.x(), translation.y(), + translation.z()) * + Eigen::Quaterniond(rotation.qw(), rotation.qx(), rotation.qy(), + rotation.qz()); +} + +void TimedTransformResolver::ToStamped(double timestamp_sec, + const Eigen::Affine3d& pose, + StampedTransform* transform) { + CHECK_NOTNULL(transform); + transform->timestamp = timestamp_sec; + transform->translation = Eigen::Translation3d( + pose.translation().x(), pose.translation().y(), pose.translation().z()); + transform->rotation = Eigen::Quaterniond(pose.linear()); +} + } // namespace transform } // namespace apollo diff --git a/modules/transform/timed_transform_resolver.h b/modules/transform/timed_transform_resolver.h index 6e0c50c0..1f66f9bd 100644 --- a/modules/transform/timed_transform_resolver.h +++ b/modules/transform/timed_transform_resolver.h @@ -17,9 +17,10 @@ #pragma once -#include +#include +#include #include -#include +#include #include "Eigen/Geometry" @@ -36,55 +37,107 @@ struct StampedTransform { EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; +enum class TransformResolveStatus { + kOk = 0, + kEmpty = 1, + kTooOld = 2, + kTooEarly = 3, + kQueryFailed = 4, +}; + struct TimedTransformResolverOptions { - float tf2_buffer_size_sec = 0.01f; + float query_timeout_sec = 0.01f; double cache_duration_sec = 1.0; - double max_extrapolation_latency_sec = 0.15; + double max_extrapolation_sec = 0.15; double latest_lookup_fallback_tolerance_sec = 0.015; bool enable_extrapolation = true; bool hardware_trigger = true; }; -class TransformCache { - public: - TransformCache() = default; - ~TransformCache() = default; - - void AddTransform(const StampedTransform& transform); - bool QueryTransform(double timestamp, StampedTransform* transform, - double max_duration = 0.0); - - void SetCacheDuration(double duration) { cache_duration_ = duration; } - - private: - std::deque transforms_; - double cache_duration_ = 1.0; -}; +class BufferInterface; class TimedTransformResolver { public: TimedTransformResolver() = default; explicit TimedTransformResolver(TransformQuery* query) : transform_query_(query) {} + TimedTransformResolver(BufferInterface* buffer, + const std::string& target_frame, + const std::string& source_frame, + const TimedTransformResolverOptions& options = {}); ~TimedTransformResolver() = default; - void SetTransformQuery(TransformQuery* query) { transform_query_ = query; } + void SetTransformQuery(TransformQuery* query); + void Configure(BufferInterface* buffer, const std::string& target_frame, + const std::string& source_frame, + const TimedTransformResolverOptions& options); + void ConfigureFrames(const std::string& target_frame, + const std::string& source_frame); void SetOptions(const TimedTransformResolverOptions& options); - bool Resolve(double timestamp, const std::string& frame_id, - const std::string& child_frame_id, StampedTransform* transform); + bool Prefetch(double timestamp_sec); - private: - TransformCache* GetTransformCache(const std::string& frame_id, - const std::string& child_frame_id); + bool PrefetchBatch(const std::vector& timestamps_sec); + + bool QueryCached(double timestamp_sec, Eigen::Affine3d* pose, + TransformResolveStatus* status = nullptr) const; + + bool QueryCachedStrict(double timestamp_sec, Eigen::Affine3d* pose, + TransformResolveStatus* status = nullptr) const; + + bool QueryCachedBatch(const std::vector& timestamps_sec, + std::vector* poses, + TransformResolveStatus* status = nullptr) const; - bool QueryTransform(double timestamp, const std::string& frame_id, - const std::string& child_frame_id, - StampedTransform* transform); + bool QueryCachedBatchStrict(const std::vector& timestamps_sec, + std::vector* poses, + TransformResolveStatus* status = nullptr) const; + bool Resolve(double timestamp_sec, StampedTransform* transform, + TransformResolveStatus* status = nullptr); + + bool Resolve(double timestamp_sec, const std::string& target_frame, + const std::string& source_frame, StampedTransform* transform, + TransformResolveStatus* status = nullptr); + + bool ResolveToAffine(double timestamp_sec, Eigen::Affine3d* pose, + TransformResolveStatus* status = nullptr); + + private: + struct PoseSample { + double timestamp_sec = 0.0; + double tx = 0.0; + double ty = 0.0; + double tz = 0.0; + double qx = 0.0; + double qy = 0.0; + double qz = 0.0; + double qw = 1.0; + }; + + static PoseSample FromAffine(double timestamp_sec, + const Eigen::Affine3d& pose); + static Eigen::Affine3d ToAffine(const PoseSample& sample); + static Eigen::Affine3d ToAffine(const TransformStamped& transform); + static void ToStamped(double timestamp_sec, const Eigen::Affine3d& pose, + StampedTransform* transform); + + bool InsertSample(double timestamp_sec, const Eigen::Affine3d& pose); + bool QueryCachedInternal(double timestamp_sec, double max_extrapolation_sec, + Eigen::Affine3d* pose, + TransformResolveStatus* status) const; + bool QueryTransform(double timestamp_sec, TransformStamped* transform) const; + bool QueryTransform(double timestamp_sec, Eigen::Affine3d* pose, + double* resolved_timestamp_sec = nullptr) const; + void ClearCache(); + + std::unique_ptr owned_query_; TransformQuery* transform_query_ = nullptr; + std::string target_frame_; + std::string source_frame_; TimedTransformResolverOptions options_; - std::unordered_map transform_caches_; + mutable std::mutex mutex_; + std::vector samples_; }; } // namespace transform diff --git a/modules/transform/timed_transform_resolver_test.cc b/modules/transform/timed_transform_resolver_test.cc index aed99cd4..02f0fe81 100644 --- a/modules/transform/timed_transform_resolver_test.cc +++ b/modules/transform/timed_transform_resolver_test.cc @@ -16,6 +16,9 @@ #include "modules/transform/timed_transform_resolver.h" +#include +#include + #include "gtest/gtest.h" #include "modules/transform/buffer_interface.h" @@ -24,15 +27,6 @@ namespace apollo { namespace transform { namespace { -StampedTransform MakeStampedTransform(double timestamp_sec, double x, double y, - double z) { - StampedTransform transform; - transform.timestamp = timestamp_sec; - transform.translation = Eigen::Translation3d(x, y, z); - transform.rotation = Eigen::Quaterniond::Identity(); - return transform; -} - TransformStamped MakeTransformMessage(double timestamp_sec, double x, double y, double z) { TransformStamped transform; @@ -51,6 +45,31 @@ TransformStamped MakeTransformMessage(double timestamp_sec, double x, double y, class FakeBuffer : public BufferInterface { public: + using Key = std::tuple; + + void AddTransform(const std::string& target_frame, + const std::string& source_frame, double timestamp_sec, + const Eigen::Affine3d& transform) { + const auto key = Key(target_frame, source_frame, + cyber::Time(timestamp_sec).ToNanosecond()); + TransformStamped stamped; + stamped.mutable_header()->set_frame_id(target_frame); + stamped.mutable_header()->set_timestamp_sec(timestamp_sec); + stamped.set_child_frame_id(source_frame); + stamped.mutable_transform()->mutable_translation()->set_x( + transform.translation().x()); + stamped.mutable_transform()->mutable_translation()->set_y( + transform.translation().y()); + stamped.mutable_transform()->mutable_translation()->set_z( + transform.translation().z()); + const Eigen::Quaterniond rotation(transform.linear()); + stamped.mutable_transform()->mutable_rotation()->set_qw(rotation.w()); + stamped.mutable_transform()->mutable_rotation()->set_qx(rotation.x()); + stamped.mutable_transform()->mutable_rotation()->set_qy(rotation.y()); + stamped.mutable_transform()->mutable_rotation()->set_qz(rotation.z()); + transforms_[key] = stamped; + } + TransformStamped lookupTransform(const std::string& target_frame, const std::string& source_frame, const cyber::Time& time, @@ -60,6 +79,11 @@ class FakeBuffer : public BufferInterface { last_query_time_ = time; last_timeout_sec_ = timeout_second; ++lookup_call_count_; + const auto it = transforms_.find( + Key(target_frame, source_frame, time.ToNanosecond())); + if (it != transforms_.end()) { + return it->second; + } return lookup_transform_; } @@ -88,6 +112,10 @@ class FakeBuffer : public BufferInterface { last_query_time_ = time; last_timeout_sec_ = timeout_second; ++can_transform_call_count_; + if (transforms_.count( + Key(target_frame, source_frame, time.ToNanosecond())) > 0) { + return true; + } if (!can_transform_result_ && errstr != nullptr) { *errstr = "can transform failed"; } @@ -135,30 +163,51 @@ class FakeBuffer : public BufferInterface { bool can_transform_result_ = true; TransformStamped lookup_transform_; TransformStamped static_transform_; + std::map transforms_; }; -TEST(TransformCacheTest, InterpolatesBetweenSamples) { - TransformCache cache; - cache.SetCacheDuration(5.0); - cache.AddTransform(MakeStampedTransform(10.0, 0.0, 0.0, 0.0)); - cache.AddTransform(MakeStampedTransform(12.0, 10.0, 0.0, 0.0)); - - StampedTransform resolved; - EXPECT_TRUE(cache.QueryTransform(11.0, &resolved, 0.0)); - EXPECT_DOUBLE_EQ(resolved.timestamp, 11.0); - EXPECT_NEAR(resolved.translation.x(), 5.0, 1e-9); +TEST(TimedTransformResolverTest, QueryCachedInterpolatesBetweenSamples) { + FakeBuffer buffer; + buffer.AddTransform( + "map", "base_link", 10.0, + Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + buffer.AddTransform( + "map", "base_link", 12.0, + Eigen::Translation3d(10.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + + TimedTransformResolver resolver( + &buffer, "map", "base_link", + TimedTransformResolverOptions{0.01f, 5.0, 1.0, 0.015, true, true}); + ASSERT_TRUE(resolver.PrefetchBatch({10.0, 12.0})); + + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + TransformResolveStatus status = TransformResolveStatus::kEmpty; + EXPECT_TRUE(resolver.QueryCached(11.0, &pose, &status)); + EXPECT_EQ(status, TransformResolveStatus::kOk); + EXPECT_NEAR(pose.translation().x(), 5.0, 1e-9); } -TEST(TransformCacheTest, RejectsExtrapolationBeyondLimit) { - TransformCache cache; - cache.SetCacheDuration(5.0); - cache.AddTransform(MakeStampedTransform(10.0, 0.0, 0.0, 0.0)); - cache.AddTransform(MakeStampedTransform(12.0, 10.0, 0.0, 0.0)); - - StampedTransform resolved; - EXPECT_FALSE(cache.QueryTransform(13.5, &resolved, 1.0)); - EXPECT_TRUE(cache.QueryTransform(12.5, &resolved, 1.0)); - EXPECT_NEAR(resolved.translation.x(), 12.5, 1e-9); +TEST(TimedTransformResolverTest, QueryCachedExtrapolatesWithinLimit) { + FakeBuffer buffer; + buffer.AddTransform( + "map", "base_link", 10.0, + Eigen::Translation3d(0.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + buffer.AddTransform( + "map", "base_link", 12.0, + Eigen::Translation3d(10.0, 0.0, 0.0) * Eigen::Quaterniond::Identity()); + + TimedTransformResolver resolver( + &buffer, "map", "base_link", + TimedTransformResolverOptions{0.01f, 5.0, 1.0, 0.015, true, true}); + ASSERT_TRUE(resolver.PrefetchBatch({10.0, 12.0})); + + Eigen::Affine3d pose = Eigen::Affine3d::Identity(); + TransformResolveStatus status = TransformResolveStatus::kEmpty; + EXPECT_FALSE(resolver.QueryCached(13.5, &pose, &status)); + EXPECT_EQ(status, TransformResolveStatus::kTooOld); + EXPECT_TRUE(resolver.QueryCached(12.5, &pose, &status)); + EXPECT_EQ(status, TransformResolveStatus::kOk); + EXPECT_NEAR(pose.translation().x(), 12.5, 1e-9); } TEST(TimedTransformResolverTest, ResolveUsesInjectedTransformQueryOnce) { @@ -169,10 +218,11 @@ TEST(TimedTransformResolverTest, ResolveUsesInjectedTransformQueryOnce) { TimedTransformResolver resolver(&query); TimedTransformResolverOptions options; options.enable_extrapolation = false; + resolver.ConfigureFrames("map", "base_link"); resolver.SetOptions(options); StampedTransform resolved; - EXPECT_TRUE(resolver.Resolve(20.0, "map", "base_link", &resolved)); + EXPECT_TRUE(resolver.Resolve(20.0, &resolved)); EXPECT_EQ(buffer.can_transform_call_count_, 1); EXPECT_EQ(buffer.lookup_call_count_, 1); EXPECT_EQ(buffer.last_target_frame_, "map"); diff --git a/modules/transform/transform_query.cc b/modules/transform/transform_query.cc index f82ac40e..ca17a3e5 100644 --- a/modules/transform/transform_query.cc +++ b/modules/transform/transform_query.cc @@ -145,6 +145,12 @@ bool TransformQuery::GetLatestStaticTransformToAffine( Eigen::Affine3d* transform) const { CHECK_NOTNULL(transform); + // If target and source frame are identical, the transform is identity. + if (target_frame_id == source_frame_id) { + *transform = Eigen::Affine3d::Identity(); + return true; + } + TransformStamped stamped_transform; if (!GetLatestStaticTransform(target_frame_id, source_frame_id, &stamped_transform)) {