@@ -61,6 +61,7 @@ void DeepObjectDetectionNode::declareParameters()
6161 this ->declare_parameter <std::string>(" class_names_path" , " " );
6262 this ->declare_parameter <int >(" Model.num_classes" , 80 );
6363 this ->declare_parameter <std::string>(" Model.bbox_format" , " cxcywh" );
64+ this ->declare_parameter <std::vector<int64_t >>(" Model.output_shape" , std::vector<int64_t >());
6465
6566 this ->declare_parameter <int >(" Preprocessing.input_width" , 640 );
6667 this ->declare_parameter <int >(" Preprocessing.input_height" , 640 );
@@ -75,15 +76,10 @@ void DeepObjectDetectionNode::declareParameters()
7576 this ->declare_parameter <double >(" Postprocessing.nms_iou_threshold" , 0.45 );
7677 this ->declare_parameter <std::string>(" Postprocessing.score_activation" , " sigmoid" );
7778 this ->declare_parameter <bool >(" Postprocessing.enable_nms" , true );
78- this ->declare_parameter <bool >(" Postprocessing.use_multi_output" , false );
79- this ->declare_parameter <int >(" Postprocessing.output_boxes_idx" , 0 );
80- this ->declare_parameter <int >(" Postprocessing.output_scores_idx" , 1 );
81- this ->declare_parameter <int >(" Postprocessing.output_classes_idx" , 2 );
8279 this ->declare_parameter <std::string>(" Postprocessing.class_score_mode" , " all_classes" );
8380 this ->declare_parameter <int >(" Postprocessing.class_score_start_idx" , -1 );
8481 this ->declare_parameter <int >(" Postprocessing.class_score_count" , -1 );
8582
86- this ->declare_parameter <bool >(" Postprocessing.layout.auto_detect" , true );
8783 this ->declare_parameter <int >(" Postprocessing.layout.batch_dim" , 0 );
8884 this ->declare_parameter <int >(" Postprocessing.layout.detection_dim" , 1 );
8985 this ->declare_parameter <int >(" Postprocessing.layout.feature_dim" , 2 );
@@ -114,6 +110,11 @@ void DeepObjectDetectionNode::declareParameters()
114110 params_.model_metadata .num_classes = this ->get_parameter (" Model.num_classes" ).as_int ();
115111 params_.model_metadata .class_names_file = this ->get_parameter (" class_names_path" ).as_string ();
116112 params_.model_metadata .bbox_format = this ->get_parameter (" Model.bbox_format" ).as_string ();
113+ auto output_shape_int = this ->get_parameter (" Model.output_shape" ).as_integer_array ();
114+ params_.model_metadata .output_shape .clear ();
115+ for (auto dim : output_shape_int) {
116+ params_.model_metadata .output_shape .push_back (static_cast <size_t >(dim));
117+ }
117118
118119 // Preprocessing parameters
119120 params_.preprocessing .input_width = this ->get_parameter (" Preprocessing.input_width" ).as_int ();
@@ -136,15 +137,10 @@ void DeepObjectDetectionNode::declareParameters()
136137 static_cast <float >(this ->get_parameter (" Postprocessing.nms_iou_threshold" ).as_double ());
137138 params_.postprocessing .score_activation = this ->get_parameter (" Postprocessing.score_activation" ).as_string ();
138139 params_.postprocessing .enable_nms = this ->get_parameter (" Postprocessing.enable_nms" ).as_bool ();
139- params_.postprocessing .use_multi_output = this ->get_parameter (" Postprocessing.use_multi_output" ).as_bool ();
140- params_.postprocessing .output_boxes_idx = this ->get_parameter (" Postprocessing.output_boxes_idx" ).as_int ();
141- params_.postprocessing .output_scores_idx = this ->get_parameter (" Postprocessing.output_scores_idx" ).as_int ();
142- params_.postprocessing .output_classes_idx = this ->get_parameter (" Postprocessing.output_classes_idx" ).as_int ();
143140 params_.postprocessing .class_score_mode = this ->get_parameter (" Postprocessing.class_score_mode" ).as_string ();
144141 params_.postprocessing .class_score_start_idx = this ->get_parameter (" Postprocessing.class_score_start_idx" ).as_int ();
145142 params_.postprocessing .class_score_count = this ->get_parameter (" Postprocessing.class_score_count" ).as_int ();
146143
147- params_.postprocessing .layout .auto_detect = this ->get_parameter (" Postprocessing.layout.auto_detect" ).as_bool ();
148144 params_.postprocessing .layout .batch_dim = this ->get_parameter (" Postprocessing.layout.batch_dim" ).as_int ();
149145 params_.postprocessing .layout .detection_dim = this ->get_parameter (" Postprocessing.layout.detection_dim" ).as_int ();
150146 params_.postprocessing .layout .feature_dim = this ->get_parameter (" Postprocessing.layout.feature_dim" ).as_int ();
@@ -182,42 +178,11 @@ deep_ros::CallbackReturn DeepObjectDetectionNode::on_configure_impl(const rclcpp
182178 loadClassNames ();
183179 preprocessor_ = std::make_unique<ImagePreprocessor>(params_.preprocessing );
184180
185- // Get allocator from base class
186- auto allocator = get_current_allocator ();
187- if (!allocator) {
188- RCLCPP_ERROR (this ->get_logger (), " Plugin did not provide allocator" );
189- return deep_ros::CallbackReturn::FAILURE;
190- }
191-
192- // dynamically get the output shape by running a dummy inference
193- std::vector<size_t > input_shape = {
194- 1 ,
195- RGB_CHANNELS,
196- static_cast <size_t >(params_.preprocessing .input_height ),
197- static_cast <size_t >(params_.preprocessing .input_width )};
198- std::vector<size_t > output_shape;
199- try {
200- PackedInput dummy;
201- dummy.shape = input_shape;
202- size_t total_elements = 1 ;
203- for (size_t dim : input_shape) {
204- total_elements *= dim;
205- }
206- dummy.data .assign (total_elements, 0 .0f );
207-
208- deep_ros::Tensor input_tensor (dummy.shape , deep_ros::DataType::FLOAT32, allocator);
209- const size_t bytes = dummy.data .size () * sizeof (float );
210- allocator->copy_from_host (input_tensor.data (), dummy.data .data (), bytes);
211-
212- auto output_tensor = run_inference (input_tensor);
213- output_shape = output_tensor.shape ();
214- } catch (const std::exception & e) {
215- RCLCPP_WARN (this ->get_logger (), " Could not determine output shape: %s" , e.what ());
216- output_shape.clear ();
217- }
181+ // Get output shape from config (optional, for logging/validation)
182+ const std::vector<size_t > & output_shape = params_.model_metadata .output_shape ;
218183
219184 auto formatShape = [](const std::vector<size_t > & shape) {
220- if (shape.empty ()) return std::string (" auto-detect " );
185+ if (shape.empty ()) return std::string (" not specified " );
221186 std::string result;
222187 for (size_t i = 0 ; i < shape.size (); ++i) {
223188 result += std::to_string (shape[i]);
@@ -227,30 +192,19 @@ deep_ros::CallbackReturn DeepObjectDetectionNode::on_configure_impl(const rclcpp
227192 };
228193
229194 if (!output_shape.empty ()) {
230- RCLCPP_INFO (this ->get_logger (), " Detected model output shape: [%s]" , formatShape (output_shape).c_str ());
195+ RCLCPP_INFO (this ->get_logger (), " Configured model output shape: [%s]" , formatShape (output_shape).c_str ());
231196 }
232197
233198 const bool use_letterbox = (params_.preprocessing .resize_method == " letterbox" );
234199
235200 GenericPostprocessor::OutputLayout layout =
236- GenericPostprocessor::autoConfigure (output_shape, params_.postprocessing .layout );
237- if (layout.auto_detect && !output_shape.empty ()) {
238- RCLCPP_INFO (
239- this ->get_logger (),
240- " Auto-detected layout: batch_dim=%zu, detection_dim=%zu, feature_dim=%zu" ,
241- layout.batch_dim ,
242- layout.detection_dim ,
243- layout.feature_dim );
244- } else if (!layout.auto_detect ) {
245- RCLCPP_INFO (
246- this ->get_logger (),
247- " Using manual layout: batch_dim=%zu, detection_dim=%zu, feature_dim=%zu" ,
248- layout.batch_dim ,
249- layout.detection_dim ,
250- layout.feature_dim );
251- } else {
252- RCLCPP_INFO (this ->get_logger (), " Layout will be auto-detected from first inference" );
253- }
201+ GenericPostprocessor::configureLayout (output_shape, params_.postprocessing .layout );
202+ RCLCPP_INFO (
203+ this ->get_logger (),
204+ " Using configured layout: batch_dim=%zu, detection_dim=%zu, feature_dim=%zu" ,
205+ layout.batch_dim ,
206+ layout.detection_dim ,
207+ layout.feature_dim );
254208
255209 postprocessor_ = std::make_unique<GenericPostprocessor>(
256210 params_.postprocessing ,
@@ -516,24 +470,27 @@ void DeepObjectDetectionNode::processImages(
516470 auto start_time = std::chrono::steady_clock::now ();
517471 std::vector<cv::Mat> processed;
518472 std::vector<ImageMeta> metas;
473+ std::vector<std_msgs::msg::Header> processed_headers;
519474 processed.reserve (images.size ());
520475 metas.reserve (images.size ());
476+ processed_headers.reserve (images.size ());
521477
522478 // Preprocess all images
523- for (const auto & img : images) {
524- if (img .empty ()) {
479+ for (size_t i = 0 ; i < images. size () && i < headers. size (); ++i ) {
480+ if (images[i] .empty ()) {
525481 RCLCPP_WARN (this ->get_logger (), " Received empty image, skipping" );
526482 continue ;
527483 }
528484
529485 ImageMeta meta;
530- cv::Mat preprocessed = preprocessor_->preprocess (img , meta);
486+ cv::Mat preprocessed = preprocessor_->preprocess (images[i] , meta);
531487 if (preprocessed.empty ()) {
532488 RCLCPP_WARN (this ->get_logger (), " Preprocessing returned empty image, skipping" );
533489 continue ;
534490 }
535491 processed.push_back (std::move (preprocessed));
536492 metas.push_back (meta);
493+ processed_headers.push_back (headers[i]);
537494 }
538495
539496 if (processed.empty ()) {
@@ -553,14 +510,8 @@ void DeepObjectDetectionNode::processImages(
553510 allocator->copy_from_host (input_tensor.data (), packed_input.data .data (), bytes);
554511
555512 // Run inference
556- std::vector<std::vector<SimpleDetection>> batch_detections;
557- if (params_.postprocessing .use_multi_output ) {
558- auto output_tensor = run_inference (input_tensor);
559- batch_detections = postprocessor_->decodeMultiOutput ({output_tensor}, metas);
560- } else {
561- auto output_tensor = run_inference (input_tensor);
562- batch_detections = postprocessor_->decode (output_tensor, metas);
563- }
513+ auto output_tensor = run_inference (input_tensor);
514+ std::vector<std::vector<SimpleDetection>> batch_detections = postprocessor_->decode (output_tensor, metas);
564515
565516 auto end_time = std::chrono::steady_clock::now ();
566517 auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count ();
@@ -576,13 +527,6 @@ void DeepObjectDetectionNode::processImages(
576527 return sum + dets.size ();
577528 }));
578529
579- // Use headers that match the processed images (may be fewer if some were skipped)
580- std::vector<std_msgs::msg::Header> processed_headers;
581- processed_headers.reserve (processed.size ());
582- for (size_t i = 0 ; i < processed.size () && i < headers.size (); ++i) {
583- processed_headers.push_back (headers[i]);
584- }
585-
586530 publishDetections (batch_detections, processed_headers, metas);
587531}
588532
0 commit comments