├── .gitignore ├── CMakeLists.txt ├── README.md ├── config ├── params_flightmare_sim.yaml └── params_snapdragon_flight.yaml ├── doc └── tracking.png ├── include └── multi_robot_tracking │ ├── JpdafFilter.h │ ├── PhdFilter.h │ ├── SimpleKalman.h │ ├── detection.h │ ├── hungarian_alg.h │ ├── kalman.h │ ├── track.h │ └── tracker_param.h ├── launch ├── demo.launch ├── demo_left.launch ├── demo_node.launch ├── demo_old.launch └── demo_right.launch ├── nodelet_plugin.xml ├── package.xml ├── rviz └── darknet_vis.rviz ├── scripts ├── flightmare_test.py ├── rmse_calc.py └── sample_data.py └── src ├── SimpleKalman.cpp ├── hungarian_alg.cpp ├── jpdaf_filter.cpp ├── kalman.cpp ├── multi_robot_tracking_node.cpp ├── multi_robot_tracking_nodelet.cpp ├── phd_filter.cpp └── track.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(multi_robot_tracking) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | 8 | find_package(catkin REQUIRED COMPONENTS 9 | geometry_msgs 10 | nodelet 11 | roscpp 12 | std_msgs 13 | tf 14 | cv_bridge 15 | nav_msgs 16 | image_transport 17 | ) 18 | find_package(Eigen3 REQUIRED NO_MODULE) 19 | 20 | catkin_package( 21 | INCLUDE_DIRS 22 | LIBRARIES 23 | CATKIN_DEPENDS 24 | geometry_msgs 25 | nodelet 26 | roscpp 27 | std_msgs 28 | tf 29 | DEPENDS 30 | EIGEN3 31 | ) 32 | 33 | # include headers 34 | include_directories( 35 | ${catkin_INCLUDE_DIRS} 36 | ${OpenCV_INCLUDE_DIRS} 37 | ${Eigen_INCLUDE_DIRS} 38 | include 39 | include/multi_robot_tracking 40 | ) 41 | 42 | add_library(multi_robot_tracking src/hungarian_alg.cpp src/SimpleKalman.cpp src/track.cpp src/jpdaf_filter.cpp src/phd_filter.cpp src/kalman.cpp src/multi_robot_tracking_nodelet.cpp) 43 | target_include_directories(multi_robot_tracking PUBLIC include ${catkin_LIBRARIES}) 44 | 45 | target_link_libraries(multi_robot_tracking PUBLIC ${catkin_LIBRARIES} Eigen3::Eigen) 46 | add_dependencies(multi_robot_tracking ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 47 | 48 | add_executable(multi_robot_tracking_node src/multi_robot_tracking_node.cpp) 49 | add_dependencies(multi_robot_tracking_node ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 50 | target_link_libraries(multi_robot_tracking_node multi_robot_tracking ${catkin_LIBRARIES}) 51 | 52 | 53 | install( 54 | TARGETS multi_robot_tracking 55 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 58 | ) 59 | 60 | # install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 61 | install(FILES nodelet_plugin.xml 62 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 63 | ) 64 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Multi Agent Tracking 2 | Multi Agent Tracking for Data Association with various filter comparison 3 | 4 | ## License 5 | Please be aware that this code was originally implemented for research purposes and may be subject to changes and any fitness for a particular purpose is disclaimed. To inquire about commercial licenses, please contact Prof. Giuseppe Loianno (loiannog@nyu.edu). 6 | ``` 7 | This program is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | This program is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with this program. If not, see . 19 | 20 | ``` 21 | ## Citation 22 | If you publish a paper with this work, please cite our paper: 23 | ``` 24 | @article{VisualTraIROS2022, 25 | url = {https://arxiv.org/abs/2207.08301}, 26 | Year = {2022}, 27 | Booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)} 28 | author = {Ge, Rundong and Lee, Moonyoung and Radhakrishnan, Vivek and Zhou, Yang and Li, Guanrui and Loianno, Giuseppe}, 29 | title = {Vision-based Relative Detection and Tracking for Teams of Micro Aerial Vehicles}} 30 | ``` 31 | 32 | ## Overview 33 | This is ROS package developed for tracking multiple agents using a camera. This software is designed for associating 2D bounding box measurements (of drones) to unique IDs and 2D position in image space. The required inputs for this software are 2d bounding box measurements and RGB image. The provided outputs are 2D positions with target associations. The motion model is based on Gaussian Linear kalman filter with probabilistic hypothesis density (PHD) filter to solve association. Can also run Joint Probability Association filter (JPDAF) by switching rosparam upon launch. This repository includes matlab implementation and evaluation as well. 34 | 35 | ![Screenshot](doc/tracking.png) 36 | 37 | 38 | **Developer: Mark Lee, Vivek Radhakrishnan
39 | Affiliation: [NYU ARPL](https://wp.nyu.edu/arpl/)
40 | Maintainer: Vivek Radhakrishnan, vr2171@nyu.edu
** 41 | 42 | #### Subscribed Topics 43 | |Name|Type|Description| 44 | |---|---|---| 45 | |`/hummingbird0/track/bounding_box`|geometry_msgs/PoseArray|output 2D position from Flightmare rosbag| 46 | |`/hummingbird0/camera/rgb`|sensor_msgs/Image|RGB image| 47 | |`/hummingbird0/imu`|sensor_msgs/Imu|IMU data|position 48 | 49 | #### Published Topics 50 | |Name|Type|Description| 51 | |---|---|---| 52 | |`/tracked_image`|sensor_msgs/Image|RGB with position, ID association labeled| 53 | |`/tracked_pose_output`|geometry_msgs/PoseArray|position (x,y) in pixel coordinate with ID association, which is the index array| 54 | |`/tracked_velocity_output`|geometry_msgs/PoseArray|linear velocity (x,y) in pixel coordinate with ID association, velocity in pixel space| 55 | 56 | #### ROS Parameters 57 | |Name|Description| 58 | |---|---| 59 | |`filter`|phd or jpdaf specify in the demo.launch file| 60 | |`input_bbox_topic`|output of py_imag_proc or /hummingbird0/track/bounding_box specify in the demo.launch file| 61 | |`input_rgb_topic`|/hires/image_raw or hummingbird0/camera/rgb specify in the demo.launch file| 62 | |`input_imu_topic`|output of dragonfly imu or hummingbird0/imu specify in the demo.launch file| 63 | |`num_drones`|how many drones in FOV specify in the demo.launch file| 64 | |`camera_cx`|CX paramerter of the camera projection matrix| 65 | |`camera_cy`|CY paramerter of the camera projection matrix| 66 | |`camera_f`'|F of the camera projection matrix| 67 | |`dt`|Hard coded time difference between imu frames (not used anymore)| 68 | |`viz_detection_height`|Image height used by the detection algorithm| 69 | |`viz_detection_width`|Image width used by the detection algorithm| 70 | |`viz_detection_offset_x`|Output drawing offset in X axis in case of black bars| 71 | |`viz_detection_offset_y`|Output drawing offset in Y axis in case of black bars| 72 | |`enable_async_pdf`|Not used anymore| 73 | |`use_generated_id`|Assigns sequential IDs to subsequent detections| 74 | |`phd/q_pos`|Process noise for the evolution of position| 75 | |`phd/q_vel`|Process noise for the evolution of velocity| 76 | |`phd/r_meas`|Measurement noise| 77 | |`phd/p_pos_init`|Initial process covariance for position| 78 | |`phd/p_vel_init`|Initial process covariance for velocity| 79 | |`phd/prune_weight_threshold`|Minimum threshold weight for pruning. If the weight is lower than this value, its removed from the next step| 80 | |`phd/prune_mahalanobis_threshold`|Maximum mahalanobis distance for merging different updates| 81 | |`phd/extract_weight_threshold`|Minimum threshold for the weight for state extraction. If the weight is lower than this value, we update the state using the preditction instead of update| 82 | |`jpdaf/q_pos`|Process noise for the evolution of position| 83 | |`jpdaf/q_vel`|Process noise for the evolution of velocity| 84 | |`jpdaf/p_pos_init`|Initial process covariance for position| 85 | |`jpdaf/p_vel_init`|Initial process covariance for velocity| 86 | |`jpdaf/r_meas`|Measurement noise| 87 | |`jpdaf/alpha_0_threshold`|JPDAF Alpha 0 threshold| 88 | |`jpdaf/alpha_cam`|pixel size ratio, use 1 for normal images| 89 | |`jpdaf/associaiton_cost`|Association cost for JPDAF| 90 | |`jpdaf/beta_0_threshold`|JPDAF beta 0 threshold| 91 | |`jpdaf/false_measurements_density`|JPDAF false measurement threshold| 92 | |`jpdaf/gamma`|JPDAF gamma| 93 | |`jpdaf/max_missed_rate`|JPDAF max missed rate| 94 | |`jpdaf/min_acceptance_rate`|JPDAF minimim acceptance rate| 95 | |`jpdaf/probability_detection`|JPDAF probability of detection| 96 | 97 | ## Install 98 | The tracking filter package is dependent on Eigen and Boost, and ROS. The additional repo can be installed below: 99 | 100 | 101 | clone the filter repository into catkin src directory and build using catkin 102 | ``` 103 | $ git clone https://github.com/arplaboratory/multi_robot_tracking.git 104 | $ cd .. 105 | $ catkin build 106 | $ source devel/setup.bash 107 | ``` 108 | 109 | retrieve exp2 rosbag data from [ARPL data folder](https://drive.google.com/drive/folders/1dSBd08ocj_x8MGDS-cl1F2HeHKFADlqP?usp=sharing) after gaining access 110 | 111 | ## Running 112 | This pacakge runs the tracking filter only -- it doesn't provide image detection. If image detection package is not available, can run with either by recorded rosbag data or by acquiring ground truth detection from simulation. Boths options are shown below. Specifiy filter rosparam in the demo.launch file to select phd or jpdaf filter. 113 | 114 | ``` 115 | $ roslaunch multi_robot_tracking demo.launch 116 | ``` 117 | 118 | A. Testing with recorded Rosbag.
119 | Move rosbag data into corresponding directory. Modify the rostopic subscriber in demo.launch if wanting to modify using a different measured input. 120 | ``` 121 | $ roslaunch multi_robot_tracking demo.launch 122 | -- open a new tab and navigate to rosbag directory 123 | $ rosbag play "bag_file_name".bag --clock 124 | ``` 125 | 126 | B. Testing with simulation.
127 | We can also utilize the Flightare simulator for photorealistic rendering to create new data. Refer to [Flightmare github page](https://github.com/arplaboratory/flightmare) for installation and setting up dependencies. This modified version of fligtmare also depends on a modified version of rotors simulator. This can be found here [Rotors Simulator github page](https://github.com/arplaboratory/rotors_simulator)
128 | Running the below code launches Flightmare simulation with Gazebo for multiple drones, multimotion creates trajectories for all drones, and phd_tracker associates each target in space. 129 | ``` 130 | $ roslaunch flightros multi_robot.launch 131 | -- open a new tab 132 | $ roslaunch multi_robot_tracking demo.launch 133 | -- open a new tab 134 | $ rosrun multi_robot_tracking flightmare_test.py 135 | ``` 136 | 137 | C. Testing Matlab for Evaluation 138 | Move the corresponding rosbag data to the Matlab directory. Make sure the GM_PHD_Initialisation_drones.m file points to the correct directory for rosbag data. 139 | 140 | -------------------------------------------------------------------------------- /config/params_flightmare_sim.yaml: -------------------------------------------------------------------------------- 1 | input_bbox_topic: "/hummingbird0/track/bounding_box" 2 | input_img_topic: "/hummingbird0/camera/rgb" 3 | input_imu_topic: "/hummingbird0/imu" 4 | 5 | init_pos_self_x: 0.0 6 | init_pos_self_y: 0.0 7 | init_pos_x_left: 3.4641 8 | init_pos_y_left: 2.0 9 | init_pos_x_right: 3.4641 10 | init_pos_y_right: -2.0 11 | id_self: 0 12 | id_left: 1 13 | id_right: 2 14 | 15 | camera_cx: 180 16 | camera_cy: 120 17 | camera_f: 120 18 | 19 | dt: 0.225 20 | 21 | viz_detection_height: 240 22 | viz_detection_width: 320 23 | viz_detection_offset_x: 0 24 | viz_detection_offset_y: 0 25 | 26 | enable_async_pdf: true 27 | 28 | use_generated_id: true 29 | 30 | phd: 31 | q_pos: 6.25 32 | q_vel: 12.5 33 | r_meas: 10 34 | p_pos_init: 5.0 35 | p_vel_init: 10.0 36 | prune_weight_threshold: 0.1 37 | prune_mahalanobis_threshold: 4.0 38 | extract_weight_threshold: 0.5 39 | 40 | jpdaf: 41 | q_pos: 100.0 42 | q_vel: 100.0 43 | p_pos_init: 10.0 44 | p_vel_init: 20.0 45 | r_meas: 2.0 46 | alpha_0_threshold: 0.95 47 | alpha_cam: 1.0 48 | associaiton_cost: 50.0 49 | beta_0_threshold: 0.5 50 | false_measurements_density: 0.00000003034 51 | gamma: 10.0 52 | max_missed_rate: 15 53 | min_acceptance_rate: 15 54 | probability_detection: 0.952662721893 55 | -------------------------------------------------------------------------------- /config/params_snapdragon_flight.yaml: -------------------------------------------------------------------------------- 1 | input_bbox_topic: "/DragonPro1/snpe_ros/detections" 2 | input_img_topic: "/DragonPro1/image_publisher/image_raw" 3 | input_imu_topic: "/DragonPro1/imu" 4 | 5 | init_pos_self_x: 0.0 6 | init_pos_self_y: 0.0 7 | init_pos_x_left: 3.4641 8 | init_pos_y_left: 2.0 9 | init_pos_x_right: 3.4641 10 | init_pos_y_right: -2.0 11 | id_self: 0 12 | id_left: 1 13 | id_right: 2 14 | 15 | camera_cx: 329 16 | camera_cy: 243 17 | camera_f: 431 18 | 19 | dt: 0.225 20 | 21 | viz_detection_height: 168 22 | viz_detection_width: 224 23 | viz_detection_offset_x: 0 24 | viz_detection_offset_y: -28 25 | 26 | enable_async_pdf: true 27 | 28 | use_generated_id: true 29 | 30 | phd: 31 | q_pos: 6.25 32 | q_vel: 12.5 33 | r_meas: 100 34 | p_pos_init: 5.0 35 | p_vel_init: 2.0 36 | prune_weight_threshold: 0.1 37 | prune_mahalanobis_threshold: 4.0 38 | extract_weight_threshold: 0.5 39 | 40 | jpdaf: 41 | q_pos: 100.0 42 | q_vel: 100.0 43 | p_pos_init: 10.0 44 | p_vel_init: 20.0 45 | r_meas: 2.0 46 | alpha_0_threshold: 0.95 47 | alpha_cam: 1.0 48 | associaiton_cost: 50.0 49 | beta_0_threshold: 0.5 50 | false_measurements_density: 0.00000003034 51 | gamma: 10.0 52 | max_missed_rate: 15 53 | min_acceptance_rate: 15 54 | probability_detection: 0.952662721893 -------------------------------------------------------------------------------- /doc/tracking.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/arplaboratory/multi_robot_tracking/08aa293d0a9d7aa74a3c4824770734af52e15578/doc/tracking.png -------------------------------------------------------------------------------- /include/multi_robot_tracking/JpdafFilter.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | //ros 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | //detection bbox 12 | //#include 13 | //#include 14 | 15 | //CV 16 | #include 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | //jpdaf 24 | #include 25 | 26 | #include 27 | 28 | 29 | #define PI 3.14159 30 | 31 | 32 | 33 | class JpdafFilter 34 | { 35 | public: 36 | JpdafFilter(); 37 | 38 | //functions 39 | void initialize_matrix(); 40 | void track(bool called_from_detection); 41 | void timer_callback(const ros::TimerEvent& event); 42 | 43 | ros::Time startTime,endTime,processTime; 44 | 45 | 46 | std::vector get_detections_flightmare(const geometry_msgs::PoseArray latest_detection); 47 | Eigen::Vector3f compute_angular_velocity(double detection_timestamp); 48 | bool imu_buffer_ok(double detection_timestamp); 49 | bool image_buffer_ok(double detection_timestamp); 50 | 51 | void publishTracks(double detection_timestamp); 52 | 53 | Eigen::MatrixXf association_matrix(const std::vector detections); 54 | 55 | void manage_new_old_tracks(std::vector detections, std::vector alphas_0, std::vector betas_0, Eigen::Vector3f omega, double time_step); 56 | std::vector create_new_tracks(std::vector detections, std::vector unassoc_detections, Eigen::Vector3f omega, double time_step); 57 | 58 | Eigen::MatrixXf compute_betas_matrix(std::vector hypothesis_mats, std::vector hypothesis_probs); 59 | 60 | std::vector generate_hypothesis_matrices(Eigen::MatrixXf assoc_mat); 61 | 62 | std::vector compute_probabilities_of_hypothesis_matrices(std::vector hypothesis_matrices, std::vector detections); 63 | 64 | double probability_of_hypothesis_unnormalized(Eigen::MatrixXf hypothesis, std::vector detections); 65 | 66 | Eigen::MatrixXf tau(Eigen::MatrixXf hypothesis);//THIS FUNCTION ASSUMES A VALID HYPOTHESIS MATRIX, NO CHECKS ARE PERFORMED 67 | Eigen::MatrixXf delta(Eigen::MatrixXf hypothesis); //THIS FUNCTION ASSUMES A VALID HYPOTHESIS MATRIX, NO CHECKS ARE PERFORMED 68 | 69 | void draw_tracks_publish_image(std::vector detections, double detection_time_stamp, std::vector projected_predictions); 70 | 71 | std::vector get_nonzero_indexes_row(Eigen::MatrixXf mat); 72 | 73 | 74 | Eigen::Matrix yprToRot(const Eigen::Matrix& ypr); 75 | 76 | void create_tracks_test_input(); 77 | 78 | void writeToFile(int nb_detections, Eigen::Vector3f omega_cam); 79 | 80 | 81 | //variables 82 | int detected_size_k; 83 | bool first_callback = true; 84 | Eigen::MatrixXf Z_k; 85 | 86 | bool track_init = true; 87 | 88 | std::vector prev_unassoc_detections; 89 | std::vector flightmare_bounding_boxes_msgs_buffer_; 90 | std::vector image_buffer_; 91 | std::vector imu_buffer_; 92 | 93 | image_transport::Publisher image_pub_; 94 | image_transport::Publisher image_debug_pub_; 95 | ros::Publisher tracks_pub_; 96 | 97 | double last_timestamp_synchronized; 98 | double last_timestamp_from_rostime; 99 | bool last_track_from_detection; 100 | 101 | std::vector tracks_; 102 | std::vector lost_tracks; 103 | 104 | TrackerParam params; 105 | 106 | ros::Timer update_timer; 107 | 108 | Eigen::Matrix3f R_cam_imu; 109 | 110 | int debug_track_counter; 111 | 112 | fstream output_file_; 113 | 114 | 115 | 116 | private: 117 | 118 | 119 | }; 120 | 121 | -------------------------------------------------------------------------------- /include/multi_robot_tracking/PhdFilter.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | //#include 4 | #include 5 | 6 | //ros 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | //detection bbox 13 | //#include 14 | //#include 15 | 16 | //CV 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | 25 | #define PI 3.14159 26 | //#define NUM_DRONES 3 27 | 28 | class PhdFilter 29 | { 30 | public: 31 | PhdFilter(); 32 | 33 | void initialize_matrix(float cam_cu, float cam_cv, float cam_f, float meas_dt=0.225); 34 | void initialize(float q_pos, float q_vel, float r_meas, float p_pos_init, float p_vel_init, 35 | float prune_weight_threshold, float prune_mahalanobis_threshold, float extract_weight_threshold); 36 | void phd_track(); 37 | void phd_predict_existing(); 38 | void phd_construct(); 39 | void phd_update(); 40 | void phd_prune(); 41 | void phd_state_extract(); 42 | void asynchronous_predict_existing(); 43 | void removeColumn(Eigen::MatrixXd& matrix, unsigned int colToRemove); 44 | void removeColumnf(Eigen::MatrixXf& matrix, unsigned int colToRemove); 45 | void removeColumni(Eigen::MatrixXi& matrix, unsigned int colToRemove); 46 | 47 | void set_num_drones(int num_drones); 48 | float clutter_intensity(const float X, const float Y); 49 | Eigen::MatrixXf left_divide(const Eigen::MatrixXf); 50 | void update_F_matrix(float input_dt); 51 | void update_A_matrix(float input_dt); 52 | 53 | 54 | ros::Time startTime,endTime,processTime; 55 | 56 | geometry_msgs::PoseArray Z_current_k; 57 | int NUM_DRONES; 58 | int detected_size_k; 59 | 60 | float last_timestamp_synchronized; 61 | double last_timestamp_from_rostime; 62 | bool first_callback = true; 63 | int numTargets_Jk_k_minus_1; 64 | int numTargets_Jk_minus_1; 65 | int L = 0; 66 | 67 | int k_iteration = 0; 68 | bool flag_asynch_start = false; 69 | bool enable_async = true; 70 | 71 | 72 | //kalman filter variables 73 | Eigen::MatrixXf F; 74 | Eigen::MatrixXf A; 75 | Eigen::MatrixXf H; 76 | Eigen::MatrixXf Q; 77 | Eigen::MatrixXf R; 78 | Eigen::MatrixXf K; 79 | 80 | //phd variables 81 | 82 | float prob_survival = 1.0; 83 | float prob_detection = 0.9;//1.0; 84 | 85 | float dt_cam = 0.125; //8hz 86 | float dt_imu = 0.01; //100hz 87 | 88 | Eigen::MatrixXf mk_minus_1; 89 | Eigen::MatrixXf wk_minus_1; 90 | Eigen::MatrixXf Pk_minus_1; 91 | Eigen::MatrixXf mk_k_minus_1; 92 | Eigen::MatrixXf wk_k_minus_1; 93 | Eigen::MatrixXf Pk_k_minus_1; 94 | Eigen::MatrixXf P_k_k; 95 | 96 | Eigen::MatrixXf S; 97 | 98 | Eigen::MatrixXf mk; 99 | Eigen::MatrixXf wk; 100 | Eigen::MatrixXf Pk; 101 | 102 | Eigen::MatrixXf mk_bar; 103 | Eigen::MatrixXf wk_bar; 104 | Eigen::MatrixXf Pk_bar; 105 | 106 | Eigen::MatrixXf mk_bar_fixed; 107 | Eigen::MatrixXf wk_bar_fixed; 108 | Eigen::MatrixXf Pk_bar_fixed; 109 | 110 | Eigen::MatrixXf mk_k_minus_1_beforePrediction; 111 | 112 | Eigen::MatrixXf Z_k; 113 | Eigen::MatrixXf Z_k_previous; 114 | Eigen::MatrixXf ang_vel_k; 115 | Eigen::MatrixXf X_k; 116 | Eigen::MatrixXf X_k_previous; 117 | 118 | Eigen::MatrixXf B; 119 | 120 | Eigen::MatrixXf Detections; 121 | Eigen::MatrixXf mahalDistance; 122 | 123 | sensor_msgs::ImagePtr image_msg; 124 | 125 | float cu, cv, f; 126 | float dt; 127 | 128 | const uint8_t n_state = 4; 129 | const uint8_t n_meas = 2; 130 | const uint8_t n_input = 3; 131 | 132 | private: 133 | 134 | 135 | }; 136 | 137 | -------------------------------------------------------------------------------- /include/multi_robot_tracking/SimpleKalman.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | //#include 4 | #include 5 | 6 | //ros 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | //CV 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | 21 | #define PI 3.14159 22 | //#define NUM_DRONES 3 23 | 24 | class KalmanFilter 25 | { 26 | public: 27 | KalmanFilter(); 28 | 29 | void initializeMatrix(float cam_cu, float cam_cv, float cam_f, float meas_dt=0.225); 30 | void initialize(float q_pos, float q_vel, float r_meas, float p_pos_init, float p_vel_init, 31 | float prune_weight_threshold, float prune_mahalanobis_threshold, float extract_weight_threshold); 32 | void kalmanTrack(); 33 | void kalmanPredict(); 34 | void kalmanUpdate(); 35 | void kalmanExtract(); 36 | void associateMeasurement(); 37 | void addIssues(int issue, float val); 38 | 39 | void setNumDrones(int num_drones); 40 | void updateA(float input_dt); 41 | 42 | void writeToFile(); 43 | 44 | void removeRow(Eigen::MatrixXf& matrix, unsigned int rowToRemove); 45 | void removeColumn(Eigen::MatrixXf& matrix, unsigned int rowToRemove); 46 | 47 | 48 | ros::Time startTime, endTime, processTime; 49 | 50 | geometry_msgs::PoseArray Z_current_k; 51 | int NUM_DRONES; 52 | int detected_size_k; 53 | 54 | float last_timestamp_synchronized; 55 | double last_timestamp_from_rostime; 56 | bool first_callback = true; 57 | 58 | int k_iteration = 0; 59 | int removed_columns = 0; 60 | 61 | //kalman filter variables 62 | Eigen::MatrixXf F; 63 | Eigen::MatrixXf A; 64 | Eigen::MatrixXf H; 65 | Eigen::MatrixXf Q; 66 | Eigen::MatrixXf R; 67 | Eigen::MatrixXf K; 68 | 69 | //phd variables 70 | 71 | float dt_cam = 0.125; //8hz 72 | float dt_imu = 0.01; //100hz 73 | 74 | Eigen::MatrixXf mk_minus_1; 75 | Eigen::MatrixXf wk_minus_1; 76 | Eigen::MatrixXf Pk_minus_1; 77 | Eigen::MatrixXf mk_k_minus_1; 78 | Eigen::MatrixXf wk_k_minus_1; 79 | Eigen::MatrixXf Pk_k_minus_1; 80 | Eigen::MatrixXf P_k_k; 81 | 82 | Eigen::MatrixXf S; 83 | 84 | Eigen::MatrixXf mk; 85 | Eigen::MatrixXf wk; 86 | Eigen::MatrixXf Pk; 87 | 88 | Eigen::MatrixXf Z_k; 89 | Eigen::MatrixXf Z_k_previous; 90 | Eigen::MatrixXf ang_vel_k; 91 | Eigen::MatrixXf X_k; 92 | Eigen::MatrixXf X_k_previous; 93 | 94 | Eigen::MatrixXf B; 95 | 96 | Eigen::MatrixXf Detections; 97 | Eigen::MatrixXf mahalDistance; 98 | 99 | sensor_msgs::ImagePtr image_msg; 100 | 101 | float cu, cv, f; 102 | float dt; 103 | 104 | const uint8_t n_state = 4; 105 | const uint8_t n_meas = 2; 106 | const uint8_t n_input = 3; 107 | 108 | uint8_t num_measurements_current; 109 | Eigen::MatrixXf::Index max_indices[100]; 110 | 111 | std::ofstream output_file; 112 | 113 | private: 114 | 115 | 116 | }; 117 | 118 | -------------------------------------------------------------------------------- /include/multi_robot_tracking/detection.h: -------------------------------------------------------------------------------- 1 | #ifndef _DETECTION_H_ 2 | #define _DETECTION_H_ 3 | 4 | #include 5 | #include 6 | 7 | 8 | class Detection 9 | { 10 | public: 11 | Detection(const float& _x, const float& _y, const int& _w, const int& _h) 12 | : m_x(_x), m_y(_y), m_w(_w), m_h(_h) 13 | { 14 | point = cv::Point2f(m_x, m_y); 15 | bbox = cv::Rect(m_x - (m_w >> 1), m_y - m_h, m_w, m_h); 16 | } 17 | const float x() const { return m_x; } 18 | const float y() const { return m_y; } 19 | const float w() const { return m_w; } 20 | const float h() const { return m_h; } 21 | const cv::Rect getRect() const { return bbox; } 22 | const cv::Point2f operator()() const 23 | { 24 | return point; 25 | } 26 | const Eigen::Vector2f getVect() const 27 | { 28 | Eigen::Vector2f p; 29 | p << x(), y(); 30 | return p; 31 | } 32 | Detection& operator=(const Detection& d_copy) 33 | { 34 | this->m_x = d_copy.x(); 35 | this->m_y = d_copy.y(); 36 | this->m_w = d_copy.w(); 37 | this->m_h = d_copy.h(); 38 | return *this; 39 | } 40 | private: 41 | float m_x, m_y; 42 | int m_w, m_h; 43 | cv::Point2f point; 44 | cv::Rect bbox; 45 | }; 46 | 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /include/multi_robot_tracking/hungarian_alg.h: -------------------------------------------------------------------------------- 1 | #ifndef _HUNGARIAN_ALG_H_ 2 | #define _HUNGARIAN_ALG_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | // http://community.topcoder.com/tc?module=Static&d1=tutorials&d2=hungarianAlgorithm 12 | typedef float track_t; 13 | typedef std::vector assignments_t; 14 | typedef std::vector distMatrix_t; 15 | 16 | class AssignmentProblemSolver 17 | { 18 | private: 19 | // -------------------------------------------------------------------------- 20 | // Computes the optimal assignment (minimum overall costs) using Munkres algorithm. 21 | // -------------------------------------------------------------------------- 22 | void assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns); 23 | void buildassignmentvector(assignments_t& assignment, bool *starMatrix, size_t nOfRows, size_t nOfColumns); 24 | void computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows); 25 | void step2a(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim); 26 | void step2b(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim); 27 | void step3(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim); 28 | void step4(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, size_t col); 29 | void step5(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim); 30 | // -------------------------------------------------------------------------- 31 | // Computes a suboptimal solution. Good for cases with many forbidden assignments. 32 | // -------------------------------------------------------------------------- 33 | void assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns); 34 | // -------------------------------------------------------------------------- 35 | // Computes a suboptimal solution. Good for cases with many forbidden assignments. 36 | // -------------------------------------------------------------------------- 37 | void assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns); 38 | 39 | public: 40 | enum TMethod 41 | { 42 | optimal, 43 | many_forbidden_assignments, 44 | without_forbidden_assignments 45 | }; 46 | 47 | void Solve(const std::vector distMatrixIn, uint nOfRows, uint nOfColumns, std::vector& assignment, TMethod Method = optimal); 48 | 49 | }; 50 | 51 | 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /include/multi_robot_tracking/kalman.h: -------------------------------------------------------------------------------- 1 | #ifndef _KALMAN_H_ 2 | #define _KALMAN_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | class Kalman 11 | { 12 | public: 13 | Kalman(const float& px, const float& py, const float& vx, const float& vy, TrackerParam params); 14 | void predict(const float dt, const Eigen::Vector3f omega); 15 | void update(const std::vector< Detection> detections, const std::vector beta, double beta_0); //Added by Max 16 | inline const Eigen::Matrix2f getS() const 17 | { 18 | return S; 19 | } 20 | Eigen::Vector2f get_z(){return z;} 21 | private: 22 | Eigen::MatrixXf C; 23 | Eigen::MatrixXf B; 24 | Eigen::Matrix2f R; //Proces measurement Covariance matrix 25 | Eigen::Vector2f T; //Proces measurement Covariance vector (accelerations) 26 | Eigen::Matrix2f S; 27 | Eigen::MatrixXf K; //Gain 28 | Eigen::Matrix4f P; //Covariance Matrix predicted error 29 | Eigen::Vector4f x; 30 | Eigen::Vector2f z; 31 | 32 | float f; 33 | float alpha; 34 | Eigen::Vector2f c; //principal point 35 | }; 36 | 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /include/multi_robot_tracking/track.h: -------------------------------------------------------------------------------- 1 | #ifndef _TRACK_H_ 2 | #define _TRACK_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | class Track 14 | { 15 | public: 16 | Track(const float& x, const float& y, const float& vx, const float& vy, TrackerParam params); 17 | 18 | void predict(float dt, Eigen::Vector3f omega); 19 | 20 | void setId(const int _id) 21 | { 22 | id = _id; 23 | } 24 | 25 | const int getId() const 26 | { 27 | return id; 28 | } 29 | const Eigen::Matrix2f S() const 30 | { 31 | return KF->getS(); 32 | } 33 | 34 | void update(const std::vector detections, std::vector beta, double beta_0); 35 | 36 | void increase_lifetime() 37 | { 38 | life_time++; 39 | } 40 | 41 | void print_life_time(){ROS_INFO("lifetime: %d", life_time);} 42 | 43 | void has_not_been_detected() 44 | { 45 | noDetections++; 46 | } 47 | 48 | void has_been_detected() 49 | { 50 | noDetections=0; 51 | } 52 | 53 | bool isDeprecated() 54 | { 55 | return noDetections >= maxMissedRate; 56 | } 57 | 58 | bool isValidated() 59 | { 60 | return life_time >= minAcceptanceRate; 61 | } 62 | 63 | Eigen::Vector2f get_z(){return KF->get_z();} 64 | 65 | cv::RotatedRect get_error_ellipse(double chisquare_val); 66 | private: 67 | int id; 68 | int maxMissedRate; 69 | int minAcceptanceRate; 70 | std::shared_ptr KF; 71 | int noDetections; 72 | int life_time; 73 | }; 74 | 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /include/multi_robot_tracking/tracker_param.h: -------------------------------------------------------------------------------- 1 | #ifndef _TRACKER_PARAM_H_ 2 | #define _TRACKER_PARAM_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace std; 11 | 12 | class TrackerParam 13 | { 14 | public: 15 | float pd = 0.952662721893; 16 | float gamma = 10; 17 | double false_measurements_density = 0.00000003034; 18 | double beta_0_threshold = 0.5; 19 | double alpha_0_threshold = 0.95; 20 | int max_missed_rate = 15; 21 | int min_acceptance_rate = 15; 22 | Eigen::Matrix2f R; 23 | //Eigen::Matrix2f T; 24 | Eigen::Vector2f T; 25 | Eigen::Matrix4f P_0; 26 | int nb_drones = 3; 27 | float assoc_cost = 50; 28 | 29 | float max_update_time_rate = 0.1; 30 | 31 | float focal_length = 564.26; 32 | float alpha_cam = 1.0; //pixel size ratio 33 | Eigen::Vector2f principal_point; 34 | 35 | string gt_topic_name; 36 | string source_odom_name; 37 | std::vector target_odom_names; 38 | 39 | string root_; 40 | string output_file_name_; 41 | 42 | 43 | 44 | TrackerParam(ros::NodeHandle nh_priv_); 45 | TrackerParam(){}; 46 | 47 | }; 48 | 49 | 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /launch/demo_left.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 45 | 46 | -------------------------------------------------------------------------------- /launch/demo_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /launch/demo_old.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /launch/demo_right.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 45 | 46 | -------------------------------------------------------------------------------- /nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | multi_robot_tracking 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | multi_robot_tracking 4 | 0.0.0 5 | The multi_robot_tracking package 6 | 7 | 8 | 9 | 10 | marklee 11 | BSD 12 | 13 | catkin 14 | 15 | 16 | darknet_ros_msgs 17 | image_transport 18 | nav_msgs 19 | cv_bridge 20 | geometry_msgs 21 | nodelet 22 | roscpp 23 | std_msgs 24 | tf 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /rviz/darknet_vis.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Image1 10 | - /Odometry1/Shape1 11 | - /Odometry2/Shape1 12 | - /Odometry3/Shape1 13 | - /Axes1 14 | - /Image2 15 | Splitter Ratio: 0.4941176474094391 16 | Tree Height: 191 17 | - Class: rviz/Selection 18 | Name: Selection 19 | - Class: rviz/Tool Properties 20 | Expanded: 21 | - /2D Pose Estimate1 22 | - /2D Nav Goal1 23 | - /Publish Point1 24 | Name: Tool Properties 25 | Splitter Ratio: 0.5886790156364441 26 | - Class: rviz/Views 27 | Expanded: 28 | - /Current View1 29 | Name: Views 30 | Splitter Ratio: 0.5 31 | - Class: rviz/Time 32 | Experimental: false 33 | Name: Time 34 | SyncMode: 0 35 | SyncSource: Image 36 | Preferences: 37 | PromptSaveOnExit: true 38 | Toolbars: 39 | toolButtonStyle: 2 40 | Visualization Manager: 41 | Class: "" 42 | Displays: 43 | - Alpha: 0.5 44 | Cell Size: 1 45 | Class: rviz/Grid 46 | Color: 160; 160; 164 47 | Enabled: true 48 | Line Style: 49 | Line Width: 0.029999999329447746 50 | Value: Lines 51 | Name: Grid 52 | Normal Cell Count: 0 53 | Offset: 54 | X: 0 55 | Y: 0 56 | Z: 0 57 | Plane: XY 58 | Plane Cell Count: 10 59 | Reference Frame: 60 | Value: true 61 | - Class: rviz/Image 62 | Enabled: true 63 | Image Topic: /hummingbird0/camera/rgb 64 | Max Value: 1 65 | Median window: 5 66 | Min Value: 0 67 | Name: Image 68 | Normalize Range: true 69 | Queue Size: 2 70 | Transport Hint: raw 71 | Unreliable: false 72 | Value: true 73 | - Angle Tolerance: 0.10000000149011612 74 | Class: rviz/Odometry 75 | Covariance: 76 | Orientation: 77 | Alpha: 0.5 78 | Color: 255; 255; 127 79 | Color Style: Unique 80 | Frame: Local 81 | Offset: 1 82 | Scale: 1 83 | Value: true 84 | Position: 85 | Alpha: 0.30000001192092896 86 | Color: 204; 51; 204 87 | Scale: 1 88 | Value: true 89 | Value: true 90 | Enabled: true 91 | Keep: 1 92 | Name: Odometry 93 | Position Tolerance: 0.10000000149011612 94 | Queue Size: 10 95 | Shape: 96 | Alpha: 1 97 | Axes Length: 0.5 98 | Axes Radius: 0.05000000074505806 99 | Color: 255; 25; 0 100 | Head Length: 0.30000001192092896 101 | Head Radius: 0.10000000149011612 102 | Shaft Length: 1 103 | Shaft Radius: 0.05000000074505806 104 | Value: Axes 105 | Topic: /vicon/DragonFly1/odom 106 | Unreliable: false 107 | Value: true 108 | - Angle Tolerance: 0.10000000149011612 109 | Class: rviz/Odometry 110 | Covariance: 111 | Orientation: 112 | Alpha: 0.5 113 | Color: 255; 255; 127 114 | Color Style: Unique 115 | Frame: Local 116 | Offset: 1 117 | Scale: 1 118 | Value: true 119 | Position: 120 | Alpha: 0.30000001192092896 121 | Color: 204; 51; 204 122 | Scale: 1 123 | Value: true 124 | Value: true 125 | Enabled: true 126 | Keep: 1 127 | Name: Odometry 128 | Position Tolerance: 0.10000000149011612 129 | Queue Size: 10 130 | Shape: 131 | Alpha: 1 132 | Axes Length: 0.5 133 | Axes Radius: 0.05000000074505806 134 | Color: 255; 25; 0 135 | Head Length: 0.30000001192092896 136 | Head Radius: 0.10000000149011612 137 | Shaft Length: 1 138 | Shaft Radius: 0.05000000074505806 139 | Value: Axes 140 | Topic: /vicon/DragonFly2/odom 141 | Unreliable: false 142 | Value: true 143 | - Angle Tolerance: 0.10000000149011612 144 | Class: rviz/Odometry 145 | Covariance: 146 | Orientation: 147 | Alpha: 0.5 148 | Color: 255; 255; 127 149 | Color Style: Unique 150 | Frame: Local 151 | Offset: 1 152 | Scale: 1 153 | Value: true 154 | Position: 155 | Alpha: 0.30000001192092896 156 | Color: 204; 51; 204 157 | Scale: 1 158 | Value: true 159 | Value: true 160 | Enabled: true 161 | Keep: 1 162 | Name: Odometry 163 | Position Tolerance: 0.10000000149011612 164 | Queue Size: 10 165 | Shape: 166 | Alpha: 1 167 | Axes Length: 0.5 168 | Axes Radius: 0.05000000074505806 169 | Color: 255; 25; 0 170 | Head Length: 0.30000001192092896 171 | Head Radius: 0.10000000149011612 172 | Shaft Length: 1 173 | Shaft Radius: 0.05000000074505806 174 | Value: Axes 175 | Topic: /vicon/DragonFly5/odom 176 | Unreliable: false 177 | Value: true 178 | - Angle Tolerance: 0.10000000149011612 179 | Class: rviz/Odometry 180 | Covariance: 181 | Orientation: 182 | Alpha: 0.5 183 | Color: 255; 255; 127 184 | Color Style: Unique 185 | Frame: Local 186 | Offset: 1 187 | Scale: 1 188 | Value: true 189 | Position: 190 | Alpha: 0.30000001192092896 191 | Color: 204; 51; 204 192 | Scale: 1 193 | Value: true 194 | Value: true 195 | Enabled: true 196 | Keep: 1 197 | Name: Odometry 198 | Position Tolerance: 0.10000000149011612 199 | Queue Size: 10 200 | Shape: 201 | Alpha: 1 202 | Axes Length: 1 203 | Axes Radius: 0.10000000149011612 204 | Color: 255; 25; 0 205 | Head Length: 0.30000001192092896 206 | Head Radius: 0.10000000149011612 207 | Shaft Length: 1 208 | Shaft Radius: 0.05000000074505806 209 | Value: Axes 210 | Topic: /vicon/TobiiGlasses/odom 211 | Unreliable: false 212 | Value: true 213 | - Alpha: 1 214 | Class: rviz/Axes 215 | Enabled: true 216 | Length: 0.5 217 | Name: Axes 218 | Radius: 0.019999999552965164 219 | Reference Frame: 220 | Value: true 221 | - Class: rviz/Image 222 | Enabled: true 223 | Image Topic: /tracked_image 224 | Max Value: 1 225 | Median window: 5 226 | Min Value: 0 227 | Name: Image 228 | Normalize Range: true 229 | Queue Size: 2 230 | Transport Hint: raw 231 | Unreliable: false 232 | Value: true 233 | Enabled: true 234 | Global Options: 235 | Background Color: 48; 48; 48 236 | Default Light: true 237 | Fixed Frame: mocap 238 | Frame Rate: 30 239 | Name: root 240 | Tools: 241 | - Class: rviz/Interact 242 | Hide Inactive Objects: true 243 | - Class: rviz/MoveCamera 244 | - Class: rviz/Select 245 | - Class: rviz/FocusCamera 246 | - Class: rviz/Measure 247 | - Class: rviz/SetInitialPose 248 | Theta std deviation: 0.2617993950843811 249 | Topic: /initialpose 250 | X std deviation: 0.5 251 | Y std deviation: 0.5 252 | - Class: rviz/SetGoal 253 | Topic: /move_base_simple/goal 254 | - Class: rviz/PublishPoint 255 | Single click: true 256 | Topic: /clicked_point 257 | Value: true 258 | Views: 259 | Current: 260 | Class: rviz/Orbit 261 | Distance: 10.568682670593262 262 | Enable Stereo Rendering: 263 | Stereo Eye Separation: 0.05999999865889549 264 | Stereo Focal Distance: 1 265 | Swap Stereo Eyes: false 266 | Value: false 267 | Field of View: 0.7853981852531433 268 | Focal Point: 269 | X: 0 270 | Y: 0 271 | Z: 0 272 | Focal Shape Fixed Size: true 273 | Focal Shape Size: 0.05000000074505806 274 | Invert Z Axis: false 275 | Name: Current View 276 | Near Clip Distance: 0.009999999776482582 277 | Pitch: 0.6297968626022339 278 | Target Frame: 279 | Yaw: 5.283600807189941 280 | Saved: ~ 281 | Window Geometry: 282 | Displays: 283 | collapsed: false 284 | Height: 895 285 | Hide Left Dock: false 286 | Hide Right Dock: true 287 | Image: 288 | collapsed: false 289 | QMainWindow State: 000000ff00000000fd0000000400000000000001e2000002e1fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000000fc000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000013f000001040000001600fffffffb0000000a0049006d0061006700650100000249000000d50000001600ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000030b000002e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 290 | Selection: 291 | collapsed: false 292 | Time: 293 | collapsed: false 294 | Tool Properties: 295 | collapsed: false 296 | Views: 297 | collapsed: true 298 | Width: 1267 299 | X: 2200 300 | Y: 87 301 | -------------------------------------------------------------------------------- /scripts/flightmare_test.py: -------------------------------------------------------------------------------- 1 | #! /bin/env python3 2 | 3 | import time 4 | 5 | import rospy 6 | 7 | from geometry_msgs.msg import PoseStamped as ps 8 | from std_msgs.msg import Empty as e 9 | from std_msgs.msg import Bool as bo 10 | 11 | class flightmareTest: 12 | def __init__(self): 13 | self.num_drones = 3 14 | 15 | self.setpoints = [ [0, 0, 2], [4, 1, 2], [2, -1, 2], 16 | [0, 0, 2], [4, -1, 2], [2, 1 ,2], 17 | [0, 0, 2], [4, 0, 3], [2, 0, 1], 18 | [0, 0, 2], [4, 0, 1], [2, 0, 3], 19 | [4, 4, 2], [8, 4, 3], [6, 4, 1]] 20 | 21 | self.sleep_times = [14, 10, 10, 10, 10, 10, 10 ,10 ,10 ,10 ,10 ,10 ,10 ,10 ,10] 22 | 23 | self.position_publishers = [] 24 | self.takeoff_publishers = [] 25 | self.land_publishers = [] 26 | self.arm_publishers = [] 27 | 28 | self.drone_namespace = "/hummingbird" 29 | self.position_topic = "/autopilot/pose_command" 30 | self.takeoff_topic = "/autopilot/start" 31 | self.land_topic = "/autopilot/land" 32 | self.arm_topic = "/bridge/arm" 33 | 34 | rospy.loginfo("Starting") 35 | 36 | for i in range(self.num_drones): 37 | self.position_publishers.append(rospy.Publisher(self.drone_namespace+str(i)+self.position_topic, ps, queue_size=1)) 38 | self.takeoff_publishers.append(rospy.Publisher(self.drone_namespace+str(i)+self.takeoff_topic, e, queue_size=1)) 39 | self.land_publishers.append(rospy.Publisher(self.drone_namespace+str(i)+self.land_topic, e, queue_size=1)) 40 | self.arm_publishers.append(rospy.Publisher(self.drone_namespace+str(i)+self.arm_topic, bo, queue_size=1)) 41 | 42 | time.sleep(3) 43 | 44 | true_msg = bo() 45 | true_msg.data = True 46 | 47 | rospy.loginfo("Arming") 48 | 49 | for i in range(self.num_drones): 50 | self.arm_publishers[i].publish(true_msg) 51 | 52 | rospy.loginfo("Takeoff command sent") 53 | time.sleep(1) 54 | for i in range(self.num_drones): 55 | self.takeoff_publishers[i].publish(e()) 56 | time.sleep(5) 57 | 58 | 59 | rospy.loginfo("Sending waypoints") 60 | for i in range(int(len(self.setpoints)/3)): 61 | for ii in range(self.num_drones): 62 | pose_command = ps() 63 | pose_command.header.stamp = rospy.Time.now() 64 | pose_command.pose.position.x = self.setpoints[(i*3)+ii][0] 65 | pose_command.pose.position.y = self.setpoints[(i*3)+ii][1] 66 | pose_command.pose.position.z = self.setpoints[(i*3)+ii][2] 67 | self.position_publishers[ii].publish(pose_command) 68 | rospy.loginfo("sent command " + str(i)) 69 | time.sleep(self.sleep_times[i]) 70 | if(rospy.is_shutdown()): 71 | break 72 | 73 | 74 | rospy.init_node("flightmare_controller") 75 | ft = flightmareTest() 76 | -------------------------------------------------------------------------------- /scripts/rmse_calc.py: -------------------------------------------------------------------------------- 1 | #! /bin/env python3 2 | 3 | import csv 4 | import math 5 | 6 | csv_fh = open("/home/vivek/ws/NYU/ARPL/RAL2021/data/simple_kalman_data_RDp1.csv") 7 | csv_reader = csv.reader(csv_fh) 8 | 9 | err_x1 = 0.0 10 | err_x2 = 0.0 11 | 12 | err = 0.001 13 | 14 | i = 0 15 | 16 | for row in csv_reader: 17 | i += 1 18 | print(i) 19 | print(row[0]) 20 | m1x = float(row[0]) 21 | m1y = float(row[1]) 22 | m2x = float(row[2]) 23 | m2y = float(row[3]) 24 | t1x = float(row[4]) 25 | t1y = float(row[5]) 26 | t2x = float(row[6]) 27 | t2y = float(row[7]) 28 | 29 | if(not m1x == 0): 30 | errA1x = math.fabs(m1x - t1x) 31 | errA2x = math.fabs(m1x - t2x) 32 | else: 33 | errA1x = 1000.0 34 | errA2x = 1000.0 35 | if(not m2x == 0): 36 | errB1x = math.fabs(m2x - t1x) 37 | errB2x = math.fabs(m2x - t2x) 38 | else: 39 | errABx = 1000.0 40 | errABx = 1000.0 41 | 42 | 43 | 44 | 45 | if(errA1x > errB1x): 46 | if(errB1x < 700): 47 | err_x1 += (errB1x*errB1x) 48 | else: 49 | if(errA1x < 700): 50 | err_x1 += (errA1x*errA1x) 51 | 52 | if(errA2x > errB2x): 53 | if(errB2x < 700): 54 | err_x2 += (errB2x*errB2x) 55 | else: 56 | if(errA2x < 700): 57 | err_x2 += (errA2x*errA2x) 58 | 59 | print(math.sqrt(err_x1/i)) 60 | print(math.sqrt(err_x2/i)) 61 | 62 | 63 | -------------------------------------------------------------------------------- /scripts/sample_data.py: -------------------------------------------------------------------------------- 1 | #! /bin/env python3 2 | 3 | import rospy 4 | 5 | from geometry_msgs.msg import PoseArray 6 | from geometry_msgs.msg import Pose 7 | 8 | 9 | class poseArrayPubber: 10 | def __init__(self): 11 | pa = PoseArray() 12 | 13 | paPub = rospy.Publisher("/hummingbird0/track/bounding_box", PoseArray, queue_size=1) 14 | nSamples = 2 15 | for i in range(nSamples): 16 | p = Pose() 17 | p.position.x = 10*(i+1) 18 | p.position.y = 10*(i+1) 19 | pa.poses.append(p) 20 | 21 | print(pa) 22 | 23 | rate = rospy.Rate(8) 24 | for i in range(100): 25 | pa.header.stamp = rospy.Time.now() 26 | pa.header.seq += 1 27 | paPub.publish(pa) 28 | rate.sleep() 29 | 30 | rospy.init_node("pa_pubber") 31 | pap = poseArrayPubber() 32 | 33 | -------------------------------------------------------------------------------- /src/SimpleKalman.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "multi_robot_tracking/SimpleKalman.h" 4 | #include 5 | 6 | 7 | using namespace std; 8 | using namespace std::chrono; 9 | 10 | KalmanFilter::KalmanFilter() 11 | { 12 | output_file.open("/home/vivek/ws/NYU/ARPL/RAL2021/data/simple_kalman_data_test.csv"); 13 | output_file << "Mx,y,x,y,x,y,x,y,Tx,y,x,y,x,y" << endl; 14 | } 15 | 16 | void KalmanFilter::kalmanTrack() 17 | { 18 | auto time_start = high_resolution_clock::now(); 19 | startTime = ros::Time::now(); 20 | k_iteration = k_iteration + 1; 21 | ROS_INFO("iter: %d",k_iteration); 22 | 23 | kalmanPredict(); // ToDo: Can be asynch ??? 24 | auto time_endP = high_resolution_clock::now(); 25 | auto durationP = duration_cast(time_endP - time_start); 26 | ROS_ERROR_STREAM("Time taken by function predict: " << durationP.count() << " microseconds" << endl); 27 | 28 | addIssues(0, 0.0); 29 | auto time_endI = high_resolution_clock::now(); 30 | auto durationI = duration_cast(time_endI - time_endP); 31 | ROS_ERROR_STREAM("Time taken by function Issue: " << durationI.count() << " microseconds" << endl); 32 | 33 | associateMeasurement(); 34 | auto time_endA = high_resolution_clock::now(); 35 | auto durationA = duration_cast(time_endA - time_endI); 36 | ROS_ERROR_STREAM("Time taken by function Associate: " << durationA.count() << " microseconds" << endl); 37 | 38 | kalmanUpdate(); 39 | auto time_endU = high_resolution_clock::now(); 40 | auto durationU = duration_cast(time_endU - time_endA); 41 | ROS_ERROR_STREAM("Time taken by function Update: " << durationU.count() << " microseconds" << endl); 42 | 43 | kalmanExtract(); 44 | auto time_endE = high_resolution_clock::now(); 45 | auto durationE = duration_cast(time_endE - time_endU); 46 | ROS_ERROR_STREAM("Time taken by function Extract: " << durationE.count() << " microseconds" << endl); 47 | 48 | auto time_end = high_resolution_clock::now(); 49 | auto duration = duration_cast(time_end - time_start); 50 | ROS_ERROR_STREAM("Time taken by function: " << duration.count() << " microseconds" << endl); 51 | 52 | if(1) 53 | writeToFile(); 54 | 55 | 56 | endTime = ros::Time::now(); 57 | ROS_WARN("end of track iteration"); 58 | Z_k.conservativeResize(n_meas, NUM_DRONES); 59 | } 60 | 61 | 62 | void KalmanFilter::initializeMatrix(float cam_cu, float cam_cv, float cam_f, float meas_dt) 63 | { 64 | ROS_INFO("first initialize matrix"); 65 | //initialize 66 | Z_k = Eigen::MatrixXf::Zero(n_meas,NUM_DRONES); 67 | Z_k_previous = Eigen::MatrixXf::Zero(n_meas,NUM_DRONES); 68 | ang_vel_k = Eigen::MatrixXf::Zero(n_input,1); 69 | 70 | mk_minus_1 = Eigen::MatrixXf::Zero(n_state,NUM_DRONES); 71 | wk_minus_1 = Eigen::MatrixXf::Zero(1,NUM_DRONES); 72 | Pk_minus_1 = Eigen::MatrixXf::Zero(n_state,NUM_DRONES*4); 73 | 74 | mk = Eigen::MatrixXf::Zero(n_state,NUM_DRONES+NUM_DRONES*NUM_DRONES); 75 | wk = Eigen::MatrixXf::Zero(1,NUM_DRONES+NUM_DRONES*NUM_DRONES); 76 | Pk = Eigen::MatrixXf::Zero(n_state,n_state*(NUM_DRONES+NUM_DRONES*NUM_DRONES) ); 77 | 78 | mk_k_minus_1 = Eigen::MatrixXf::Zero(n_state,NUM_DRONES); 79 | wk_k_minus_1 = Eigen::MatrixXf::Zero(1,NUM_DRONES); 80 | Pk_k_minus_1 = Eigen::MatrixXf::Zero(n_state,n_state*NUM_DRONES); 81 | P_k_k = Eigen::MatrixXf::Zero(n_state,n_state*NUM_DRONES); 82 | S = Eigen::MatrixXf::Zero(n_meas,n_meas*NUM_DRONES); 83 | 84 | F = Eigen::MatrixXf::Zero(n_state,n_state); 85 | A = Eigen::MatrixXf::Zero(n_state,n_state); 86 | H = Eigen::MatrixXf::Zero(n_meas,n_state); 87 | Q = Eigen::MatrixXf::Zero(n_state,n_state); 88 | R = Eigen::MatrixXf::Zero(n_meas, n_meas); 89 | K = Eigen::MatrixXf::Zero(n_state,n_meas*NUM_DRONES); 90 | 91 | X_k = Eigen::MatrixXf::Zero(n_state,NUM_DRONES); 92 | X_k_previous = Eigen::MatrixXf::Zero(n_state,NUM_DRONES); 93 | 94 | B = Eigen::MatrixXf::Zero(n_state,n_input*NUM_DRONES); 95 | 96 | Detections = Eigen::MatrixXf::Zero(4,NUM_DRONES); 97 | 98 | 99 | cu = cam_cu; 100 | cv = cam_cv; 101 | f = cam_f; 102 | dt = 0.01; 103 | } 104 | 105 | void KalmanFilter::initialize(float q_pos, float q_vel, float r_meas, float p_pos_init, float p_vel_init, 106 | float prune_weight_threshold, float prune_mahalanobis_threshold, float extract_weight_threshold) 107 | { 108 | Eigen::MatrixXf P_k_init; 109 | P_k_init = Eigen::MatrixXf(n_state,n_state); 110 | P_k_init << 111 | p_pos_init, 0, 0, 0, 112 | 0, p_vel_init, 0, 0, 113 | 0, 0, p_pos_init, 0, 114 | 0, 0, 0, p_vel_init; 115 | P_k_init = P_k_init * 1; 116 | 117 | 118 | for(int i = 0; i < Z_k.cols(); i ++) 119 | { 120 | //store Z into mk (x,y) 121 | ROS_INFO_STREAM("ZK: \n" << Z_k << endl); 122 | // mk_minus_1.block(0, i, n_state, 1) = H.transpose()*Z_k.block(0,i, n_meas,1); 123 | mk_minus_1.block(0, i, n_state, 1) << Z_k.block(0, i, 1, 1), 0, Z_k.block(1, i, 1, 1), 0; 124 | ROS_INFO_STREAM("mk_minus_1: \n" << mk_minus_1 << endl); 125 | //store pre-determined weight into wk (from matlab) 126 | wk_minus_1(i) = .0016; 127 | //store pre-determined weight into Pk (from paper) 128 | Pk_minus_1.block(0,i*4, n_state,n_state) = P_k_init; 129 | } 130 | 131 | A << 1,dt_cam,0,0, 132 | 0,1,0,0, 133 | 0,0,1,dt_cam, 134 | 0,0,0,1; 135 | 136 | //Measurement Matrix 137 | H << 1, 0, 0 , 0, 138 | 0, 0, 1, 0; 139 | 140 | //Process noise covariance, given in Vo&Ma. 141 | Q << q_pos, 0, 0, 0, 142 | 0, q_vel, 0, 0, 143 | 0, 0, q_pos, 0, 144 | 0, 0, 0, q_vel; 145 | 146 | //Measurement Noise 147 | R << r_meas, 0, 148 | 0, r_meas; 149 | 150 | } 151 | 152 | /* 153 | * Prediction step is done async. The prediction is called whenever we get an IMU message. 154 | * The update step is called once we get a measurement from the network 155 | */ 156 | void KalmanFilter::kalmanPredict() 157 | { 158 | ROS_INFO("======= 0. asynch predict ======= \n"); 159 | //update A 160 | updateA(dt_cam); 161 | Eigen::MatrixXf Bu_temp = Eigen::MatrixXf::Zero(n_state,n_meas); 162 | F = Eigen::MatrixXf(n_state,n_state); 163 | float omega_x = ang_vel_k(0); 164 | float omega_y = ang_vel_k(1); 165 | float omega_z = ang_vel_k(2); 166 | float pu = 0; 167 | float pv = 0; 168 | 169 | Eigen::MatrixXf P_temp; 170 | P_temp = Eigen::MatrixXf(n_state,n_state); 171 | for (int i = 0; i < NUM_DRONES; i++) 172 | { 173 | Bu_temp = B.block(0,n_input*i, n_state,n_input) * ang_vel_k; 174 | ROS_INFO_STREAM("Bu:\n" << Bu_temp << endl); 175 | mk_minus_1.block(0,i, n_state,1) = A * mk_minus_1.block(0,i, n_state,1) + Bu_temp; 176 | 177 | P_temp = Pk_minus_1.block(0,n_state*i, n_state,n_state); 178 | 179 | pu = X_k(0,i); 180 | pv = X_k(2,i); 181 | F(0,0) = (dt_imu*omega_y*(2*cu - 2*pu))/f - (dt_imu*omega_x*(cv - pv))/f + 1; F(0,1) = dt; F(0,2) = dt_imu*omega_z - (dt_imu*omega_x*(cu - pu))/f; F(0,3) = 0; 182 | F(1,0) = 0; F(1,1) = 1; F(1,2) = 0; F(1,3) = 0; 183 | F(2,0) = (dt_imu*omega_y*(cv - pv))/f - dt_imu*omega_z; F(2,1) = 0; F(2,2) = (dt_imu*omega_y*(cu - pu))/f - (dt_imu*omega_x*(2*cv - 2*pv))/f + 1; F(2,3) = dt; 184 | F(3,0) = 0; F(3,1) = 0; F(3,2) = 0; F(3,3) = 1; 185 | P_temp = Q + F* P_temp * F.transpose(); 186 | Pk_minus_1.block(0,n_state*i, n_state,n_state) = P_temp; 187 | } 188 | 189 | ROS_INFO_STREAM("WK|K-1:\n" << wk_minus_1 << endl); 190 | ROS_INFO_STREAM("mK|K-1:\n" << mk_minus_1 << endl); 191 | ROS_INFO_STREAM("PK|K-1:\n" << Pk_minus_1 << endl); 192 | } 193 | 194 | void KalmanFilter::associateMeasurement() 195 | { 196 | ROS_INFO("============ 2. Associate ============= "); 197 | wk = Eigen::MatrixXf::Zero(NUM_DRONES, detected_size_k); 198 | if(detected_size_k==0) 199 | return; 200 | Eigen::MatrixXf thisZ, meanDelta_pdf, cov_pdf, cov_pdf_inv; 201 | 202 | thisZ = Eigen::MatrixXf(n_meas,1); 203 | meanDelta_pdf = Eigen::MatrixXf(n_meas,1); 204 | cov_pdf = Eigen::MatrixXf(n_meas,n_meas); 205 | cov_pdf_inv = Eigen::MatrixXf(n_meas,n_meas); 206 | 207 | Eigen::MatrixXf PHt, HPHt, identity4; 208 | PHt = Eigen::MatrixXf(n_state,n_meas); 209 | HPHt = Eigen::MatrixXf(n_meas,n_meas); 210 | identity4 = Eigen::MatrixXf::Identity(n_meas,n_meas); 211 | 212 | 213 | Eigen::MatrixXf w_new_exponent = Eigen::MatrixXf(1,1); 214 | 215 | for(int i=0; i max_val(i)) 252 | { 253 | ROS_WARN_STREAM("Found a bad index in associate"); 254 | ROS_INFO_STREAM("Found col max at index" << col_max_index << " with value " << col_max); 255 | wk(i, max_indices[i]) = -20; 256 | i--; 257 | ROS_INFO_STREAM("wk - \n" << wk); 258 | continue; 259 | } 260 | else 261 | { 262 | for(int j =0; j -1) 267 | wk(j, max_indices[i]) = -100.0; 268 | } 269 | } 270 | ROS_INFO_STREAM("max_indices - \n" << max_indices[i]); 271 | ROS_INFO_STREAM("wk - \n" << wk); 272 | } 273 | 274 | ROS_INFO_STREAM("max_val - \n" << max_val); 275 | 276 | 277 | } 278 | 279 | void KalmanFilter::kalmanUpdate() 280 | { 281 | ROS_INFO("============ 3. Update ============= "); 282 | if(detected_size_k==0) 283 | return; 284 | Eigen::MatrixXf thisZ, meanDelta_pdf, cov_pdf; 285 | 286 | thisZ = Eigen::MatrixXf(n_meas,1); 287 | meanDelta_pdf = Eigen::MatrixXf(n_meas,1); 288 | cov_pdf = Eigen::MatrixXf(n_meas,n_meas); 289 | 290 | Eigen::MatrixXf PHt, HPHt, identity4, identity2, thisPk_minus_1; 291 | PHt = Eigen::MatrixXf(n_state,n_meas); 292 | HPHt = Eigen::MatrixXf(n_meas,n_meas); 293 | identity4 = Eigen::MatrixXf::Identity(n_state,n_state); 294 | identity2 = Eigen::MatrixXf::Identity(n_meas,n_meas); 295 | thisPk_minus_1 = Eigen::MatrixXf(n_meas,n_meas); 296 | 297 | for(int i=0; i -1 && max_indices[i] < detected_size_k) 300 | { 301 | thisZ = Z_k.block(0,max_indices[i], n_meas,1); 302 | thisPk_minus_1 = Pk_minus_1.block(0,n_state*i, n_state,n_state); 303 | ROS_INFO_STREAM("thisZ \n" << thisZ); 304 | if(thisZ(0) == 0 && thisZ(1) == 0) 305 | { 306 | mk.block(0,i, n_state, 1) = mk_minus_1.block(0,i, n_state,1); 307 | Pk.block(0,n_state*i, n_state,n_state) = thisPk_minus_1; 308 | continue; 309 | } 310 | meanDelta_pdf = thisZ - H*mk_minus_1.block(0,i, n_state,1); 311 | ROS_INFO_STREAM("meanDelta_pdf \n" << meanDelta_pdf); 312 | 313 | PHt = thisPk_minus_1 * H.transpose(); 314 | ROS_INFO_STREAM("PHt \n" << PHt); 315 | HPHt = H*PHt; 316 | 317 | S.block(0,n_meas*i, n_meas,n_meas) = HPHt + R; 318 | ROS_INFO_STREAM("S \n" << S); 319 | K.block(0,n_meas*i, n_state,n_meas) = PHt * (HPHt + R).llt().solve(identity2); 320 | ROS_INFO_STREAM("K \n" << K); 321 | 322 | mk.block(0,i, n_state, 1) = mk_minus_1.block(0,i, n_state,1) + K.block(0,n_meas*i, n_state,n_meas)*meanDelta_pdf; 323 | ROS_INFO_STREAM("mk \n" << mk); 324 | Pk.block(0,n_state*i, n_state,n_state) = (identity4 - K.block(0,n_meas*i, n_state,n_meas)*H)*thisPk_minus_1; 325 | ROS_INFO_STREAM("pk \n" << Pk); 326 | } 327 | else 328 | { 329 | mk.block(0,i, n_state, 1) = mk_minus_1.block(0,i, n_state,1); 330 | Pk.block(0,n_state*i, n_state,n_state) = Pk_minus_1.block(0,n_state*i, n_state,n_state); 331 | } 332 | } 333 | } 334 | 335 | 336 | void KalmanFilter::kalmanExtract() 337 | { 338 | 339 | ROS_INFO("============ 5. extract ============= "); 340 | if(detected_size_k==0) 341 | return; 342 | Eigen::MatrixXf velocity, position; 343 | velocity = Eigen::MatrixXf(2,1); 344 | position = Eigen::MatrixXf(2,1); 345 | float gain_fine_tuned = 2.0; 346 | 347 | ROS_INFO_STREAM("DT Cam: " << dt_cam << "\n"); 348 | //update state for next iterations 349 | X_k = mk_minus_1; 350 | if(k_iteration > 3) 351 | { 352 | //update state for next iterations 353 | mk_minus_1 = mk; 354 | Pk_minus_1 = Pk.cwiseAbs(); 355 | ROS_INFO_STREAM("mK in extract: \n" << mk_minus_1); 356 | ROS_INFO_STREAM("X_k in extract: \n" << X_k); 357 | ROS_INFO_STREAM("PK in extract: \n" << Pk_minus_1); 358 | } 359 | if (k_iteration > 3) 360 | { 361 | for (int i = 0; i < NUM_DRONES; i++) 362 | { 363 | position.block<1, 1>(0, 0) = (X_k.block<1,1>(0,i) - X_k_previous.block<1,1>(0,i)); 364 | position.block<1, 1>(1, 0) = (X_k.block<1,1>(2,i) - X_k_previous.block<1,1>(2,i)); 365 | velocity = position/ (dt_cam*gain_fine_tuned) ; 366 | mk_minus_1.block<1,1>(1,i) = velocity.block<1,1>(0,0); 367 | mk_minus_1.block<1,1>(3,i) = velocity.block<1,1>(1,0); 368 | ROS_INFO_STREAM("--- position: " << endl << position << endl); 369 | ROS_INFO_STREAM("--- dt_cam: " << endl << dt_cam << endl); 370 | ROS_INFO_STREAM("--- dt: " << endl << dt << endl); 371 | ROS_INFO_STREAM("--- velocity: " << endl << velocity << endl); 372 | } 373 | 374 | } 375 | X_k_previous = X_k; 376 | ROS_INFO_STREAM("X_k_previous:\n" << X_k_previous); 377 | 378 | ROS_INFO_STREAM("Z_k:\n" << Z_k); 379 | } 380 | 381 | void KalmanFilter::updateA(float input_dt) 382 | { 383 | A << 1,input_dt,0,0, 384 | 0,1,0,0, 385 | 0,0,1,input_dt, 386 | 0,0,0,1; 387 | } 388 | 389 | void KalmanFilter::setNumDrones(int num_drones_in) 390 | { 391 | NUM_DRONES = num_drones_in; 392 | } 393 | 394 | void KalmanFilter::addIssues(int issue_id, float value) 395 | { 396 | ROS_INFO("======= 1. Add Issue ======= \n"); 397 | if(issue_id == 1) // Gaussian noise 398 | { 399 | float measurement_covariance = 15; 400 | float noise_scalar = value; 401 | for(int i=0; i detection_probability) 430 | { 431 | // removeColumn(Z_k, i); 432 | // detected_size_k --; 433 | // i --; 434 | // removed_columns ++; 435 | Z_k(0, i) = 0.0; 436 | Z_k(1, i) = 0.0; 437 | } 438 | } 439 | ROS_INFO_STREAM("FN New ZK:\n" << Z_k); 440 | } 441 | 442 | } 443 | 444 | void KalmanFilter::removeRow(Eigen::MatrixXf& matrix, unsigned int rowToRemove) 445 | { 446 | unsigned int numRows = matrix.rows()-1; 447 | unsigned int numCols = matrix.cols(); 448 | 449 | if( rowToRemove < numRows ) 450 | matrix.block(rowToRemove,0,numRows-rowToRemove,numCols) = matrix.block(rowToRemove+1,0,numRows-rowToRemove,numCols); 451 | 452 | matrix.conservativeResize(numRows,numCols); 453 | } 454 | 455 | void KalmanFilter::removeColumn(Eigen::MatrixXf& matrix, unsigned int colToRemove) 456 | { 457 | unsigned int numRows = matrix.rows(); 458 | unsigned int numCols = matrix.cols()-1; 459 | 460 | if( colToRemove < numCols ) 461 | matrix.block(0,colToRemove,numRows,numCols-colToRemove) = matrix.block(0,colToRemove+1,numRows,numCols-colToRemove); 462 | 463 | matrix.conservativeResize(numRows,numCols); 464 | } 465 | 466 | void KalmanFilter::writeToFile() 467 | { 468 | for(int i=0; i 2 | #include 3 | 4 | 5 | void AssignmentProblemSolver::Solve( 6 | const std::vector distMatrixIn, 7 | uint nOfRows, 8 | uint nOfColumns, 9 | std::vector& assignment, 10 | TMethod Method 11 | ) 12 | { 13 | assignment.resize(nOfRows, -1); 14 | 15 | track_t cost = 0; 16 | 17 | switch (Method) 18 | { 19 | case optimal: 20 | assignmentoptimal(assignment, cost, distMatrixIn, nOfRows, nOfColumns); 21 | break; 22 | 23 | case many_forbidden_assignments: 24 | assignmentsuboptimal1(assignment, cost, distMatrixIn, nOfRows, nOfColumns); 25 | break; 26 | 27 | case without_forbidden_assignments: 28 | assignmentsuboptimal2(assignment, cost, distMatrixIn, nOfRows, nOfColumns); 29 | break; 30 | } 31 | 32 | return; 33 | } 34 | // -------------------------------------------------------------------------- 35 | // Computes the optimal assignment (minimum overall costs) using Munkres algorithm. 36 | // -------------------------------------------------------------------------- 37 | void AssignmentProblemSolver::assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns) 38 | { 39 | // Generate distance cv::Matrix 40 | // and check cv::Matrix elements positiveness :) 41 | 42 | // Total elements number 43 | size_t nOfElements = nOfRows * nOfColumns; 44 | // Memory allocation 45 | track_t* distMatrix = (track_t *)malloc(nOfElements * sizeof(track_t)); 46 | // Pointer to last element 47 | track_t* distMatrixEnd = distMatrix + nOfElements; 48 | 49 | for (size_t row = 0; row < nOfElements; row++) 50 | { 51 | track_t value = distMatrixIn[row]; 52 | assert(value >= 0); 53 | distMatrix[row] = value; 54 | } 55 | 56 | // Memory allocation 57 | bool* coveredColumns = (bool*)calloc(nOfColumns, sizeof(bool)); 58 | bool* coveredRows = (bool*)calloc(nOfRows, sizeof(bool)); 59 | bool* starMatrix = (bool*)calloc(nOfElements, sizeof(bool)); 60 | bool* primeMatrix = (bool*)calloc(nOfElements, sizeof(bool)); 61 | bool* newStarMatrix = (bool*)calloc(nOfElements, sizeof(bool)); /* used in step4 */ 62 | 63 | /* preliminary steps */ 64 | if (nOfRows <= nOfColumns) 65 | { 66 | for (size_t row = 0; row < nOfRows; row++) 67 | { 68 | /* find the smallest element in the row */ 69 | track_t* distMatrixTemp = distMatrix + row; 70 | track_t minValue = *distMatrixTemp; 71 | distMatrixTemp += nOfRows; 72 | while (distMatrixTemp < distMatrixEnd) 73 | { 74 | track_t value = *distMatrixTemp; 75 | if (value < minValue) 76 | { 77 | minValue = value; 78 | } 79 | distMatrixTemp += nOfRows; 80 | } 81 | /* subtract the smallest element from each element of the row */ 82 | distMatrixTemp = distMatrix + row; 83 | while (distMatrixTemp < distMatrixEnd) 84 | { 85 | *distMatrixTemp -= minValue; 86 | distMatrixTemp += nOfRows; 87 | } 88 | } 89 | /* Steps 1 and 2a */ 90 | for (size_t row = 0; row < nOfRows; row++) 91 | { 92 | for (size_t col = 0; col < nOfColumns; col++) 93 | { 94 | if (distMatrix[row + nOfRows*col] == 0) 95 | { 96 | if (!coveredColumns[col]) 97 | { 98 | starMatrix[row + nOfRows * col] = true; 99 | coveredColumns[col] = true; 100 | break; 101 | } 102 | } 103 | } 104 | } 105 | } 106 | else /* if(nOfRows > nOfColumns) */ 107 | { 108 | for (size_t col = 0; col < nOfColumns; col++) 109 | { 110 | /* find the smallest element in the column */ 111 | track_t* distMatrixTemp = distMatrix + nOfRows*col; 112 | track_t* columnEnd = distMatrixTemp + nOfRows; 113 | track_t minValue = *distMatrixTemp++; 114 | while (distMatrixTemp < columnEnd) 115 | { 116 | track_t value = *distMatrixTemp++; 117 | if (value < minValue) 118 | { 119 | minValue = value; 120 | } 121 | } 122 | /* subtract the smallest element from each element of the column */ 123 | distMatrixTemp = distMatrix + nOfRows*col; 124 | while (distMatrixTemp < columnEnd) 125 | { 126 | *distMatrixTemp++ -= minValue; 127 | } 128 | } 129 | /* Steps 1 and 2a */ 130 | for (size_t col = 0; col < nOfColumns; col++) 131 | { 132 | for (size_t row = 0; row < nOfRows; row++) 133 | { 134 | if (distMatrix[row + nOfRows*col] == 0) 135 | { 136 | if (!coveredRows[row]) 137 | { 138 | starMatrix[row + nOfRows*col] = true; 139 | coveredColumns[col] = true; 140 | coveredRows[row] = true; 141 | break; 142 | } 143 | } 144 | } 145 | } 146 | 147 | for (size_t row = 0; row < nOfRows; row++) 148 | { 149 | coveredRows[row] = false; 150 | } 151 | } 152 | /* move to step 2b */ 153 | step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, (nOfRows <= nOfColumns) ? nOfRows : nOfColumns); 154 | /* compute cost and remove invalid assignments */ 155 | computeassignmentcost(assignment, cost, distMatrixIn, nOfRows); 156 | /* free allocated memory */ 157 | free(distMatrix); 158 | free(coveredColumns); 159 | free(coveredRows); 160 | free(starMatrix); 161 | free(primeMatrix); 162 | free(newStarMatrix); 163 | return; 164 | } 165 | // -------------------------------------------------------------------------- 166 | // 167 | // -------------------------------------------------------------------------- 168 | void AssignmentProblemSolver::buildassignmentvector(assignments_t& assignment, bool *starMatrix, size_t nOfRows, size_t nOfColumns) 169 | { 170 | for (size_t row = 0; row < nOfRows; row++) 171 | { 172 | for (size_t col = 0; col < nOfColumns; col++) 173 | { 174 | if (starMatrix[row + nOfRows * col]) 175 | { 176 | assignment[row] = static_cast(col); 177 | break; 178 | } 179 | } 180 | } 181 | } 182 | // -------------------------------------------------------------------------- 183 | // 184 | // -------------------------------------------------------------------------- 185 | void AssignmentProblemSolver::computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows) 186 | { 187 | for (size_t row = 0; row < nOfRows; row++) 188 | { 189 | const int col = assignment[row]; 190 | if (col >= 0) 191 | { 192 | cost += distMatrixIn[row + nOfRows * col]; 193 | } 194 | } 195 | } 196 | 197 | // -------------------------------------------------------------------------- 198 | // 199 | // -------------------------------------------------------------------------- 200 | void AssignmentProblemSolver::step2a(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim) 201 | { 202 | bool *starMatrixTemp, *columnEnd; 203 | /* cover every column containing a starred zero */ 204 | for (size_t col = 0; col < nOfColumns; col++) 205 | { 206 | starMatrixTemp = starMatrix + nOfRows * col; 207 | columnEnd = starMatrixTemp + nOfRows; 208 | while (starMatrixTemp < columnEnd) 209 | { 210 | if (*starMatrixTemp++) 211 | { 212 | coveredColumns[col] = true; 213 | break; 214 | } 215 | } 216 | } 217 | /* move to step 3 */ 218 | step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 219 | } 220 | 221 | // -------------------------------------------------------------------------- 222 | // 223 | // -------------------------------------------------------------------------- 224 | void AssignmentProblemSolver::step2b(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim) 225 | { 226 | /* count covered columns */ 227 | size_t nOfCoveredColumns = 0; 228 | for (size_t col = 0; col < nOfColumns; col++) 229 | { 230 | if (coveredColumns[col]) 231 | { 232 | nOfCoveredColumns++; 233 | } 234 | } 235 | if (nOfCoveredColumns == minDim) 236 | { 237 | /* algorithm finished */ 238 | buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns); 239 | } 240 | else 241 | { 242 | /* move to step 3 */ 243 | step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 244 | } 245 | } 246 | 247 | // -------------------------------------------------------------------------- 248 | // 249 | // -------------------------------------------------------------------------- 250 | void AssignmentProblemSolver::step3(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim) 251 | { 252 | bool zerosFound = true; 253 | while (zerosFound) 254 | { 255 | zerosFound = false; 256 | for (size_t col = 0; col < nOfColumns; col++) 257 | { 258 | if (!coveredColumns[col]) 259 | { 260 | for (size_t row = 0; row < nOfRows; row++) 261 | { 262 | if ((!coveredRows[row]) && (distMatrix[row + nOfRows*col] == 0)) 263 | { 264 | /* prime zero */ 265 | primeMatrix[row + nOfRows*col] = true; 266 | /* find starred zero in current row */ 267 | size_t starCol = 0; 268 | for (; starCol < nOfColumns; starCol++) 269 | { 270 | if (starMatrix[row + nOfRows * starCol]) 271 | { 272 | break; 273 | } 274 | } 275 | if (starCol == nOfColumns) /* no starred zero found */ 276 | { 277 | /* move to step 4 */ 278 | step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim, row, col); 279 | return; 280 | } 281 | else 282 | { 283 | coveredRows[row] = true; 284 | coveredColumns[starCol] = false; 285 | zerosFound = true; 286 | break; 287 | } 288 | } 289 | } 290 | } 291 | } 292 | } 293 | /* move to step 5 */ 294 | step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 295 | } 296 | 297 | // -------------------------------------------------------------------------- 298 | // 299 | // -------------------------------------------------------------------------- 300 | void AssignmentProblemSolver::step4(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim, size_t row, size_t col) 301 | { 302 | const size_t nOfElements = nOfRows * nOfColumns; 303 | /* generate temporary copy of starMatrix */ 304 | for (size_t n = 0; n < nOfElements; n++) 305 | { 306 | newStarMatrix[n] = starMatrix[n]; 307 | } 308 | /* star current zero */ 309 | newStarMatrix[row + nOfRows*col] = true; 310 | /* find starred zero in current column */ 311 | size_t starCol = col; 312 | size_t starRow = 0; 313 | for (; starRow < nOfRows; starRow++) 314 | { 315 | if (starMatrix[starRow + nOfRows * starCol]) 316 | { 317 | break; 318 | } 319 | } 320 | while (starRow < nOfRows) 321 | { 322 | /* unstar the starred zero */ 323 | newStarMatrix[starRow + nOfRows*starCol] = false; 324 | /* find primed zero in current row */ 325 | size_t primeRow = starRow; 326 | size_t primeCol = 0; 327 | for (; primeCol < nOfColumns; primeCol++) 328 | { 329 | if (primeMatrix[primeRow + nOfRows * primeCol]) 330 | { 331 | break; 332 | } 333 | } 334 | /* star the primed zero */ 335 | newStarMatrix[primeRow + nOfRows*primeCol] = true; 336 | /* find starred zero in current column */ 337 | starCol = primeCol; 338 | for (starRow = 0; starRow < nOfRows; starRow++) 339 | { 340 | if (starMatrix[starRow + nOfRows * starCol]) 341 | { 342 | break; 343 | } 344 | } 345 | } 346 | /* use temporary copy as new starMatrix */ 347 | /* delete all primes, uncover all rows */ 348 | for (size_t n = 0; n < nOfElements; n++) 349 | { 350 | primeMatrix[n] = false; 351 | starMatrix[n] = newStarMatrix[n]; 352 | } 353 | for (size_t n = 0; n < nOfRows; n++) 354 | { 355 | coveredRows[n] = false; 356 | } 357 | /* move to step 2a */ 358 | step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 359 | } 360 | 361 | // -------------------------------------------------------------------------- 362 | // 363 | // -------------------------------------------------------------------------- 364 | void AssignmentProblemSolver::step5(assignments_t& assignment, track_t *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, size_t nOfRows, size_t nOfColumns, size_t minDim) 365 | { 366 | /* find smallest uncovered element h */ 367 | float h = std::numeric_limits::max(); 368 | for (size_t row = 0; row < nOfRows; row++) 369 | { 370 | if (!coveredRows[row]) 371 | { 372 | for (size_t col = 0; col < nOfColumns; col++) 373 | { 374 | if (!coveredColumns[col]) 375 | { 376 | const float value = distMatrix[row + nOfRows*col]; 377 | if (value < h) 378 | { 379 | h = value; 380 | } 381 | } 382 | } 383 | } 384 | } 385 | /* add h to each covered row */ 386 | for (size_t row = 0; row < nOfRows; row++) 387 | { 388 | if (coveredRows[row]) 389 | { 390 | for (size_t col = 0; col < nOfColumns; col++) 391 | { 392 | distMatrix[row + nOfRows*col] += h; 393 | } 394 | } 395 | } 396 | /* subtract h from each uncovered column */ 397 | for (size_t col = 0; col < nOfColumns; col++) 398 | { 399 | if (!coveredColumns[col]) 400 | { 401 | for (size_t row = 0; row < nOfRows; row++) 402 | { 403 | distMatrix[row + nOfRows*col] -= h; 404 | } 405 | } 406 | } 407 | /* move to step 3 */ 408 | step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 409 | } 410 | 411 | 412 | // -------------------------------------------------------------------------- 413 | // Computes a suboptimal solution. Good for cases without forbidden assignments. 414 | // -------------------------------------------------------------------------- 415 | void AssignmentProblemSolver::assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns) 416 | { 417 | /* make working copy of distance Matrix */ 418 | const size_t nOfElements = nOfRows * nOfColumns; 419 | float* distMatrix = (float*)malloc(nOfElements * sizeof(float)); 420 | for (size_t n = 0; n < nOfElements; n++) 421 | { 422 | distMatrix[n] = distMatrixIn[n]; 423 | } 424 | 425 | /* recursively search for the minimum element and do the assignment */ 426 | for (;;) 427 | { 428 | /* find minimum distance observation-to-track pair */ 429 | float minValue = std::numeric_limits::max(); 430 | size_t tmpRow = 0; 431 | size_t tmpCol = 0; 432 | for (size_t row = 0; row < nOfRows; row++) 433 | { 434 | for (size_t col = 0; col < nOfColumns; col++) 435 | { 436 | const float value = distMatrix[row + nOfRows*col]; 437 | if (value != std::numeric_limits::max() && (value < minValue)) 438 | { 439 | minValue = value; 440 | tmpRow = row; 441 | tmpCol = col; 442 | } 443 | } 444 | } 445 | 446 | if (minValue != std::numeric_limits::max()) 447 | { 448 | assignment[tmpRow] = static_cast(tmpCol); 449 | cost += minValue; 450 | for (size_t n = 0; n < nOfRows; n++) 451 | { 452 | distMatrix[n + nOfRows*tmpCol] = std::numeric_limits::max(); 453 | } 454 | for (size_t n = 0; n < nOfColumns; n++) 455 | { 456 | distMatrix[tmpRow + nOfRows*n] = std::numeric_limits::max(); 457 | } 458 | } 459 | else 460 | { 461 | break; 462 | } 463 | } 464 | 465 | free(distMatrix); 466 | } 467 | // -------------------------------------------------------------------------- 468 | // Computes a suboptimal solution. Good for cases with many forbidden assignments. 469 | // -------------------------------------------------------------------------- 470 | void AssignmentProblemSolver::assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, size_t nOfRows, size_t nOfColumns) 471 | { 472 | /* make working copy of distance Matrix */ 473 | const size_t nOfElements = nOfRows * nOfColumns; 474 | float* distMatrix = (float *)malloc(nOfElements * sizeof(float)); 475 | for (size_t n = 0; n < nOfElements; n++) 476 | { 477 | distMatrix[n] = distMatrixIn[n]; 478 | } 479 | 480 | /* allocate memory */ 481 | int* nOfValidObservations = (int *)calloc(nOfRows, sizeof(int)); 482 | int* nOfValidTracks = (int *)calloc(nOfColumns, sizeof(int)); 483 | 484 | /* compute number of validations */ 485 | bool infiniteValueFound = false; 486 | bool finiteValueFound = false; 487 | for (size_t row = 0; row < nOfRows; row++) 488 | { 489 | for (size_t col = 0; col < nOfColumns; col++) 490 | { 491 | if (distMatrix[row + nOfRows*col] != std::numeric_limits::max()) 492 | { 493 | nOfValidTracks[col] += 1; 494 | nOfValidObservations[row] += 1; 495 | finiteValueFound = true; 496 | } 497 | else 498 | { 499 | infiniteValueFound = true; 500 | } 501 | } 502 | } 503 | 504 | if (infiniteValueFound) 505 | { 506 | if (!finiteValueFound) 507 | { 508 | return; 509 | } 510 | bool repeatSteps = true; 511 | 512 | while (repeatSteps) 513 | { 514 | repeatSteps = false; 515 | 516 | /* step 1: reject assignments of multiply validated tracks to singly validated observations */ 517 | for (size_t col = 0; col < nOfColumns; col++) 518 | { 519 | bool singleValidationFound = false; 520 | for (size_t row = 0; row < nOfRows; row++) 521 | { 522 | if (distMatrix[row + nOfRows * col] != std::numeric_limits::max() && (nOfValidObservations[row] == 1)) 523 | { 524 | singleValidationFound = true; 525 | break; 526 | } 527 | 528 | if (singleValidationFound) 529 | { 530 | for (size_t row = 0; row < nOfRows; row++) 531 | if ((nOfValidObservations[row] > 1) && distMatrix[row + nOfRows*col] != std::numeric_limits::max()) 532 | { 533 | distMatrix[row + nOfRows*col] = std::numeric_limits::max(); 534 | nOfValidObservations[row] -= 1; 535 | nOfValidTracks[col] -= 1; 536 | repeatSteps = true; 537 | } 538 | } 539 | } 540 | } 541 | 542 | /* step 2: reject assignments of multiply validated observations to singly validated tracks */ 543 | if (nOfColumns > 1) 544 | { 545 | for (size_t row = 0; row < nOfRows; row++) 546 | { 547 | bool singleValidationFound = false; 548 | for (size_t col = 0; col < nOfColumns; col++) 549 | { 550 | if (distMatrix[row + nOfRows*col] != std::numeric_limits::max() && (nOfValidTracks[col] == 1)) 551 | { 552 | singleValidationFound = true; 553 | break; 554 | } 555 | } 556 | 557 | if (singleValidationFound) 558 | { 559 | for (size_t col = 0; col < nOfColumns; col++) 560 | { 561 | if ((nOfValidTracks[col] > 1) && distMatrix[row + nOfRows*col] != std::numeric_limits::max()) 562 | { 563 | distMatrix[row + nOfRows*col] = std::numeric_limits::max(); 564 | nOfValidObservations[row] -= 1; 565 | nOfValidTracks[col] -= 1; 566 | repeatSteps = true; 567 | } 568 | } 569 | } 570 | } 571 | } 572 | } /* while(repeatSteps) */ 573 | 574 | /* for each multiply validated track that validates only with singly validated */ 575 | /* observations, choose the observation with minimum distance */ 576 | for (size_t row = 0; row < nOfRows; row++) 577 | { 578 | if (nOfValidObservations[row] > 1) 579 | { 580 | bool allSinglyValidated = true; 581 | float minValue = std::numeric_limits::max(); 582 | size_t tmpCol = 0; 583 | for (size_t col = 0; col < nOfColumns; col++) 584 | { 585 | const float value = distMatrix[row + nOfRows*col]; 586 | if (value != std::numeric_limits::max()) 587 | { 588 | if (nOfValidTracks[col] > 1) 589 | { 590 | allSinglyValidated = false; 591 | break; 592 | } 593 | else if ((nOfValidTracks[col] == 1) && (value < minValue)) 594 | { 595 | tmpCol = col; 596 | minValue = value; 597 | } 598 | } 599 | } 600 | 601 | if (allSinglyValidated) 602 | { 603 | assignment[row] = static_cast(tmpCol); 604 | cost += minValue; 605 | for (size_t n = 0; n < nOfRows; n++) 606 | { 607 | distMatrix[n + nOfRows*tmpCol] = std::numeric_limits::max(); 608 | } 609 | for (size_t n = 0; n < nOfColumns; n++) 610 | { 611 | distMatrix[row + nOfRows*n] = std::numeric_limits::max(); 612 | } 613 | } 614 | } 615 | } 616 | 617 | // for each multiply validated observation that validates only with singly validated track, choose the track with minimum distance 618 | for (size_t col = 0; col < nOfColumns; col++) 619 | { 620 | if (nOfValidTracks[col] > 1) 621 | { 622 | bool allSinglyValidated = true; 623 | float minValue = std::numeric_limits::max(); 624 | size_t tmpRow = 0; 625 | for (size_t row = 0; row < nOfRows; row++) 626 | { 627 | const float value = distMatrix[row + nOfRows*col]; 628 | if (value != std::numeric_limits::max()) 629 | { 630 | if (nOfValidObservations[row] > 1) 631 | { 632 | allSinglyValidated = false; 633 | break; 634 | } 635 | else if ((nOfValidObservations[row] == 1) && (value < minValue)) 636 | { 637 | tmpRow = row; 638 | minValue = value; 639 | } 640 | } 641 | } 642 | 643 | if (allSinglyValidated) 644 | { 645 | assignment[tmpRow] = static_cast(col); 646 | cost += minValue; 647 | for (size_t n = 0; n < nOfRows; n++) 648 | { 649 | distMatrix[n + nOfRows*col] = std::numeric_limits::max(); 650 | } 651 | for (size_t n = 0; n < nOfColumns; n++) 652 | { 653 | distMatrix[tmpRow + nOfRows*n] = std::numeric_limits::max(); 654 | } 655 | } 656 | } 657 | } 658 | } /* if(infiniteValueFound) */ 659 | 660 | 661 | /* now, recursively search for the minimum element and do the assignment */ 662 | for (;;) 663 | { 664 | /* find minimum distance observation-to-track pair */ 665 | float minValue = std::numeric_limits::max(); 666 | size_t tmpRow = 0; 667 | size_t tmpCol = 0; 668 | for (size_t row = 0; row < nOfRows; row++) 669 | { 670 | for (size_t col = 0; col < nOfColumns; col++) 671 | { 672 | const float value = distMatrix[row + nOfRows*col]; 673 | if (value != std::numeric_limits::max() && (value < minValue)) 674 | { 675 | minValue = value; 676 | tmpRow = row; 677 | tmpCol = col; 678 | } 679 | } 680 | } 681 | 682 | if (minValue != std::numeric_limits::max()) 683 | { 684 | assignment[tmpRow] = static_cast(tmpCol); 685 | cost += minValue; 686 | for (size_t n = 0; n < nOfRows; n++) 687 | { 688 | distMatrix[n + nOfRows*tmpCol] = std::numeric_limits::max(); 689 | } 690 | for (size_t n = 0; n < nOfColumns; n++) 691 | { 692 | distMatrix[tmpRow + nOfRows*n] = std::numeric_limits::max(); 693 | } 694 | } 695 | else 696 | { 697 | break; 698 | } 699 | } 700 | 701 | /* free allocated memory */ 702 | free(nOfValidObservations); 703 | free(nOfValidTracks); 704 | free(distMatrix); 705 | } 706 | 707 | 708 | -------------------------------------------------------------------------------- /src/jpdaf_filter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "multi_robot_tracking/JpdafFilter.h" 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace std; 11 | using namespace std::chrono; 12 | 13 | JpdafFilter::JpdafFilter() 14 | { 15 | initialize_matrix(); 16 | } 17 | 18 | void JpdafFilter::initialize_matrix() 19 | { 20 | //ROS_INFO("init matrix"); 21 | // R_cam_imu << -0.9994192917454484, -0.015152697184557497, 0.03052007626234578, 22 | // 0.003454872095575218, -0.9361296277702554, -0.351638143365486, 23 | // 0.03389901393594679, -0.3513285012331849, 0.9356383601987547; 24 | 25 | R_cam_imu << 0 , -1, 0, 26 | 0, 0, -1, 27 | 1, 0, 0; 28 | } 29 | 30 | void JpdafFilter::track(bool called_from_detection) 31 | { 32 | auto time_start = high_resolution_clock::now(); 33 | startTime = ros::Time::now(); 34 | geometry_msgs::PoseArray latest_pose_detection; 35 | 36 | if(called_from_detection) 37 | { 38 | latest_pose_detection = flightmare_bounding_boxes_msgs_buffer_.back(); 39 | } 40 | 41 | else 42 | { 43 | std::vector emptyVector_pose; 44 | latest_pose_detection.poses = emptyVector_pose; 45 | } 46 | 47 | auto detections = get_detections_flightmare(latest_pose_detection); 48 | 49 | // ROS_INFO("Detections:"); 50 | // for(int d=0; d<(int)detections.size(); d++) 51 | // { 52 | // cout << detections[d].getVect() << endl; 53 | // } 54 | 55 | if(track_init) 56 | { 57 | ROS_WARN("Tracker not initialized"); 58 | for (uint d=0; d projected_predictions; 107 | 108 | 109 | ROS_WARN("========= predict step ========"); 110 | for(uint t=0; t betas_0; 137 | for(uint t=0; t beta(betas_matrix.rows()); 141 | ROS_INFO("2"); 142 | double beta_0 = 1.0; 143 | for(int m=0; m alphas_0; for(int m=0; m(time_end - time_start); 178 | ROS_ERROR_STREAM("Time taken by function: " << duration.count() << " microseconds" << endl); 179 | 180 | // draw_tracks_publish_image(detections, (double)(last_timestamp_synchronized + time_step), projected_predictions); 181 | // publishTracks((double)(last_timestamp_synchronized + time_step)); 182 | 183 | } 184 | 185 | } 186 | flightmare_bounding_boxes_msgs_buffer_.clear(); 187 | 188 | debug_track_counter++; 189 | 190 | update_timer.start(); 191 | 192 | } 193 | 194 | std::vector JpdafFilter::get_detections_flightmare(const geometry_msgs::PoseArray latest_detection) 195 | { 196 | // ROS_WARN("Converting PoseArray to Detection format"); 197 | std::vector norm_det; 198 | for(uint i=0; i < latest_detection.poses.size(); i++) 199 | { 200 | Detection one_det(latest_detection.poses[i].position.x,latest_detection.poses[i].position.y,1,1); 201 | norm_det.push_back(one_det); 202 | } 203 | 204 | return norm_det; 205 | } 206 | 207 | Eigen::Vector3f JpdafFilter::compute_angular_velocity(double detection_timestamp) 208 | { 209 | ROS_INFO("Imu buffer length: %d", (int)imu_buffer_.size()); 210 | ROS_INFO("Detection timestamp: %f", detection_timestamp); 211 | 212 | if(!imu_buffer_ok(detection_timestamp)) 213 | { 214 | Eigen::Vector3f omega(0, 0, 0); 215 | return omega; 216 | } 217 | 218 | 219 | int imu_buffer_index = 0; 220 | while(imu_buffer_index != (int)imu_buffer_.size()-1) 221 | { 222 | if((double)imu_buffer_[imu_buffer_index].header.stamp.toSec() >= detection_timestamp) 223 | { 224 | break; 225 | } 226 | imu_buffer_index++; 227 | } 228 | 229 | ROS_INFO("Selected imu buffer timestamp: %f", imu_buffer_[imu_buffer_index].header.stamp.toSec()); 230 | 231 | Eigen::Vector3f omega_imu; 232 | omega_imu << imu_buffer_[imu_buffer_index].angular_velocity.x, imu_buffer_[imu_buffer_index].angular_velocity.y, imu_buffer_[imu_buffer_index].angular_velocity.z; 233 | 234 | Eigen::Vector3f omega_cam; 235 | omega_cam = R_cam_imu*omega_imu; 236 | 237 | cout << "omega cam: " << endl << omega_cam << endl; 238 | 239 | std::vector temp; 240 | for(int i=imu_buffer_index; i<(int)imu_buffer_.size(); i++) 241 | { 242 | temp.push_back(imu_buffer_[i]); 243 | } 244 | imu_buffer_.clear(); 245 | imu_buffer_ = temp; 246 | 247 | return omega_cam; 248 | 249 | } 250 | 251 | bool JpdafFilter::imu_buffer_ok(double detection_timestamp) 252 | { 253 | if((int)imu_buffer_.size() > 1000) 254 | { 255 | ROS_FATAL("Imu buffer is too long, there is a problem somewhere"); 256 | exit(1); 257 | } 258 | else if((int)imu_buffer_.size() == 0) 259 | { 260 | // ROS_ERROR("Imu buffer length is 0. Assuming no orientation change"); 261 | return false; 262 | } 263 | else if(detection_timestamp - imu_buffer_.back().header.stamp.toSec() > 0.5) 264 | { 265 | ROS_ERROR("timestamp: %f, imu: %f", detection_timestamp, imu_buffer_.back().header.stamp.toSec() ); 266 | ROS_ERROR("Imu buffer is running too late compaired to the detections"); 267 | // exit(0); 268 | return false; 269 | } 270 | else if(detection_timestamp - imu_buffer_.front().header.stamp.toSec() < 0) 271 | { 272 | ROS_WARN("Imu buffer doesn't contain elements prior to the detection. Assuming to angular velocity"); 273 | return false; 274 | } 275 | else 276 | { 277 | return true; 278 | } 279 | } 280 | 281 | Eigen::MatrixXf JpdafFilter::association_matrix(const std::vector detections) 282 | { 283 | Eigen::MatrixXf q(detections.size(), tracks_.size()+1); 284 | q.setZero(); 285 | 286 | for(uint i=0; i JpdafFilter::generate_hypothesis_matrices(Eigen::MatrixXf assoc_mat) 304 | { 305 | std::vector hypothesis_matrices; 306 | 307 | if(assoc_mat.rows() == 0) 308 | { 309 | Eigen::MatrixXf hypothesis(assoc_mat.rows(), assoc_mat.cols()); 310 | hypothesis.setZero(); 311 | hypothesis_matrices.push_back(hypothesis); 312 | return hypothesis_matrices; 313 | } 314 | 315 | std::vector> non_zero_indexes_per_row; 316 | for(int i=0; i row_iterators(assoc_mat.rows(), 0); 322 | 323 | bool finished = false; 324 | 325 | while(!finished) 326 | { 327 | //creating hypothesis matrix and pushing it if feasible 328 | Eigen::MatrixXf hypothesis(assoc_mat.rows(), assoc_mat.cols()); 329 | hypothesis.setZero(); 330 | for(int i=0; i1) 346 | feasible = false; 347 | } 348 | if(feasible) 349 | { 350 | hypothesis_matrices.push_back(hypothesis); 351 | } 352 | 353 | //switching iterators for next hypothesis matrix 354 | row_iterators[0] = row_iterators[0]+1; 355 | for(int i=0; i detections) 376 | { 377 | auto tau_ = tau(hypothesis); 378 | auto delta_ = delta(hypothesis); 379 | int nb_associated_measurements = (int)tau_.sum(); 380 | int phi = tau_.rows() - nb_associated_measurements; // nb of false measurements 381 | int nb_associated_tracks = (int)delta_.sum(); 382 | 383 | std::vector y_tilds; 384 | std::vector Ss; 385 | 386 | for(int i=0; i JpdafFilter::compute_probabilities_of_hypothesis_matrices(std::vector hypothesis_matrices, std::vector detections) 446 | { 447 | std::vector probabilities; 448 | if(hypothesis_matrices[0].rows() == 0) 449 | { 450 | double prob = 1; 451 | probabilities.push_back(prob); 452 | return probabilities; 453 | } 454 | for(uint h=0; h hypothesis_mats, std::vector hypothesis_probs) 484 | { 485 | Eigen::MatrixXf betas_matrix(hypothesis_mats[0].rows(), hypothesis_mats[0].cols()); 486 | betas_matrix.setZero(); 487 | for(int i=0; i<(int)hypothesis_mats.size(); i++) 488 | { 489 | betas_matrix += hypothesis_probs[i]*hypothesis_mats[i]; 490 | } 491 | return betas_matrix; 492 | } 493 | 494 | std::vector JpdafFilter::create_new_tracks(std::vector detections, std::vector unassoc_detections_idx, Eigen::Vector3f omega, double time_step) 495 | { 496 | std::vector new_tracks; 497 | int fff = 0; 498 | 499 | const uint& prev_unassoc_size = prev_unassoc_detections.size(); 500 | const uint& unassoc_size = unassoc_detections_idx.size(); 501 | std::vector unassoc_detections; 502 | 503 | for(uint i=0; i costs(unassoc_size * prev_unassoc_size); 523 | for(uint i = 0; i < prev_unassoc_size; ++i) 524 | { 525 | for(uint j = 0; j < unassoc_size; ++j) 526 | { 527 | auto tmp = Eigen::Vector2f(unassoc_detections[j].x()-prev_unassoc_detections[i].x(), unassoc_detections[j].y()-prev_unassoc_detections[i].y()); 528 | costs.at(i + j * prev_unassoc_size ) = tmp.norm(); 529 | costMat(i, j) = costs.at(i + j*prev_unassoc_size); 530 | } 531 | } 532 | std::vector assignments; 533 | AssignmentProblemSolver APS; 534 | APS.Solve(costs, prev_unassoc_size, unassoc_size, assignments, AssignmentProblemSolver::optimal); 535 | //returned assignments is of length previous unassigned 536 | 537 | const uint& assSize = assignments.size(); 538 | Eigen::MatrixXf assigmentsBin(prev_unassoc_size, unassoc_size); 539 | assigmentsBin.setZero(); 540 | for(uint i = 0; i < assSize; ++i) 541 | { 542 | if( assignments[i] != -1 && costMat(i, assignments[i]) < params.assoc_cost)//TODO: should add offset due to motion into comparison 543 | { 544 | assigmentsBin(i, assignments[i]) = 1; 545 | } 546 | } 547 | for(uint i = 0; i < prev_unassoc_size; ++i) 548 | { 549 | for(uint j = 0; j < unassoc_size; ++j) 550 | { 551 | if(assigmentsBin(i, j)) 552 | { 553 | Eigen::MatrixXf B; 554 | B = Eigen::MatrixXf(2, 3); 555 | B << 0, 0, 0, 556 | 0, 0, 0; 557 | B(0,0) = ((unassoc_detections.at(j).x()-params.principal_point(0))*(unassoc_detections.at(j).y()-params.principal_point(1)))/params.focal_length; 558 | B(0,1) = -(params.focal_length*params.alpha_cam + (unassoc_detections.at(j).x()-params.principal_point(0))*(unassoc_detections.at(j).x()-params.principal_point(0))/(params.focal_length*params.alpha_cam)); 559 | B(0,2) = params.alpha_cam*(unassoc_detections.at(j).y()-params.principal_point(1)); 560 | 561 | B(1,0) = (params.focal_length + (unassoc_detections.at(j).y()-params.principal_point(1))*(unassoc_detections.at(j).y()-params.principal_point(1))/params.focal_length); 562 | B(1,1) = -((unassoc_detections.at(j).x()-params.principal_point(0))*(unassoc_detections.at(j).y()-params.principal_point(1)))/(params.alpha_cam*params.focal_length); 563 | B(1,2) = -(unassoc_detections.at(j).x()-params.principal_point(0))/params.alpha_cam; 564 | 565 | auto speed_offset = B*omega; 566 | 567 | const float& vx = (unassoc_detections.at(j).x()-prev_unassoc_detections.at(i).x())/time_step - speed_offset(0); 568 | const float& vy = (unassoc_detections.at(j).y()-prev_unassoc_detections.at(i).y())/time_step - speed_offset(1); 569 | 570 | Track tr(unassoc_detections.at(j).x(), unassoc_detections.at(j).y(), vx, vy, params); 571 | new_tracks.push_back(tr); 572 | ROS_INFO("created new track with position %f %f, speed %f %f", unassoc_detections.at(j).x(), unassoc_detections.at(j).y(), vx, vy); 573 | } 574 | } 575 | } 576 | Eigen::MatrixXf sumAssoc(1, unassoc_size); 577 | sumAssoc.setZero(); 578 | for(uint i = 0; i < prev_unassoc_size; ++i) 579 | { 580 | sumAssoc += assigmentsBin.row(i); 581 | } 582 | 583 | prev_unassoc_detections.clear(); 584 | for(uint i=0; i detections, std::vector alphas_0, std::vector betas_0, Eigen::Vector3f omega, double time_step) 596 | { 597 | //ROS_INFO("manage new old tracks: alphas_0 length=%d, betas_0 length=%d", (int)alphas_0.size(), (int)betas_0.size()); 598 | for(uint i=0; i unassoc_detections_idx; 604 | 605 | for(uint i=0; i= params.alpha_0_threshold) 608 | { 609 | unassoc_detections_idx.push_back((int)i); 610 | } 611 | } 612 | 613 | ROS_ERROR_STREAM("TEST !!"); 614 | auto new_tracks = create_new_tracks(detections, unassoc_detections_idx, omega, time_step); 615 | ROS_ERROR_STREAM("TEST !!!!!!"); 616 | 617 | for(uint j=0; j= params.beta_0_threshold) 620 | { 621 | tracks_[j].has_not_been_detected(); 622 | } 623 | else 624 | { 625 | tracks_[j].has_been_detected(); 626 | } 627 | } 628 | 629 | std::vector tmp; 630 | for(uint t=0; t JpdafFilter::get_nonzero_indexes_row(Eigen::MatrixXf mat) 668 | { 669 | std::vector nonzero_elements; 670 | if (mat.rows() != 1) 671 | { 672 | ROS_ERROR("get_nonzero_elements_row called, argument not row!"); 673 | return nonzero_elements; 674 | } 675 | for (int i=0; i 2 | 3 | using namespace std; 4 | 5 | Kalman::Kalman(const float& px, const float& py, const float& vx, const float& vy, TrackerParam params) 6 | { 7 | /* 8 | * params.R << 2, 0, 0, 2; 9 | * params.T << 100, 100; 10 | * 11 | * params.P_0 << 10, 0, 0, 0, 12 | * 0, 20, 0, 0, 13 | * 0, 0, 10, 0, 14 | * 0, 0, 0, 20; 15 | * 16 | * params.principal_point << 180, 120; 17 | * params.focal_length = 120; 18 | */ 19 | 20 | //CONTROL INPUT model Matrix 21 | B = Eigen::MatrixXf(4, 3); 22 | B << 0, 0, 0, 23 | 0, 0, 0, 24 | 0, 0, 0, 25 | 0, 0, 0; 26 | 27 | //STATE OBSERVATION MATRIX 28 | C = Eigen::MatrixXf(2, 4); 29 | C << 1, 0, 0, 0, 30 | 0, 0, 1, 0; 31 | 32 | //INITIAL COVARIANCE MATRIX 33 | //GAIN 34 | K = Eigen::MatrixXf(4, 2); 35 | 36 | //measurement noise covariance matrix 37 | R = params.R; 38 | 39 | T = params.T; 40 | 41 | f = params.focal_length; 42 | alpha = params.alpha_cam; 43 | c = params.principal_point; 44 | 45 | x << px, vx, py, vy; 46 | z = C * x; 47 | P = params.P_0; 48 | 49 | //cout << "P" << endl << P << endl; 50 | 51 | } 52 | 53 | 54 | void Kalman::predict(const float dt, const Eigen::Vector3f omega) 55 | { 56 | Eigen::Matrix4f A; 57 | A << 1, dt, 0, 0, 58 | 0, 1, 0, 0, 59 | 0, 0, 1, dt, 60 | 0, 0, 0, 1; 61 | 62 | //Lets try an other way: 63 | Eigen::Matrix4f Q; 64 | Q << std::pow(dt, 2)*T(0)/2, 0, 0, 0, 65 | 0, dt*T(1), 0, 0, 66 | 0, 0, std::pow(dt, 2)*T(0)/2, 0, 67 | 0, 0, 0, dt*T(1); 68 | 69 | 70 | // cout << "Q: " << endl << Q << endl;//Q has very low position variances and very high speed variances 71 | 72 | 73 | Eigen::Vector3f u = omega*dt; 74 | 75 | 76 | B(0,0) = ((z(0)-c(0))*(z(1)-c(1)))/f; 77 | B(0,1) = -(f*alpha + (z(0)-c(0))*(z(0)-c(0))/(f*alpha)); 78 | B(0,2) = alpha*(z(1)-c(1)); 79 | 80 | B(2,0) = (f + (z(1)-c(1))*(z(1)-c(1))/f); 81 | B(2,1) = -((z(0)-c(0))*(z(1)-c(1)))/(alpha*f); 82 | B(2,2) = -(z(0)-c(0))/alpha; 83 | 84 | B(1,0) = 0; 85 | B(1,1) = 0; 86 | B(1,2) = 0; 87 | 88 | B(3,0) = 0; 89 | B(3,1) = 0; 90 | B(3,2) = 0; 91 | 92 | if((isnan(B.array())).any()) // may happen if one track flies to infinity. The return is just to avoid a crash 93 | { 94 | ROS_FATAL("B contains NaNs"); 95 | return; 96 | } 97 | if((isinf(B.array())).any()) 98 | { 99 | ROS_FATAL("B contains infs"); 100 | return; 101 | } 102 | 103 | Eigen::MatrixXf ang_vel; 104 | ang_vel = B*u; 105 | cout << "B: " << B << endl; 106 | cout << "u: " << u << endl; 107 | cout << "Bu: " << ang_vel << endl; 108 | 109 | x = A*x + ang_vel; 110 | 111 | // cout << "x predict:" << endl << x << endl; 112 | Eigen::MatrixXf F = Eigen::MatrixXf(4, 4); 113 | F(0,0) = (dt*u(1)*(2*c(0) - 2*z(0)))/f - (dt*u(0)*(c(1) - z(1)))/f + 1; F(0,1) = dt; F(0,2) = dt*u(2) - (dt*u(0)*(c(0) - z(0)))/f; F(0,3) = 0; 114 | F(2,0) = 0; F(2,1) = 1; F(2,2) = 0; F(2,3) = 0; 115 | F(1,0) = (dt*u(1)*(c(1) - z(1)))/f - dt*u(2); F(1,1) = 0; F(1,2) = (dt*u(1)*(c(0) - z(0)))/f - (dt*u(0)*(2*c(1) - 2*z(1)))/f + 1; F(1,3) = dt; 116 | F(3,0) = 0; F(3,1) = 0; F(3,2) = 0; F(3,3) = 1; 117 | 118 | 119 | P = A * P * A.transpose() + Q; //ttt 120 | 121 | // cout << "P: " << endl << P << endl; 122 | 123 | //the following bugs should not happen anymore, but I leave the checks in case some bug percists 124 | if(P.determinant() < 0) 125 | { 126 | ROS_FATAL("Predicted covariance determinant is negative! %f", P.determinant()); 127 | exit(0); 128 | } 129 | if((isnan(P.array())).any()) 130 | { 131 | ROS_FATAL("P contains NaNs"); 132 | exit(0); 133 | } 134 | if((isinf(P.array())).any()) 135 | { 136 | ROS_FATAL("P contains infs"); 137 | exit(0); 138 | } 139 | 140 | //Error Measurement Covariance Matrix 141 | S = C * P * C.transpose() + R; 142 | //cout << "S: " << endl << S << endl; 143 | 144 | 145 | z = C * x; 146 | 147 | return; 148 | } 149 | 150 | 151 | 152 | void Kalman::update(const std::vector detections, const std::vector beta, double beta_0) 153 | { 154 | 155 | K = P * C.transpose() * S.inverse(); 156 | cout << "K:" << endl << K << endl; 157 | 158 | std::vector nus; 159 | for(uint i=0; i 2 | #include 3 | 4 | //cv image 5 | // #include 6 | // #include 7 | 8 | //ros 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | //phd filter class 18 | #include 19 | 20 | //jpdaf filter class 21 | #include 22 | 23 | //Simple Kalman filter class 24 | #include 25 | 26 | //export and store csv 27 | #include 28 | #include 29 | 30 | using namespace std; 31 | 32 | bool want_export_toCSV = true; 33 | 34 | 35 | class multi_robot_tracking_Node 36 | { 37 | public: 38 | multi_robot_tracking_Node(){} 39 | ~multi_robot_tracking_Node(){} 40 | 41 | void init(); //default init of nodelet 42 | 43 | //callback functions 44 | void detection_Callback(const geometry_msgs::PoseArray& in_PoseArray); //bbox to track 45 | // void image_Callback(const sensor_msgs::ImageConstPtr &img_msg); //rgb raw 46 | void imu_Callback(const sensor_msgs::ImuConstPtr &imu_msg); //rgb raw 47 | void ground_truth_Callback(const geometry_msgs::PoseArray& in_PoseArray); //bbox projection from ground truth 48 | 49 | Eigen::MatrixXf get_B_ang_vel_matrix(float x, float y); //return B matrix for each measurement 50 | 51 | 52 | //extra helper functions 53 | void draw_image(); 54 | void init_matrices(); 55 | void publish_tracks(); 56 | void associate_consensus(); 57 | Eigen::MatrixXf project_2d_to_3d(Eigen::MatrixXf position); 58 | void consensus_sort(); 59 | 60 | std::string filter_to_use_; //choose between phd or jpdaf 61 | std::string input_bbox_topic; //choose between /hummingbird1/track/bounding_box or /image_processor/objects_center 62 | std::string input_img_topic; //choose between /hummingbird1/track/bounding_box or /image_processor/objects_center 63 | std::string input_imu_topic; 64 | int num_drones; 65 | 66 | float init_pos_x_left =0; //init position to read from rosparam for consensus 67 | float init_pos_y_left =0; 68 | float init_pos_x_right = 0; 69 | float init_pos_y_right = 0; 70 | float init_pos_self_x = 0; 71 | float init_pos_self_y = 0; 72 | int id_left = 0; 73 | int id_right = 0; 74 | 75 | 76 | bool consensus_sort_complete = true; 77 | 78 | ros::Time img_timestamp; 79 | ros::Time prev_img_timestamp; 80 | ros::Time bbox_timestamp; 81 | ros::Time imu_timestamp; 82 | double previous_timestamp = 0; //t-1 83 | double current_timestamp = 0; //t 84 | double delta_timestamp = 0; //dt 85 | double imu_time = 0; 86 | int detection_seq = 0; 87 | 88 | bool first_track_flag = false; //flag after 1st track update, use to update asynchronous prediction 89 | 90 | std::vector image_buffer_; 91 | std::vector sensor_imu_buffer_; 92 | 93 | 94 | //filters initialize 95 | PhdFilter phd_filter_; 96 | JpdafFilter jpdaf_filter_; 97 | KalmanFilter kalman_filter_; 98 | 99 | //sub and pub 100 | image_transport::Publisher image_pub_; 101 | ros::Publisher pose_glass2drone_pub_; 102 | ros::Publisher pose_glass2drone_proj_pub_; 103 | ros::Publisher tracked_pose_pub_; 104 | ros::Publisher tracked_velocity_pub_; 105 | 106 | ros::Subscriber detection_sub_; 107 | ros::Subscriber image_sub_; 108 | ros::Subscriber imu_sub_; 109 | ros::Subscriber groundtruth_sub_; 110 | 111 | //output RGB data, pose data 112 | // cv::Mat input_image; 113 | // cv::Mat previous_image; 114 | sensor_msgs::ImagePtr image_msg; 115 | sensor_msgs::Imu imu_; 116 | 117 | 118 | //3D matrices for transform 119 | Eigen::MatrixXf rotm_world2cam; 120 | Eigen::MatrixXf Hmatfiller1x4; 121 | Eigen::MatrixXf k_matrix3x3; 122 | Eigen::MatrixXf k_matrix3x3_inv; 123 | 124 | //init world coordinates for consensus 125 | Eigen::MatrixXf positions_world_coordinate; 126 | Eigen::MatrixXf positions_cam_coordinate; 127 | Eigen::MatrixXf projected_2d_initial_coord; 128 | 129 | Eigen::MatrixXd id_consensus; 130 | Eigen::MatrixXd id_array_init; 131 | 132 | 133 | //B matrix constants for ang velocity 134 | float cx, cy, f; 135 | 136 | float filter_dt; 137 | 138 | bool enable_async_pdf; 139 | 140 | //Detection image frame 141 | int detection_height, detection_width; 142 | int detection_offset_x, detection_offset_y; 143 | 144 | //Camera frame size 145 | int image_height, image_width; 146 | 147 | //phd_filter_parameters 148 | float q_pos, q_vel, r_meas; 149 | float p_pos_init, p_vel_init; 150 | float phd_prune_weight_threshold; 151 | float phd_prune_mahalanobis_dist_threshold; 152 | float phd_extract_weight_threshold; 153 | 154 | //output csv file 155 | ofstream outputFile; 156 | }; 157 | 158 | 159 | void multi_robot_tracking_Node::init_matrices() 160 | { 161 | ROS_INFO("init matrix for drone num: %d",num_drones); 162 | ROS_WARN("nodelet start init matrix... verify cam K matrix for simulation or snapdragon pro!"); 163 | k_matrix3x3 = Eigen::MatrixXf(3,3); 164 | k_matrix3x3_inv = Eigen::MatrixXf(3,3); 165 | 166 | positions_world_coordinate = Eigen::MatrixXf(3,num_drones); 167 | positions_cam_coordinate = Eigen::MatrixXf(3,num_drones); 168 | projected_2d_initial_coord = Eigen::MatrixXf(2,num_drones); 169 | 170 | 171 | id_consensus = Eigen::MatrixXd(1,num_drones); 172 | id_array_init = Eigen::MatrixXd(1,num_drones); 173 | for(int i=0; iheader.stamp - imu_timestamp; 260 | phd_filter_.dt_imu = timeDifIMU.toSec(); 261 | ROS_WARN("imu time: %f", phd_filter_.dt_imu); 262 | imu_.angular_velocity = imu_msg->angular_velocity; 263 | imu_.angular_velocity_covariance = imu_msg->angular_velocity_covariance; 264 | imu_.header = imu_msg->header; 265 | imu_.linear_acceleration = imu_msg->linear_acceleration; 266 | imu_.linear_acceleration_covariance = imu_msg->linear_acceleration_covariance; 267 | imu_.orientation = imu_msg->orientation; 268 | imu_.orientation_covariance = imu_msg->orientation_covariance; 269 | if(phd_filter_.first_callback == false) 270 | { 271 | phd_filter_.ang_vel_k(0) = imu_msg->angular_velocity.x; 272 | phd_filter_.ang_vel_k(1) = imu_msg->angular_velocity.y; 273 | phd_filter_.ang_vel_k(2) = imu_msg->angular_velocity.z; 274 | 275 | //apply rotation from imu2cam frame 276 | phd_filter_.ang_vel_k.block<3,1>(0,0) = rotm_world2cam * phd_filter_.ang_vel_k; 277 | 278 | //asynchronous motion prediction 279 | if(first_track_flag) 280 | { 281 | phd_filter_.asynchronous_predict_existing(); 282 | publish_tracks(); 283 | } 284 | } 285 | // if(filter_to_use_.compare("kalman") == 0) 286 | // { 287 | // kalman_filter_.ang_vel_k(0) = imu_msg->angular_velocity.x; 288 | // kalman_filter_.ang_vel_k(1) = imu_msg->angular_velocity.y; 289 | // kalman_filter_.ang_vel_k(2) = imu_msg->angular_velocity.z; 290 | 291 | // //apply rotation from imu2cam frame 292 | // kalman_filter_.ang_vel_k.block<3,1>(0,0) = rotm_world2cam * phd_filter_.ang_vel_k; 293 | 294 | // //asynchronous motion prediction 295 | // if(first_track_flag) 296 | // { 297 | // kalman_filter_.kalmanPredict(); 298 | // publish_tracks(); 299 | // } 300 | // } 301 | imu_timestamp = imu_msg->header.stamp; 302 | } 303 | 304 | Eigen::MatrixXf multi_robot_tracking_Node::get_B_ang_vel_matrix(float x, float y) 305 | { 306 | Eigen::MatrixXf temp_B_matrix; 307 | temp_B_matrix = Eigen::MatrixXf::Zero(4,3); 308 | 309 | temp_B_matrix(0,0) = (x-cx)*(y-cy)/f; temp_B_matrix(0,1) = -(pow((x-cx),2)/f)-f; temp_B_matrix(0,2) = (y-cy); 310 | temp_B_matrix(1,0) = 0; temp_B_matrix(1,1) = 0; temp_B_matrix(1,2) = 0; 311 | temp_B_matrix(2,0) = f+(pow((y-cy),2))/f; temp_B_matrix(2,1) = (x-cx)*(y-cy)/f; temp_B_matrix(2,2) = -x+cx; 312 | temp_B_matrix(3,0) = 0; temp_B_matrix(3,1) = 0; temp_B_matrix(3,2) = 0; 313 | 314 | temp_B_matrix = temp_B_matrix * phd_filter_.dt_imu; 315 | return temp_B_matrix; 316 | 317 | } 318 | 319 | 320 | /* callback for 2D image to call phd track when using flightmare rosbag data 321 | * input: PoseArray 322 | * output: N/A 323 | */ 324 | void multi_robot_tracking_Node::detection_Callback(const geometry_msgs::PoseArray& in_PoseArray) 325 | { 326 | if(in_PoseArray.poses.size() > num_drones) 327 | { 328 | ROS_ERROR("MORE DETECTIONS THAN NO OF DRONES !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n"); 329 | return; 330 | } 331 | 332 | //get time of detection 333 | bbox_timestamp = in_PoseArray.header.stamp; 334 | current_timestamp = bbox_timestamp.toSec(); 335 | 336 | 337 | 338 | 339 | ROS_WARN("bbox time: %f, dt: %f, imu time: %f",current_timestamp, delta_timestamp, imu_time); 340 | 341 | // ROS_INFO("detected size: %lu ", in_PoseArray.poses.size() ); 342 | jpdaf_filter_.last_timestamp_synchronized = in_PoseArray.header.stamp.toSec(); 343 | 344 | //store Z 345 | 346 | //========= use jpdaf filter =========== 347 | if(filter_to_use_.compare("jpdaf") == 0) 348 | { 349 | 350 | jpdaf_filter_.detected_size_k = in_PoseArray.poses.size(); 351 | 352 | //store Z 353 | jpdaf_filter_.flightmare_bounding_boxes_msgs_buffer_.push_back(in_PoseArray); 354 | //store imu 355 | jpdaf_filter_.imu_buffer_.push_back(imu_); 356 | 357 | // jpdaf_filter_.Z_k = Eigen::MatrixXf::Zero(4,jpdaf_filter_.detected_size_k); 358 | 359 | // for(int i =0; i < jpdaf_filter_.detected_size_k; i++) 360 | // { 361 | // jpdaf_filter_.Z_k(0,i) = in_PoseArray.poses[i].position.x; 362 | // jpdaf_filter_.Z_k(1,i) = in_PoseArray.poses[i].position.y; 363 | // } 364 | 365 | jpdaf_filter_.track(true); 366 | } 367 | 368 | //========= use phd filter =========== 369 | else if(filter_to_use_.compare("phd") == 0) 370 | { 371 | 372 | if(phd_filter_.first_callback) 373 | { 374 | phd_filter_.set_num_drones(num_drones); 375 | phd_filter_.initialize_matrix(cx, cy, f, filter_dt); 376 | } 377 | 378 | 379 | phd_filter_.detected_size_k = in_PoseArray.poses.size(); 380 | 381 | for(int i =0; i < phd_filter_.detected_size_k; i++) 382 | { 383 | //store Z 384 | // x, y, w, h 385 | phd_filter_.Z_k(0,i) = in_PoseArray.poses[i].position.x; 386 | phd_filter_.Z_k(1,i) = in_PoseArray.poses[i].position.y; 387 | 388 | phd_filter_.Detections(0,i) = in_PoseArray.poses[i].position.x; 389 | phd_filter_.Detections(1,i) = in_PoseArray.poses[i].position.y; 390 | phd_filter_.Detections(2,i) = in_PoseArray.poses[i].orientation.x; 391 | phd_filter_.Detections(3,i) = in_PoseArray.poses[i].orientation.y; 392 | } 393 | ROS_INFO_STREAM("Num Meas: " << phd_filter_.detected_size_k << "\n"); 394 | ROS_INFO_STREAM("Z_k_CB: " << endl << phd_filter_.Z_k << "\n"); 395 | ROS_INFO_STREAM("WK-1: " << phd_filter_.wk << "\n"); 396 | if(phd_filter_.first_callback) 397 | { 398 | delta_timestamp = 0.125;//0.143; //0.225 399 | phd_filter_.dt_cam = delta_timestamp; 400 | 401 | phd_filter_.initialize(q_pos, q_vel, r_meas, p_pos_init, p_vel_init, 402 | phd_prune_weight_threshold, 403 | phd_prune_mahalanobis_dist_threshold, 404 | phd_extract_weight_threshold); 405 | phd_filter_.first_callback = false; 406 | 407 | previous_timestamp = current_timestamp; 408 | } 409 | 410 | else 411 | { 412 | 413 | 414 | delta_timestamp = 0.143; //hard-coded for 4.5 Hz TO DO FIX 415 | // delta_timestamp = current_timestamp - previous_timestamp; 416 | //check for data with no timestamp and thus dt = 0 417 | 418 | phd_filter_.dt_cam = delta_timestamp; 419 | previous_timestamp = current_timestamp; 420 | for(int i =0; i < phd_filter_.X_k.cols(); i++) 421 | { 422 | phd_filter_.B.block<4,3>(0,3*i) = get_B_ang_vel_matrix(phd_filter_.X_k(0,i),phd_filter_.X_k(2,i)); 423 | } 424 | 425 | phd_filter_.phd_track(); 426 | first_track_flag = true; 427 | 428 | //after tracking, store previous Z value to update velocity 429 | for(int i =0; i < phd_filter_.detected_size_k; i++) 430 | { 431 | //store Z 432 | phd_filter_.Z_k_previous(0,i) = in_PoseArray.poses[i].position.x; 433 | phd_filter_.Z_k_previous(1,i) = in_PoseArray.poses[i].position.y; 434 | 435 | } 436 | 437 | 438 | phd_filter_.B = Eigen::MatrixXf::Zero(4,3*num_drones); 439 | 440 | //update for B ang vel matrix 441 | //store B matrix for ang velocity 442 | 443 | 444 | // consensus_sort(); 445 | 446 | // imu_timestamp = in_PoseArray.header.stamp; 447 | } 448 | } 449 | 450 | else if(filter_to_use_.compare("kalman") == 0) 451 | { 452 | 453 | if(kalman_filter_.first_callback) 454 | { 455 | kalman_filter_.setNumDrones(num_drones); 456 | kalman_filter_.initializeMatrix(cx, cy, f, filter_dt); 457 | } 458 | 459 | 460 | kalman_filter_.detected_size_k = in_PoseArray.poses.size(); 461 | 462 | 463 | for(int i =0; i < kalman_filter_.detected_size_k; i++) 464 | { 465 | //store Z 466 | // x, y, w, h 467 | kalman_filter_.Z_k(0,i) = in_PoseArray.poses[i].position.x; 468 | kalman_filter_.Z_k(1,i) = in_PoseArray.poses[i].position.y; 469 | 470 | kalman_filter_.Detections(0,i) = in_PoseArray.poses[i].position.x; 471 | kalman_filter_.Detections(1,i) = in_PoseArray.poses[i].position.y; 472 | kalman_filter_.Detections(2,i) = in_PoseArray.poses[i].orientation.x; 473 | kalman_filter_.Detections(3,i) = in_PoseArray.poses[i].orientation.y; 474 | } 475 | ROS_INFO_STREAM("Num Meas: " << kalman_filter_.detected_size_k << "\n"); 476 | ROS_INFO_STREAM("Z_k_CB: " << endl << kalman_filter_.Z_k << "\n"); 477 | ROS_INFO_STREAM("WK-1: " << kalman_filter_.wk << "\n"); 478 | if(kalman_filter_.first_callback) 479 | { 480 | delta_timestamp = 0.125;//0.143; //0.225 481 | kalman_filter_.dt_cam = delta_timestamp; 482 | 483 | kalman_filter_.initialize(q_pos, q_vel, r_meas, p_pos_init, p_vel_init, 484 | phd_prune_weight_threshold, 485 | phd_prune_mahalanobis_dist_threshold, 486 | phd_extract_weight_threshold); 487 | kalman_filter_.first_callback = false; 488 | 489 | previous_timestamp = current_timestamp; 490 | } 491 | 492 | else 493 | { 494 | 495 | 496 | delta_timestamp = 0.143; //hard-coded for 4.5 Hz TO DO FIX 497 | // delta_timestamp = current_timestamp - previous_timestamp; 498 | //check for data with no timestamp and thus dt = 0 499 | 500 | kalman_filter_.dt_cam = delta_timestamp; 501 | previous_timestamp = current_timestamp; 502 | for(int i =0; i < phd_filter_.X_k.cols(); i++) 503 | { 504 | kalman_filter_.B.block<4,3>(0,3*i) = get_B_ang_vel_matrix(phd_filter_.X_k(0,i),phd_filter_.X_k(2,i)); 505 | } 506 | 507 | kalman_filter_.kalmanTrack(); 508 | ROS_INFO_STREAM("Finished track"); 509 | first_track_flag = true; 510 | 511 | //after tracking, store previous Z value to update velocity 512 | // for(int i =0; i < phd_filter_.detected_size_k; i++) 513 | // { 514 | // //store Z 515 | // kalman_filter_.Z_k_previous(0,i) = in_PoseArray.poses[i].position.x; 516 | // kalman_filter_.Z_k_previous(1,i) = in_PoseArray.poses[i].position.y; 517 | 518 | // } 519 | 520 | 521 | kalman_filter_.B = Eigen::MatrixXf::Zero(4,3*num_drones); 522 | 523 | //update for B ang vel matrix 524 | //store B matrix for ang velocity 525 | 526 | 527 | // consensus_sort(); 528 | 529 | // imu_timestamp = in_PoseArray.header.stamp; 530 | } 531 | } 532 | 533 | ROS_INFO_STREAM("Pub track"); 534 | publish_tracks(); 535 | } 536 | 537 | /* given known initial projected coordinated, and Left/Right ID 538 | * sort the id_consensus according to the estimated target 539 | */ 540 | void multi_robot_tracking_Node::consensus_sort() 541 | { 542 | if(!consensus_sort_complete) 543 | { 544 | float euclidian_distance = 0; 545 | float min_distance = 1000000; 546 | int min_index = 0; 547 | //get init proj 548 | 549 | //get measurement 550 | 551 | //compare euclidian distance for each target with init projection 552 | for (int z = 0; z < phd_filter_.X_k.cols(); z++) 553 | { 554 | min_index = 0; 555 | min_distance = 100000; 556 | 557 | for(int index = 0; index < num_drones; index++) 558 | { 559 | cout << "X_k: " << phd_filter_.X_k(0,z) << "projX: " << projected_2d_initial_coord(0,index) << endl; 560 | euclidian_distance = abs(phd_filter_.X_k(0,z) - projected_2d_initial_coord(0,index)) + abs(phd_filter_.X_k(1,z) - projected_2d_initial_coord(1,index)) ; 561 | //store min index 562 | if(euclidian_distance < min_distance) 563 | { 564 | min_distance = euclidian_distance; 565 | min_index = index; 566 | } 567 | } 568 | id_consensus(z) = int(id_array_init(min_index)); 569 | 570 | } 571 | 572 | //sort ID accordingly 573 | consensus_sort_complete = true; 574 | cout << " **************** consensus ID: " << id_consensus << "**************** " << endl; 575 | } 576 | 577 | } 578 | 579 | 580 | /* 581 | * 582 | */ 583 | void multi_robot_tracking_Node::associate_consensus() 584 | { 585 | //ROS_WARN("inside consensus func"); 586 | 587 | //get delta init positions in world coordinate 588 | Eigen::MatrixXf delta_left_to_self, delta_right_to_self; 589 | Eigen::MatrixXf projected_2d_padding; 590 | delta_left_to_self = Eigen::MatrixXf::Zero(3,1); 591 | delta_right_to_self = Eigen::MatrixXf::Zero(3,1); 592 | projected_2d_padding = Eigen::MatrixXf::Zero(3,num_drones); 593 | 594 | //delta_left_to_self(0) = init_pos_x_left - init_pos_self_x; 595 | //delta_left_to_self(1) = init_pos_y_left - init_pos_self_y; 596 | delta_left_to_self(0) = 3.4641; 597 | delta_left_to_self(1) = 2; 598 | delta_left_to_self(2) = 0; 599 | 600 | // delta_right_to_self(0) = init_pos_x_right - init_pos_self_x; 601 | // delta_right_to_self(1) = init_pos_y_right - init_pos_self_y; 602 | delta_right_to_self(0) = 3.4641; 603 | delta_right_to_self(1) = -2; 604 | delta_right_to_self(2) = 0; 605 | 606 | cout << "deltaL: " << endl << delta_left_to_self << endl; 607 | cout << "deltaR: " << endl << delta_right_to_self << endl; 608 | 609 | positions_world_coordinate.block<3,1>(0,0) = delta_left_to_self; 610 | positions_world_coordinate.block<3,1>(0,1) = delta_right_to_self; 611 | 612 | //get delta init positions in camera coordinate 613 | positions_cam_coordinate.block<3,1>(0,0) = rotm_world2cam * positions_world_coordinate.block<3,1>(0,0) ; 614 | positions_cam_coordinate.block<3,1>(0,1) = rotm_world2cam * positions_world_coordinate.block<3,1>(0,1) ; 615 | 616 | cout << "positions_cam_coordinate: " << endl << positions_cam_coordinate << endl; 617 | 618 | 619 | //project into 2D space 620 | for(int i =0; i < num_drones;i++) 621 | { 622 | projected_2d_padding.block<3,1>(0,i) = k_matrix3x3 * positions_cam_coordinate.block<3,1>(0,i); 623 | projected_2d_padding.block<3,1>(0,i) = projected_2d_padding.block<3,1>(0,i) / projected_2d_padding(2,i); 624 | projected_2d_initial_coord.block<2,1>(0,i) = projected_2d_padding.block<2,1>(0,i); 625 | } 626 | 627 | cout << "projected_2d_initial_coord: " << endl << projected_2d_initial_coord << endl; 628 | 629 | 630 | } 631 | 632 | 633 | void multi_robot_tracking_Node::publish_tracks() 634 | { 635 | // ROS_INFO("publish tracks"); 636 | 637 | geometry_msgs::PoseArray tracked_output_pose, tracked_velocity_pose; 638 | geometry_msgs::Pose temp_pose, temp_velocity; 639 | 640 | if(filter_to_use_.compare("jpdaf") == 0) 641 | { 642 | //TO DO fill in publishing for jpdaf as well 643 | } 644 | 645 | else 646 | { 647 | 648 | //store estimated PHD X_k into tracked output 649 | for(int i =0; i < phd_filter_.X_k.cols(); i++) { 650 | 651 | temp_pose.position.x = phd_filter_.X_k(0,i); 652 | temp_pose.position.y = phd_filter_.X_k(1,i); 653 | tracked_output_pose.poses.push_back(temp_pose); 654 | 655 | temp_velocity.position.x = phd_filter_.X_k(2,i); 656 | temp_velocity.position.y = phd_filter_.X_k(3,i); 657 | tracked_velocity_pose.poses.push_back(temp_velocity); 658 | } 659 | 660 | } 661 | 662 | //publish output (stored as either jpdaf or phd) 663 | tracked_output_pose.header.stamp = bbox_timestamp; 664 | tracked_velocity_pose.header.stamp = bbox_timestamp; 665 | tracked_pose_pub_.publish(tracked_output_pose); 666 | tracked_velocity_pub_.publish(tracked_velocity_pose); 667 | 668 | //empty tracked output 669 | while(!tracked_output_pose.poses.empty()) { 670 | tracked_output_pose.poses.pop_back(); 671 | } 672 | 673 | while(!tracked_velocity_pose.poses.empty()) { 674 | tracked_velocity_pose.poses.pop_back(); 675 | } 676 | 677 | // ROS_INFO("end publish tracks"); 678 | 679 | } 680 | 681 | /* Nodelet init function to handle subscribe/publish 682 | * input: N/A 683 | * output: N/A 684 | */ 685 | void multi_robot_tracking_Node::init(void) 686 | { 687 | ros::NodeHandle nh; 688 | ros::NodeHandle priv_nh("~"); 689 | 690 | if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Error) ) { 691 | ros::console::notifyLoggerLevelsChanged(); 692 | } 693 | 694 | image_transport::ImageTransport it(nh); 695 | 696 | priv_nh.param("filter",filter_to_use_,"phd"); //store which filter to use 697 | priv_nh.param("input_bbox_topic",input_bbox_topic,"/DragonPro1/snpe_ros/detections"); //input bbox topic 698 | priv_nh.param("input_img_topic",input_img_topic,"DragonPro1/image_publisher/image_raw"); //input img topic 699 | priv_nh.param("input_imu_topic",input_imu_topic,"/DragonPro1/imu"); //input imu topic 700 | 701 | priv_nh.param("num_drones",num_drones,2); 702 | 703 | //consensus - init coordinates read in from launch file 704 | priv_nh.param("init_pos_self_x",init_pos_self_x,0); 705 | priv_nh.param("init_pos_self_y",init_pos_self_y,0); 706 | priv_nh.param("init_pos_x_left",init_pos_x_left,0); 707 | priv_nh.param("init_pos_y_left",init_pos_y_left,0); 708 | priv_nh.param("init_pos_x_right",init_pos_x_right,0); 709 | priv_nh.param("init_pos_y_right",init_pos_y_right,0); 710 | 711 | priv_nh.param("id_left",id_left,0); 712 | priv_nh.param("id_right",id_right,0); 713 | 714 | priv_nh.param("camera_cx", cx, 0); 715 | priv_nh.param("camera_cy", cy, 0); 716 | priv_nh.param("camera_f", f, 0); 717 | priv_nh.param("dt", filter_dt, 0.225); 718 | 719 | priv_nh.param("viz_detection_height", detection_height, 168); 720 | priv_nh.param("viz_detection_width", detection_width, 224); 721 | priv_nh.param("viz_detection_offset_x", detection_offset_x, 0); 722 | priv_nh.param("viz_detection_offset_y", detection_offset_y, 28); 723 | priv_nh.param("use_generated_id", consensus_sort_complete,0); 724 | 725 | priv_nh.param("phd/q_pos", q_pos, 6.25); 726 | priv_nh.param("phd/q_vel", q_vel, 12.5); 727 | priv_nh.param("phd/p_pos_init", p_pos_init, 5.0); 728 | priv_nh.param("phd/p_vel_init", p_vel_init, 2.0); 729 | priv_nh.param("phd/r_meas", r_meas, 45); 730 | priv_nh.param("phd/prune_weight_threshold", phd_prune_weight_threshold, 1e-1); 731 | priv_nh.param("phd/prune_mahalanobis_threshold_", phd_prune_mahalanobis_dist_threshold, 4.0); 732 | priv_nh.param("phd/extract_weight_threshold", phd_extract_weight_threshold, 5e-1); 733 | 734 | 735 | ROS_INFO_STREAM("Consensus sort during init " << consensus_sort_complete); 736 | 737 | if(filter_to_use_.compare("phd") == 0) //using phd 738 | { 739 | ROS_WARN("will be using: %s", filter_to_use_.c_str()); 740 | init_matrices(); //initialize matrix for storing 3D pose 741 | //associate_consensus(); //determine 2d position from known init positions 742 | 743 | } 744 | 745 | else if(filter_to_use_.compare("kalman") == 0) //using kalman 746 | { 747 | ROS_WARN("will be using: %s", filter_to_use_.c_str()); 748 | init_matrices(); //initialize matrix for storing 3D pose 749 | //associate_consensus(); //determine 2d position from known init positions 750 | 751 | } 752 | 753 | else if (filter_to_use_.compare("jpdaf") == 0) { 754 | ROS_WARN("will be using: %s", filter_to_use_.c_str()); 755 | } 756 | 757 | else { 758 | ROS_ERROR("wrong filter param input"); 759 | return; 760 | } 761 | 762 | 763 | //bbox subscription of PoseArray Type 764 | detection_sub_ = priv_nh.subscribe(input_bbox_topic, 10, &multi_robot_tracking_Node::detection_Callback, this); 765 | //img subscription 766 | // image_sub_ = priv_nh.subscribe(input_img_topic, 10, &multi_robot_tracking_Node::image_Callback, this); 767 | //imu subscription 768 | imu_sub_ = priv_nh.subscribe(input_imu_topic, 10, &multi_robot_tracking_Node::imu_Callback, this); 769 | //groundtruth bbox subscription 770 | groundtruth_sub_ = priv_nh.subscribe("/hummingbird0/ground_truth/bounding_box", 10, &multi_robot_tracking_Node::ground_truth_Callback, this); 771 | 772 | 773 | image_pub_ = it.advertise("tracked_image",1); 774 | tracked_pose_pub_ = nh.advertise("tracked_pose_output",1); 775 | tracked_velocity_pub_ = nh.advertise("tracked_velocity_output",1); 776 | 777 | //init export csv file 778 | outputFile.open("/home/marklee/rosbag/groundtruth_estimate_Asynch.csv"); 779 | outputFile << "Time" << "," << "GND_truth_X1" << "," << "GND_truth_Y1" << "," << "GND_truth_X2" << "," << "GND_truth_Y2" << "," 780 | << "est_X1" << "," << "est_Y1" << "," << "est_X2" << "," << "est_Y2" << "," << std::endl; 781 | 782 | } 783 | 784 | int main(int argc, char** argv) 785 | { 786 | ros::init(argc, argv, "multi_robot_tracking_node"); 787 | multi_robot_tracking_Node mrt; 788 | mrt.init(); 789 | 790 | ros::spin(); 791 | } -------------------------------------------------------------------------------- /src/phd_filter.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "multi_robot_tracking/PhdFilter.h" 4 | #include 5 | 6 | using namespace std; 7 | using namespace std::chrono; 8 | 9 | // #define TIMING 10 | 11 | PhdFilter::PhdFilter() 12 | 13 | { 14 | 15 | } 16 | 17 | void PhdFilter::phd_track() 18 | { 19 | #ifdef TIMING 20 | auto time_start = high_resolution_clock::now(); 21 | #endif TIMING 22 | startTime = ros::Time::now(); 23 | k_iteration = k_iteration + 1; 24 | ROS_INFO("iter: %d",k_iteration); 25 | 26 | //kalmanPredict(); // ToDo: Can be asynch ??? 27 | #ifdef TIMING 28 | auto time_endP = high_resolution_clock::now(); 29 | auto durationP = duration_cast(time_endP - time_start); 30 | ROS_ERROR_STREAM("Time taken by function predict: " << durationP.count() << " microseconds" << endl); 31 | #endif 32 | 33 | phd_construct(); 34 | #ifdef TIMING 35 | auto time_endI = high_resolution_clock::now(); 36 | auto durationI = duration_cast(time_endI - time_endP); 37 | ROS_ERROR_STREAM("Time taken by function Issue: " << durationI.count() << " microseconds" << endl); 38 | #endif 39 | 40 | phd_update(); 41 | #ifdef TIMING 42 | auto time_endA = high_resolution_clock::now(); 43 | auto durationA = duration_cast(time_endA - time_endI); 44 | ROS_ERROR_STREAM("Time taken by function Associate: " << durationA.count() << " microseconds" << endl); 45 | #endif 46 | 47 | phd_prune(); 48 | #ifdef TIMING 49 | auto time_endU = high_resolution_clock::now(); 50 | auto durationU = duration_cast(time_endU - time_endA); 51 | ROS_ERROR_STREAM("Time taken by function Update: " << durationU.count() << " microseconds" << endl); 52 | #endif 53 | 54 | phd_state_extract(); 55 | #ifdef TIMING 56 | auto time_endE = high_resolution_clock::now(); 57 | auto durationE = duration_cast(time_endE - time_endU); 58 | ROS_ERROR_STREAM("Time taken by function Extract: " << durationE.count() << " microseconds" << endl); 59 | 60 | auto time_end = high_resolution_clock::now(); 61 | auto duration = duration_cast(time_end - time_start); 62 | ROS_ERROR_STREAM("Time taken by function: " << duration.count() << " microseconds" << endl); 63 | #endif 64 | 65 | startTime = ros::Time::now(); 66 | k_iteration = k_iteration + 1; 67 | ROS_INFO("iter: %d",k_iteration); 68 | endTime = ros::Time::now(); 69 | ROS_WARN("end of track iteration"); 70 | 71 | } 72 | 73 | 74 | void PhdFilter::initialize_matrix(float cam_cu, float cam_cv, float cam_f, float meas_dt) 75 | { 76 | ROS_INFO("first initialize matrix"); 77 | //initialize 78 | Z_k = Eigen::MatrixXf::Zero(n_meas,NUM_DRONES); 79 | Z_k_previous = Eigen::MatrixXf::Zero(n_meas,NUM_DRONES); 80 | ang_vel_k = Eigen::MatrixXf::Zero(n_input,1); 81 | 82 | mk_minus_1 = Eigen::MatrixXf::Zero(n_state,NUM_DRONES); 83 | wk_minus_1 = Eigen::MatrixXf::Zero(1,NUM_DRONES); 84 | Pk_minus_1 = Eigen::MatrixXf::Zero(n_state,NUM_DRONES*4); 85 | 86 | mk = Eigen::MatrixXf::Zero(n_state,NUM_DRONES+NUM_DRONES*NUM_DRONES); 87 | wk = Eigen::MatrixXf::Zero(1,NUM_DRONES+NUM_DRONES*NUM_DRONES); 88 | Pk = Eigen::MatrixXf::Zero(n_state,n_state*(NUM_DRONES+NUM_DRONES*NUM_DRONES) ); 89 | 90 | mk_bar = Eigen::MatrixXf::Zero(n_state,NUM_DRONES); 91 | wk_bar = Eigen::MatrixXf::Zero(1,NUM_DRONES); 92 | Pk_bar = Eigen::MatrixXf::Zero(n_state,n_state*NUM_DRONES); 93 | 94 | mk_bar_fixed = Eigen::MatrixXf::Zero(n_state,NUM_DRONES); 95 | wk_bar_fixed = Eigen::MatrixXf::Zero(1,NUM_DRONES); 96 | Pk_bar_fixed = Eigen::MatrixXf::Zero(n_state,n_state*NUM_DRONES); 97 | 98 | mk_k_minus_1 = Eigen::MatrixXf::Zero(n_state,NUM_DRONES); 99 | wk_k_minus_1 = Eigen::MatrixXf::Zero(1,NUM_DRONES); 100 | Pk_k_minus_1 = Eigen::MatrixXf::Zero(n_state,n_state*NUM_DRONES); 101 | P_k_k = Eigen::MatrixXf::Zero(n_state,n_state*NUM_DRONES); 102 | S = Eigen::MatrixXf::Zero(n_meas,n_meas*NUM_DRONES); 103 | 104 | F = Eigen::MatrixXf::Zero(n_state,n_state); 105 | A = Eigen::MatrixXf::Zero(n_state,n_state); 106 | H = Eigen::MatrixXf::Zero(n_meas,n_state); 107 | Q = Eigen::MatrixXf::Zero(n_state,n_state); 108 | R = Eigen::MatrixXf::Zero(n_meas, n_meas); 109 | K = Eigen::MatrixXf::Zero(n_state,n_meas*NUM_DRONES); 110 | 111 | X_k = Eigen::MatrixXf::Zero(n_state,NUM_DRONES); 112 | X_k_previous = Eigen::MatrixXf::Zero(n_state,NUM_DRONES); 113 | 114 | B = Eigen::MatrixXf::Zero(n_state,n_input*NUM_DRONES); 115 | 116 | mk_k_minus_1_beforePrediction = Eigen::MatrixXf::Zero(n_state,NUM_DRONES); 117 | Detections = Eigen::MatrixXf::Zero(4,NUM_DRONES); 118 | 119 | 120 | cu = cam_cu; 121 | cv = cam_cv; 122 | f = cam_f; 123 | dt = 0.01; 124 | } 125 | 126 | void PhdFilter::set_num_drones(int num_drones_in) 127 | { 128 | NUM_DRONES = num_drones_in; 129 | } 130 | 131 | void PhdFilter::initialize(float q_pos, float q_vel, float r_meas, float p_pos_init, float p_vel_init, 132 | float prune_weight_threshold, float prune_mahalanobis_threshold, float extract_weight_threshold) 133 | { 134 | ROS_INFO("======= Initialize ======= \n"); 135 | Eigen::MatrixXf P_k_init; 136 | P_k_init = Eigen::MatrixXf(n_state,n_state); 137 | P_k_init << 138 | p_pos_init, 0, 0, 0, 139 | 0, p_vel_init, 0, 0, 140 | 0, 0, p_pos_init, 0, 141 | 0, 0, 0, p_vel_init; 142 | P_k_init = P_k_init * 1; 143 | 144 | 145 | for(int i = 0; i < Z_k.cols(); i ++) 146 | { 147 | //store Z into mk (x,y) 148 | ROS_INFO_STREAM("ZK: \n" << Z_k << endl); 149 | // mk_minus_1.block(0, i, n_state, 1) = H.transpose()*Z_k.block(0,i, n_meas,1); 150 | mk_minus_1.block(0, i, n_state, 1) << Z_k.block(0, i, 1, 1), 0, Z_k.block(1, i, 1, 1), 0; 151 | ROS_INFO_STREAM("mk_minus_1: \n" << mk_minus_1 << endl); 152 | //store pre-determined weight into wk (from matlab) 153 | wk_minus_1(i) = .0016; 154 | //store pre-determined weight into Pk (from paper) 155 | Pk_minus_1.block(0,i*4, n_state,n_state) = P_k_init; 156 | } 157 | 158 | A << 1,dt_imu,0,0, 159 | 0,1,0,0, 160 | 0,0,1,dt_imu, 161 | 0,0,0,1; 162 | 163 | //Measurement Matrix 164 | H << 1, 0, 0 , 0, 165 | 0, 0, 1, 0; 166 | 167 | //Process noise covariance, given in Vo&Ma. 168 | Q << q_pos, 0, 0, 0, 169 | 0, q_vel, 0, 0, 170 | 0, 0, q_pos, 0, 171 | 0, 0, 0, q_vel; 172 | 173 | //Measurement Noise 174 | R << r_meas, 0, 175 | 0, r_meas; 176 | 177 | numTargets_Jk_minus_1 = NUM_DRONES; 178 | } 179 | 180 | /* 181 | * Prediction step is done async. The prediction is called whenever we get an IMU message. 182 | * The update step is called once we get a measurement from the network 183 | */ 184 | void PhdFilter::asynchronous_predict_existing() 185 | { 186 | ROS_INFO("======= 0. asynch predict ======= \n"); 187 | //update A 188 | update_A_matrix(dt_imu); 189 | 190 | wk_minus_1 = prob_survival * wk_minus_1; 191 | Eigen::MatrixXf Bu_temp = Eigen::MatrixXf::Zero(n_state,n_meas); 192 | F = Eigen::MatrixXf(n_state,n_state); 193 | 194 | float omega_x = ang_vel_k(0); 195 | float omega_y = ang_vel_k(1); 196 | float omega_z = ang_vel_k(2); 197 | float pu = 0; 198 | float pv = 0; 199 | 200 | Eigen::MatrixXf P_temp; 201 | P_temp = Eigen::MatrixXf(n_state,n_state); 202 | 203 | 204 | //ROS_INFO("size mk-1: %lu, size B: %lu",mk_minus_1.cols(), B.cols()); 205 | //ROS_INFO_STREAM("A:\n" << A << endl); 206 | for (int i = 0; i < mk_minus_1.cols(); i++) 207 | { 208 | // float qAcc = sqrt((mk_k_minus_1(1, i))*(mk_k_minus_1(1, i)) + (mk_k_minus_1(3, i)*(mk_k_minus_1(3, i)))); 209 | // qAcc = ((qAcc*2)+1); 210 | // ROS_INFO_STREAM("!!!!!!!!!!QACC = " << qAcc); 211 | Eigen::MatrixXf Q_temp = Q; 212 | Q_temp = Q;// * qAcc; 213 | // ROS_INFO("iteration: %d", i); 214 | // ROS_INFO_STREAM("B:\n" << B.block(0,n_input*i, n_state,n_input)); 215 | Bu_temp = B.block(0,n_input*i, n_state,n_input) * ang_vel_k; // 216 | // ROS_INFO_STREAM("Bu:\n" << Bu_temp); 217 | mk_minus_1.block(0,i, n_state,1) = A * mk_minus_1.block(0,i, n_state,1) + Bu_temp; 218 | P_temp = Pk_minus_1.block(0,n_state*i, n_state,n_state); 219 | pu = mk_minus_1(0,i); 220 | pv = mk_minus_1(2,i); 221 | 222 | F(0,0) = (dt_imu*omega_y*(2*cu - 2*pu))/f - (dt_imu*omega_x*(cv - pv))/f + 1; F(0,1) = dt; F(0,2) = dt_imu*omega_z - (dt_imu*omega_x*(cu - pu))/f; F(0,3) = 0; 223 | F(1,0) = 0; F(1,1) = 1; F(1,2) = 0; F(1,3) = 0; 224 | F(2,0) = (dt_imu*omega_y*(cv - pv))/f - dt_imu*omega_z; F(2,1) = 0; F(2,2) = (dt_imu*omega_y*(cu - pu))/f - (dt_imu*omega_x*(2*cv - 2*pv))/f + 1; F(2,3) = dt; 225 | F(3,0) = 0; F(3,1) = 0; F(3,2) = 0; F(3,3) = 1; 226 | 227 | P_temp = Q + F* P_temp * F.transpose(); 228 | Pk_minus_1.block(0,n_state*i, n_state,n_state) = P_temp; 229 | } 230 | 231 | X_k = mk_minus_1; 232 | wk_k_minus_1 = wk_minus_1; 233 | mk_k_minus_1 = mk_minus_1; 234 | Pk_k_minus_1 = Pk_minus_1; 235 | numTargets_Jk_k_minus_1 = numTargets_Jk_minus_1; 236 | 237 | ROS_INFO_STREAM("WK|K-1:\n" << wk_k_minus_1 << endl); 238 | ROS_INFO_STREAM("mK|K-1:\n" << mk_k_minus_1 << endl); 239 | ROS_INFO_STREAM("PK|K-1:\n" << Pk_k_minus_1 << endl); 240 | } 241 | 242 | 243 | void PhdFilter::phd_construct() 244 | { 245 | ROS_INFO("======= 2. construct ======= \n"); 246 | wk_k_minus_1 = wk_minus_1; 247 | mk_k_minus_1 = mk_minus_1; 248 | Pk_k_minus_1 = Pk_minus_1; 249 | numTargets_Jk_k_minus_1 = numTargets_Jk_minus_1; 250 | 251 | Eigen::MatrixXf PHt, HPHt, identity4; 252 | PHt = Eigen::MatrixXf(n_state,n_meas); 253 | HPHt = Eigen::MatrixXf(n_meas,n_meas); 254 | 255 | identity4 = Eigen::MatrixXf::Identity(4,4); 256 | 257 | for(int j = 0; j < numTargets_Jk_k_minus_1; j ++) 258 | { 259 | PHt = Pk_k_minus_1.block(0,n_state*j, n_state,n_state) * H.transpose(); 260 | HPHt = H*PHt; 261 | K.block(0,n_meas*j, n_state,n_meas) = PHt * (HPHt + R).inverse(); 262 | S.block(0,n_meas*j, n_meas,n_meas) = HPHt + R; 263 | Eigen::MatrixXf t1 = (identity4 - K.block(0,n_meas*j, n_state,n_meas)*H)*Pk_k_minus_1.block(0,n_state*j, n_state,n_state); 264 | P_k_k.block(0,n_state*j, n_state,n_state) = t1; 265 | } 266 | ROS_INFO_STREAM("m_k_minus_1:\n" << mk_minus_1 << "\n"); 267 | ROS_INFO_STREAM("P_k_k: " << endl << P_k_k << endl); 268 | ROS_INFO_STREAM("K: " << endl << K << endl); 269 | } 270 | 271 | void PhdFilter::phd_update() 272 | { 273 | ROS_INFO("======= 3. update ======= \n"); 274 | 275 | //1. set up matrix size 276 | mahalDistance = Eigen::MatrixXf::Zero(1,numTargets_Jk_k_minus_1 * detected_size_k + numTargets_Jk_k_minus_1); 277 | wk = Eigen::MatrixXf::Zero(1,numTargets_Jk_k_minus_1 * detected_size_k + numTargets_Jk_k_minus_1); 278 | mk = Eigen::MatrixXf::Zero(n_state,numTargets_Jk_k_minus_1 * detected_size_k + numTargets_Jk_k_minus_1); 279 | Pk = Eigen::MatrixXf::Zero(n_state, n_state * (numTargets_Jk_k_minus_1 * detected_size_k + numTargets_Jk_k_minus_1)); 280 | 281 | 282 | Eigen::MatrixXf thisZ, meanDelta_pdf, cov_pdf; 283 | Eigen::MatrixXf w_new, w_new_exponent; 284 | 285 | 286 | thisZ = Eigen::MatrixXf(n_meas,1); 287 | meanDelta_pdf = Eigen::MatrixXf(n_meas,1); 288 | cov_pdf = Eigen::MatrixXf(n_meas,n_meas); 289 | w_new = Eigen::MatrixXf(1,1); 290 | w_new_exponent = Eigen::MatrixXf(1,1); 291 | 292 | int index = 0; 293 | L = 0; 294 | 295 | for (int i = 0; i < numTargets_Jk_k_minus_1; i++ ) 296 | { 297 | wk(i) = (1-prob_detection) * wk_k_minus_1(i); 298 | mk.block(0,i, n_state,1) = mk_k_minus_1.block(0,i, n_state,1); 299 | Pk.block(0,n_state*i, n_state,n_state) = Pk_k_minus_1.block(0,n_state*i, n_state,n_state); 300 | mahalDistance(i) = 0; 301 | } 302 | 303 | for (int z = 0; z < detected_size_k; z++) 304 | { 305 | L = L+1; 306 | 307 | for (int j = 0; j < numTargets_Jk_k_minus_1; j++) 308 | { 309 | thisZ = Z_k.block(0,z, n_meas,1); 310 | 311 | index = (L) * numTargets_Jk_k_minus_1 + j; //3~11 312 | ROS_INFO("Index: %d, L: %d\n", index, L); 313 | //update weight (multivar prob distr) 314 | meanDelta_pdf = thisZ - H*mk_k_minus_1.block(0,j, n_state,1); 315 | ROS_INFO_STREAM("Mean Delta PDF: " << meanDelta_pdf << endl); 316 | 317 | cov_pdf = S.block(0,n_meas*j, n_meas,n_meas); 318 | ROS_INFO_STREAM("Cov PDF: " << cov_pdf << endl); 319 | w_new_exponent = -0.5 * (meanDelta_pdf.transpose() * cov_pdf.inverse() * meanDelta_pdf).transpose() * (meanDelta_pdf.transpose() * cov_pdf.inverse() * meanDelta_pdf) ; 320 | ROS_INFO_STREAM("W_new_exponent: " << w_new_exponent << endl); 321 | 322 | mahalDistance(index) = fabs(w_new_exponent(0)); 323 | 324 | double q_val = wk_k_minus_1(j) * pow(2*PI, -1/2) * pow(cov_pdf.determinant(),-0.5) * exp(w_new_exponent(0,0)); 325 | ROS_INFO_STREAM("q_val: " << q_val << endl); 326 | wk(index)= q_val; 327 | 328 | double w_numerator = prob_detection * wk_k_minus_1(j) * q_val; 329 | 330 | //update mean 331 | mk.block(0,index, n_state,1) = mk_k_minus_1.block(0,j, n_state,1); 332 | mk.block(0,index, n_state, 1) = mk_k_minus_1.block(0,j, n_state,1) + K.block(0,n_meas*j, n_state,n_meas)*meanDelta_pdf; 333 | //update cov 334 | Pk.block(0,n_state*index, n_state,n_state) = P_k_k.block(0,n_state*j, n_state,n_state); 335 | 336 | // ROS_INFO("wk: %f",wk(index)); 337 | // ROS_INFO_STREAM << "mk: "<< mk << endl; 338 | 339 | } 340 | 341 | ROS_INFO_STREAM("mk(pre): " << endl << mk << endl); 342 | ROS_INFO_STREAM("pk(pre): " << endl << Pk << endl); 343 | ROS_INFO_STREAM("wk(pre): " << endl << wk << endl); 344 | 345 | //normalize weights 346 | float weight_tally = 0; 347 | float old_weight; 348 | 349 | 350 | //sum weights 351 | for(int i = 0; i < numTargets_Jk_k_minus_1; i ++) 352 | { 353 | index = (L) * numTargets_Jk_k_minus_1 + i; 354 | weight_tally = weight_tally + wk(index); 355 | } 356 | 357 | 358 | //divide sum weights 359 | for(int i = 0; i < numTargets_Jk_k_minus_1; i ++) 360 | { 361 | index = (L) * numTargets_Jk_k_minus_1 + i; 362 | old_weight = wk(index); 363 | // float measZx = Z_k(0,i); 364 | // float measZy = Z_k(1,i); 365 | wk(index) = old_weight / (clutter_intensity(X_k(0,i),X_k(2,i))/1.0+ weight_tally); 366 | // wk(index) = old_weight / ( weight_tally); 367 | } 368 | 369 | } 370 | ROS_INFO_STREAM("Mahalanobis Distance: " << mahalDistance); 371 | ROS_INFO_STREAM("wk(post): " << endl << setprecision(3) << wk << endl); 372 | ROS_INFO_STREAM("mk(post): " << endl << setprecision(3) << mk << endl); 373 | ROS_INFO_STREAM("Pk(post): " << endl << setprecision(3) << Pk << endl); 374 | 375 | } 376 | 377 | void PhdFilter::phd_prune() 378 | { 379 | 380 | ROS_INFO_STREAM("======= 4.Pruning ======="); 381 | Eigen::MatrixXi::Index maxRow, maxCol; 382 | Eigen::MatrixXi I, I_copy; 383 | Eigen::MatrixXf I_weights; 384 | 385 | I = Eigen::MatrixXi(1, 0); 386 | I_copy = Eigen::MatrixXi(1, 0); 387 | I_weights = Eigen::MatrixXf(1, 0); 388 | 389 | int I_counter = 0; 390 | float weight_threshold = 0.1; 391 | float mahalanobis_threshold = 4.0; 392 | int l = 0; 393 | 394 | for(int i=0; i weight_threshold) 397 | { 398 | I_counter += 1; 399 | I.conservativeResize(1, I_counter); 400 | I_weights.conservativeResize(1, I_counter); 401 | I(0, I_counter-1) = i; 402 | I_weights(0, I_counter-1) = wk(i); 403 | } 404 | } 405 | I_copy = I; 406 | 407 | ROS_INFO_STREAM("I is:\n" << I << "\nWK is:\n" << wk); 408 | 409 | Eigen::MatrixXf wk_bar_fixed_k = Eigen::MatrixXf::Zero(1, 0); 410 | Eigen::MatrixXf mk_bar_fixed_k = Eigen::MatrixXf::Zero(4, 0); 411 | Eigen::MatrixXf Pk_bar_fixed_k = Eigen::MatrixXf::Zero(4, 0); 412 | Eigen::MatrixXi index_order = Eigen::MatrixXi::Zero(1, 0); 413 | 414 | Eigen::MatrixXf highWeights = Eigen::MatrixXf::Zero(1, I.cols()); 415 | 416 | Eigen::MatrixXi L = Eigen::MatrixXi::Zero(1, I.cols()); 417 | 418 | 419 | while(I.cols() != 0) 420 | { 421 | l++; 422 | ROS_INFO_STREAM("Current l: " << l); 423 | if(I_copy.cols() == 0) 424 | break; 425 | 426 | if(l > NUM_DRONES * n_meas) 427 | break; 428 | 429 | Eigen::MatrixXf highWeights = Eigen::MatrixXf::Zero(1, I.cols()); 430 | for(int i=0; i= NUM_DRONES) 567 | { 568 | newIndex(i) = newIndex(i) % NUM_DRONES; 569 | } 570 | } 571 | 572 | for(int i=0; i NUM_DRONES) 575 | { 576 | continue; 577 | } 578 | 579 | int sortedIndex = newIndex(i); 580 | wk_bar_fixed.block(0, sortedIndex, 1, 1) = wk_bar_fixed_k.block(0, i, 1, 1); 581 | mk_bar_fixed.block(0, sortedIndex, n_state, 1) = mk_bar_fixed_k.block(0, i, n_state, 1); 582 | Pk_bar_fixed.block(0, n_state*sortedIndex, n_state, n_state) = Pk_bar_fixed_k.block(0, n_state*i, n_state, n_state); 583 | } 584 | ROS_INFO_STREAM("wk_bar_fixed is:\n" << wk_bar_fixed << "\n"); 585 | ROS_INFO_STREAM("mk_bar_fixed is:\n" << mk_bar_fixed << "\n"); 586 | ROS_INFO_STREAM("Pk_bar_fixed is:\n" << Pk_bar_fixed << "\n"); 587 | 588 | numTargets_Jk_minus_1 = wk_bar_fixed.cols(); 589 | ROS_INFO_STREAM("numTargets_Jk_minus_1: " << numTargets_Jk_minus_1); 590 | 591 | } 592 | 593 | void PhdFilter::phd_state_extract() 594 | { 595 | 596 | ROS_INFO("============ 5. extract ============= "); 597 | Eigen::MatrixXf velocity, position; 598 | velocity = Eigen::MatrixXf(2,1); 599 | position = Eigen::MatrixXf(2,1); 600 | float gain_fine_tuned = 1.0; 601 | float weight_threshold_for_extraction = 0.5; 602 | 603 | ROS_INFO_STREAM("DT Cam: " << dt_cam << "\n"); 604 | //update state for next iterations 605 | 606 | 607 | 608 | // ROS_DEBUG_STREAM("--- X_k: " << endl << X_k << endl); 609 | // X_k = mk_minus_1; 610 | if(k_iteration > 3) 611 | { 612 | //update state for next iterations 613 | // wk_minus_1 = wk_bar_fixed; 614 | // mk_minus_1 = mk_bar_fixed; 615 | // Pk_minus_1 = Pk_bar_fixed.cwiseAbs(); 616 | 617 | // X_k = mk_minus_1; 618 | // cout << "--- X_k: " << endl << X_k << endl; 619 | for(int i=0; i 3) 655 | { 656 | for (int i = 0; i < wk_bar_fixed.cols(); i++) 657 | { 658 | position.block<1, 1>(0, 0) = (X_k.block<1,1>(0,i) - X_k_previous.block<1,1>(0,i)); 659 | position.block<1, 1>(1, 0) = (X_k.block<1,1>(2,i) - X_k_previous.block<1,1>(2,i)); 660 | velocity = position/ (dt_cam*gain_fine_tuned) ; 661 | mk_minus_1.block<1,1>(1,i) = velocity.block<1,1>(0,0); 662 | mk_minus_1.block<1,1>(3,i) = velocity.block<1,1>(1,0); 663 | ROS_INFO_STREAM("--- position: " << endl << position << endl); 664 | ROS_INFO_STREAM("--- dt_cam: " << endl << dt_cam << endl); 665 | ROS_INFO_STREAM("--- dt: " << endl << dt << endl); 666 | ROS_INFO_STREAM("--- velocity: " << endl << velocity << endl); 667 | } 668 | 669 | } 670 | wk_minus_1 = wk_bar_fixed; 671 | mk_minus_1 = mk_bar_fixed; 672 | Pk_minus_1 = Pk_bar_fixed.cwiseAbs(); 673 | X_k_previous = X_k; 674 | 675 | } 676 | 677 | 678 | 679 | float PhdFilter::clutter_intensity(const float ZmeasureX, const float ZmeasureY) 680 | { 681 | float xMin = 0; 682 | float xMax = 224; 683 | float yMin = 0; 684 | float yMax = 224; 685 | float uniform_dist = 0; 686 | float clutter_intensity = 0; 687 | float lambda = 12.5*pow(10,-8); 688 | float volume = 4*pow(10,6); 689 | 690 | if(ZmeasureX < xMin) return 0; 691 | else if (ZmeasureX > xMax) return 0; 692 | else if (ZmeasureY < yMin) return 0; 693 | else if (ZmeasureY > yMax) return 0; 694 | else 695 | { 696 | uniform_dist = 1 / ( (xMax - xMin)*(yMax - yMin) ); // Convert this to a constant initialized at startup 697 | 698 | } 699 | 700 | clutter_intensity = lambda * volume * uniform_dist; 701 | 702 | return clutter_intensity; 703 | } 704 | 705 | void PhdFilter::removeColumn(Eigen::MatrixXd& matrix, unsigned int colToRemove) 706 | { 707 | unsigned int numRows = matrix.rows(); 708 | unsigned int numCols = matrix.cols()-1; 709 | 710 | if( colToRemove < numCols ) 711 | matrix.block(0,colToRemove,numRows,numCols-colToRemove) = matrix.block(0,colToRemove+1,numRows,numCols-colToRemove); 712 | 713 | matrix.conservativeResize(numRows,numCols); 714 | } 715 | 716 | void PhdFilter::removeColumnf(Eigen::MatrixXf& matrix, unsigned int colToRemove) 717 | { 718 | unsigned int numRows = matrix.rows(); 719 | unsigned int numCols = matrix.cols()-1; 720 | 721 | if( colToRemove < numCols ) 722 | matrix.block(0,colToRemove,numRows,numCols-colToRemove) = matrix.block(0,colToRemove+1,numRows,numCols-colToRemove); 723 | 724 | matrix.conservativeResize(numRows,numCols); 725 | } 726 | 727 | void PhdFilter::removeColumni(Eigen::MatrixXi& matrix, unsigned int colToRemove) 728 | { 729 | unsigned int numRows = matrix.rows(); 730 | unsigned int numCols = matrix.cols()-1; 731 | 732 | if( colToRemove < numCols ) 733 | matrix.block(0,colToRemove,numRows,numCols-colToRemove) = matrix.block(0,colToRemove+1,numRows,numCols-colToRemove); 734 | 735 | matrix.conservativeResize(numRows,numCols); 736 | } 737 | 738 | void PhdFilter::update_A_matrix(float input_dt) 739 | { 740 | A << 1,input_dt,0,0, 741 | 0,1,0,0, 742 | 0,0,1,input_dt, 743 | 0,0,0,1; 744 | } 745 | 746 | void PhdFilter::update_F_matrix(float input_dt) 747 | { 748 | ROS_INFO_STREAM("This should not happen !\n"); 749 | return; 750 | } 751 | -------------------------------------------------------------------------------- /src/track.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | Track::Track(const float& x, const float& y, const float& vx, const float& vy, TrackerParam params) 5 | { 6 | 7 | /* 8 | * params.R << 2, 0, 0, 2; 9 | * params.T << 50000, 50000; 10 | * 11 | * params.P_0 << 20, 0, 0, 0, 12 | * 0, 20, 0, 0, 13 | * 0, 0, 8, 0, 14 | * 0, 0, 0, 8; 15 | * 16 | * params.principal_point << 489.72, 267.87; 17 | */ 18 | 19 | KF = std::shared_ptr(new Kalman(x, y, vx, vy, params)); 20 | life_time = 0; 21 | noDetections = 0; 22 | maxMissedRate = params.max_missed_rate; 23 | minAcceptanceRate = params.min_acceptance_rate; 24 | id = -1; 25 | } 26 | 27 | 28 | void Track::predict(float dt, Eigen::Vector3f omega) 29 | { 30 | KF->predict(dt, omega); 31 | } 32 | 33 | 34 | void Track::update(const std::vector detections, std::vector beta, double beta_0) 35 | { 36 | KF->update(detections, beta, beta_0); 37 | } 38 | 39 | 40 | cv::RotatedRect Track::get_error_ellipse(double chisquare_val){ 41 | 42 | //Get the eigenvalues and eigenvectors 43 | cv::Mat eigenvalues, eigenvectors; 44 | 45 | cv::Mat S_cv; 46 | 47 | eigen2cv(S(), S_cv); 48 | 49 | cv::eigen(S_cv, eigenvalues, eigenvectors); 50 | 51 | //Calculate the angle between the largest eigenvector and the x-axis 52 | double angle = atan2(-eigenvectors.at(0,1), eigenvectors.at(0,0)); // - minus sign because y axis is inverted 53 | 54 | //Shift the angle to the [0, 2pi] interval instead of [-pi, pi] 55 | if(angle < 0) 56 | angle += 2*M_PI; 57 | 58 | //Conver to degrees instead of radians 59 | angle = 180*angle/M_PI; 60 | 61 | //Calculate the size of the minor and major axes 62 | float halfmajoraxissize=chisquare_val*sqrt(eigenvalues.at(0)); 63 | float halfminoraxissize=chisquare_val*sqrt(eigenvalues.at(1)); 64 | 65 | //Return the oriented ellipse 66 | //The -angle is used because OpenCV defines the angle clockwise instead of anti-clockwise 67 | cv::Point2f mean((int)(get_z())(0), (int)(get_z())(1)); 68 | return cv::RotatedRect(mean, cv::Size2f(halfmajoraxissize, halfminoraxissize), -angle); 69 | 70 | 71 | 72 | } 73 | --------------------------------------------------------------------------------