Remove dependency to jskbbox and rosmarker in ukf tracker

NuronLabs · Nov 6, 2018 · 4fa1dd4 · 4fa1dd4
1 parent 1637baa
commit 4fa1dd4
Show file tree

Hide file tree

Showing 5 changed files with 25 additions and 325 deletions.
diff --git a/...perception/detection/lidar_tracker/packages/lidar_imm_ukf_pda_track/include/imm_ukf_pda.h b/...perception/detection/lidar_tracker/packages/lidar_imm_ukf_pda_track/include/imm_ukf_pda.h
@@ -40,11 +40,6 @@
 
 #include <tf/transform_listener.h>
 
-#include <jsk_recognition_msgs/BoundingBox.h>
-#include <jsk_recognition_msgs/BoundingBoxArray.h>
-
-#include <visualization_msgs/MarkerArray.h>
-
 #include "autoware_msgs/DetectedObject.h"
 #include "autoware_msgs/DetectedObjectArray.h"
 
@@ -80,9 +75,6 @@ class ImmUkfPda
   // switch sukf and ImmUkfPda
   bool use_sukf_;
 
-  // whether if publish debug ros markers
-  bool is_debug_;
-
   // whether if benchmarking tracking result
   bool is_benchmark_;
   int frame_count_;
@@ -106,36 +98,19 @@ class ImmUkfPda
   ros::NodeHandle node_handle_;
   ros::Subscriber sub_detected_array_;
   ros::Publisher pub_object_array_;
-  ros::Publisher pub_jskbbox_array_;
-  ros::Publisher pub_adas_direction_array_;
-  ros::Publisher pub_adas_prediction_array_;
-  ros::Publisher pub_points_array_;
-  ros::Publisher pub_texts_array_;
 
   std_msgs::Header input_header_;
 
   void callback(const autoware_msgs::DetectedObjectArray& input);
-  void setPredictionObject();
-  void relayJskbbox(const autoware_msgs::DetectedObjectArray& input,
-                    jsk_recognition_msgs::BoundingBoxArray& jskbboxes_output);
   void transformPoseToGlobal(const autoware_msgs::DetectedObjectArray& input,
                              autoware_msgs::DetectedObjectArray& transformed_input);
-  void transformPoseToLocal(jsk_recognition_msgs::BoundingBoxArray& jskbboxes_output,
-                            autoware_msgs::DetectedObjectArray& detected_objects_output);
+  void transformPoseToLocal(autoware_msgs::DetectedObjectArray& detected_objects_output);
   void measurementValidation(const autoware_msgs::DetectedObjectArray& input, UKF& target, const bool second_init,
                              const Eigen::VectorXd& max_det_z, const Eigen::MatrixXd& max_det_s,
                              std::vector<autoware_msgs::DetectedObject>& object_vec, std::vector<bool>& matching_vec);
   void getNearestEuclidCluster(const UKF& target, const std::vector<autoware_msgs::DetectedObject>& object_vec,
                                autoware_msgs::DetectedObject& object, double& min_dist);
-  void getRightAngleBBox(const std::vector<double> nearest_bbox, std::vector<double>& rightAngle_bbox);
   void associateBB(const std::vector<autoware_msgs::DetectedObject>& object_vec, UKF& target);
-  double getBBoxYaw(const UKF target);
-  double getJskBBoxArea(const jsk_recognition_msgs::BoundingBox& jsk_bb);
-  double getJskBBoxYaw(const jsk_recognition_msgs::BoundingBox& jsk_bb);
-  void updateBB(UKF& target);
-  void mergeOverSegmentation(const std::vector<UKF> targets);
-
-  void updateJskLabel(const UKF& target, jsk_recognition_msgs::BoundingBox& bb);
   void updateBehaviorState(const UKF& target, autoware_msgs::DetectedObject& object);
 
   void initTracker(const autoware_msgs::DetectedObjectArray& input, double timestamp);
@@ -153,18 +128,14 @@ class ImmUkfPda
   void staticClassification();
 
   void makeOutput(const autoware_msgs::DetectedObjectArray& input,
-                  jsk_recognition_msgs::BoundingBoxArray& jskbboxes_output,
-                  autoware_msgs::DetectedObjectArray& detected_objects_output);
+                autoware_msgs::DetectedObjectArray& detected_objects_output);
 
   void removeUnnecessaryTarget();
 
-  void pubDebugRosMarker(const autoware_msgs::DetectedObjectArray& input);
-
   void dumpResultText(autoware_msgs::DetectedObjectArray& detected_objects);
 
   void tracker(const autoware_msgs::DetectedObjectArray& transformed_input,
-               jsk_recognition_msgs::BoundingBoxArray& jskbboxes_output,
-               autoware_msgs::DetectedObjectArray& detected_objects_output);
+              autoware_msgs::DetectedObjectArray& detected_objects_output);
 
 public:
   ImmUkfPda();

diff --git a/...mputing/perception/detection/lidar_tracker/packages/lidar_imm_ukf_pda_track/include/ukf.h b/...mputing/perception/detection/lidar_tracker/packages/lidar_imm_ukf_pda_track/include/ukf.h
@@ -40,7 +40,7 @@
 #include <pcl/point_cloud.h>
 
 #include <geometry_msgs/PoseStamped.h>
-#include <jsk_recognition_msgs/BoundingBox.h>
+// #include <jsk_recognition_msgs/BoundingBox.h>
 
 #include "autoware_msgs/DetectedObject.h"
 
@@ -72,9 +72,6 @@ class UKF
 
   std::string label_;
 
-  //* initially set to false, set to true in first call of ProcessMeasurement
-  bool is_initialized_;
-
   //* state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
   Eigen::MatrixXd x_merge_;
 
@@ -139,8 +136,8 @@ class UKF
   //* Sigma point spreading parameter
   double lambda_;
 
-  int count_;
-  int count_empty_;
+  // int count_;
+  // int count_empty_;
 
   double mode_match_prob_cv2cv_;
   double mode_match_prob_ctrv2cv_;
@@ -191,10 +188,8 @@ class UKF
   // bounding box params
   bool is_vis_bb_;
 
-  jsk_recognition_msgs::BoundingBox jsk_bb_;
-  jsk_recognition_msgs::BoundingBox best_jsk_bb_;
-
-  bool is_best_jsk_bb_empty_;
+  geometry_msgs::Pose bb_pose_;
+  geometry_msgs::Vector3 bb_dimensions_;
 
   double best_yaw_;
   double bb_yaw_;

diff --git a/.../detection/lidar_tracker/packages/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track.launch b/.../detection/lidar_tracker/packages/lidar_imm_ukf_pda_track/launch/imm_ukf_pda_track.launch
@@ -28,7 +28,6 @@
         <param name="pointcloud_frame"        value="$(arg pointcloud_frame)" />
         <param name="tracking_frame"          value="$(arg tracking_frame)" />
         <param name="use_sukf"                value="$(arg use_sukf)" />
-        <param name="is_debug"                value="$(arg is_debug)" />
 
         <remap from="/detection/lidar_objects"         to="$(arg tracker_input_topic)" />
         <remap from="/detection/lidar_tracker/bounding_boxes" to="$(arg tracker_output_jskbb_topic)" />