├── .gitignore
├── include
    └── kf_tracker
    │   ├── CKalmanFilter.h
    │   └── featureDetection.h
├── LICENSE
├── CHANGELOG.rst
├── src
    ├── CKalmanFilter.cpp
    ├── featureDetection.cpp
    ├── main_naive.cpp
    └── main.cpp
├── package.xml
├── README.md
└── CMakeLists.txt


/.gitignore:
--------------------------------------------------------------------------------
1 | *.pro
2 | *~
3 | *.pro.user
4 | *.sw?
5 | 


--------------------------------------------------------------------------------
/include/kf_tracker/CKalmanFilter.h:
--------------------------------------------------------------------------------
 1 | #include <iostream>
 2 | #include <vector>
 3 | 
 4 | #include <opencv2/core/core.hpp>
 5 | #include <opencv2/highgui/highgui.hpp>
 6 | #include <opencv2/imgproc/imgproc.hpp>
 7 | #include <opencv2/video/tracking.hpp>
 8 | 
 9 | using namespace cv;
10 | using namespace std;
11 | 
12 | class CKalmanFilter
13 | {
14 | public:
15 | 	CKalmanFilter(vector<Vec2f>);
16 | 	~CKalmanFilter();
17 | 	vector<Vec2f> predict();
18 | 	vector<Vec2f> update(vector<Vec2f>);
19 | 
20 | 	KalmanFilter* kalman;
21 | 	vector<Vec2f> prevResult;
22 | 
23 | };


--------------------------------------------------------------------------------
/include/kf_tracker/featureDetection.h:
--------------------------------------------------------------------------------
 1 | #include <iostream>
 2 | #include <vector>
 3 | #include <fstream>
 4 | #include <string>
 5 | 
 6 | 
 7 | #include <opencv2/core/core.hpp>
 8 | #include <opencv2/highgui/highgui.hpp>
 9 | #include <opencv2/imgproc/imgproc.hpp>
10 | #include <opencv2/video/tracking.hpp>
11 | 
12 | using namespace cv;
13 | using namespace std;
14 | 
15 | class featureDetection{
16 | 
17 | public:
18 | 	featureDetection();
19 | 	~featureDetection();
20 | 	void filteringPipeLine(Mat);
21 | 	vector<Vec2f> houghTransform();
22 | 	Mat lineItr(Mat,vector<Vec2f>, string);
23 | 	int _width, _height;
24 | 
25 | protected:
26 | 	bool findIntersection(vector<Point>, Point&);
27 | 	vector<Point2f> ransac(vector<Point2f>);
28 | 	void visualize(Mat);
29 | 	vector<Vec2f> _lines;
30 | 	ofstream myfile;
31 | 	Mat _detectedEdges;
32 | 	int _LMWidth;
33 | 	int _thres;
34 | 	float _rho, _theta,_ransacThres, _houghThres;
35 | 	
36 | };
37 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2020 Praveen Palanisamy
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/CHANGELOG.rst:
--------------------------------------------------------------------------------
 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 2 | Changelog for package multi_object_tracking_lidar
 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 4 | 
 5 | 1.0.4 (2021-08-29)
 6 | ------------------
 7 | * Merge pull request `#46 <https://github.com/praveen-palanisamy/multiple-object-tracking-lidar/issues/46>`_ from praveen-palanisamy/rm-topic-slash-prefix
 8 |   Remove topic slash prefix
 9 | * Apply clang-format-10
10 | * Rm slash prefix (deprecated in TF2)
11 | * Add note to filter NaNs in input point clouds
12 | * Contributors: Praveen Palanisamy
13 | 
14 | 1.0.3 (2020-06-27)
15 | ------------------
16 | * Merge pull request #26 from artursg/noetic-devel
17 |   C++11 --> C++14 to allow compiling with later versions of PCL, ROS Neotic
18 | * Compiles under ROS Noetic
19 | * Merge pull request #25 from praveen-palanisamy/add-license-1
20 |   Add MIT LICENSE
21 | * Add LICENSE
22 | * Merge pull request #24 from mzahran001/patch-1
23 |   Fix broken hyperlink to wiki page in README
24 | * Fixing link error
25 | * Updated README to make clustering approach for 3D vs 2D clear #21
26 | * Added DOI and citing info
27 | * Contributors: Artur Sagitov, Mohamed Zahran, Praveen Palanisamy
28 | 
29 | 1.0.2 (2019-12-01)
30 | ------------------
31 | * Added link to wiki pages
32 | * Updated readme with suuported pointcloud sources
33 | * Updated README to clarify real, sim, dataset LiDAR data
34 | * Contributors: Praveen Palanisamy
35 | 
36 | 1.0.1 (2019-04-26)
37 | ------------------
38 | * Fixed cv_bridge build depend
39 | * Removed indirection op to be compatible with OpenCV 3+
40 | * Added visualization_msgs & cv_bridge build & run dependencies
41 | * Contributors: Praveen Palanisamy
42 | 
43 | 1.0.0 (2019-04-13)
44 | ------------------
45 | * Updated README with usage instructions
46 | * Renamed node name to kf_tracker to match bin name
47 | * Changed package name to multi_object_tracking_lidar
48 | * Updated package info & version num
49 | * Updated with a short demo on sample AV LIDAR scans
50 | * Added README with a short summary of the code
51 | * Working state of Multiple object stable tracking using Lidar scans with an extended Kalman Filter (rosrun kf_tracker tracker). A naive tracker is implemented in main_naive.cpp for comparison (rosrun kf_tracker naive_tracker).
52 | * v2. Unsupervised clustering is incorporated into the same node (tracker).
53 | * v1. Object ID/data association works. In this version PCL based unsupervised clustering is done separately.
54 | * Contributors: Praveen Palanisamy
55 | 


--------------------------------------------------------------------------------
/src/CKalmanFilter.cpp:
--------------------------------------------------------------------------------
 1 | #include <iostream>
 2 | #include "CKalmanFilter.h"
 3 | 
 4 | using namespace cv;
 5 | using namespace std;
 6 | 
 7 | // Constructor
 8 | CKalmanFilter::CKalmanFilter(vector<Vec2f> p){
 9 | 
10 | 	kalman = new KalmanFilter( 4, 4, 0 ); // 4 measurement and state parameters
11 | 	kalman->transitionMatrix = (Mat_<float>(4, 4) << 1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1);
12 | 
13 | 	// Initialization
14 | 	prevResult = p;
15 | 	kalman->statePre.at<float>(0) = p[0][0]; // r1
16 | 	kalman->statePre.at<float>(1) = p[0][1]; // theta1
17 | 	kalman->statePre.at<float>(2) = p[1][0]; // r2
18 | 	kalman->statePre.at<float>(3) = p[1][1]; // theta2
19 | 
20 | 	kalman->statePost.at<float>(0)=p[0][0];
21 | 	kalman->statePost.at<float>(1)=p[0][1];
22 | 	kalman->statePost.at<float>(2)=p[1][0];
23 | 	kalman->statePost.at<float>(3)=p[1][1];
24 | 
25 | 	setIdentity(kalman->measurementMatrix);
26 | 	setIdentity(kalman->processNoiseCov, Scalar::all(1e-4));
27 | 	setIdentity(kalman->measurementNoiseCov, Scalar::all(1e-1));
28 | 	setIdentity(kalman->errorCovPost, Scalar::all(5));
29 | }
30 | 
31 | // Destructor
32 | CKalmanFilter::~CKalmanFilter(){
33 | 	delete kalman;
34 | }
35 | 
36 | // Prediction
37 | vector<Vec2f> CKalmanFilter::predict(){
38 | 	Mat prediction = kalman->predict(); // predict the state of the next frame
39 | 	prevResult[0][0] = prediction.at<float>(0);prevResult[0][1] = prediction.at<float>(1);
40 | 	prevResult[1][0] = prediction.at<float>(2);prevResult[1][1] = prediction.at<float>(3);
41 | 	return prevResult;
42 | 
43 | }
44 | 
45 | // Correct the prediction based on the measurement
46 | vector<Vec2f> CKalmanFilter::update(vector<Vec2f> measure){
47 | 
48 | 	
49 | 	Mat_<float> measurement(4,1);
50 | 	measurement.setTo(Scalar(0));
51 | 
52 | 	measurement.at<float>(0) = measure[0][0];measurement.at<float>(1) = measure[0][1];
53 | 	measurement.at<float>(2) = measure[1][0];measurement.at<float>(3) = measure[1][1];
54 | 
55 | 	Mat estimated = kalman->correct(measurement); // Correct the state of the next frame after obtaining the measurements
56 | 	
57 | 	// Update the measurement	
58 | 	if(estimated.at<float>(0) < estimated.at<float>(2)){
59 | 		measure[0][0] = estimated.at<float>(0);measure[0][1] = estimated.at<float>(1);
60 | 		measure[1][0] = estimated.at<float>(2);measure[1][1] = estimated.at<float>(3);
61 | 	}
62 | 	else{
63 | 		measure[0][0] = estimated.at<float>(2);measure[0][1] = estimated.at<float>(3);
64 | 		measure[1][0] = estimated.at<float>(0);measure[1][1] = estimated.at<float>(1);
65 | 	}
66 | 
67 | 	waitKey(1);
68 | 	
69 | 	return measure; // return the measurement
70 | 
71 | }


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>multi_object_tracking_lidar</name>
 4 |   <version>1.0.4</version>
 5 |   <description>ROS package for Multiple objects detection, tracking and classification from LIDAR scans/point-clouds</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="praveen.palanisamy@outlook.com">Praveen Palanisamy</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>MIT</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/kf_tracker</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, mutiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintianers, but could be -->
27 |   <!-- Example: -->
28 |   <author email="praveen.palanisamy@outlook.com">Praveen Palanisamy</author> -->
29 | 
30 | 
31 |   <!-- The *_depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use build_depend for packages you need at compile time: -->
35 |   <!--   <build_depend>message_generation</build_depend> -->
36 |   <!-- Use buildtool_depend for build tool packages: -->
37 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
38 |   <!-- Use run_depend for packages you need at runtime: -->
39 |   <!--   <run_depend>message_runtime</run_depend> -->
40 |   <!-- Use test_depend for packages you need only for testing: -->
41 |   <!--   <test_depend>gtest</test_depend> -->
42 |   <buildtool_depend>catkin</buildtool_depend>
43 |   <build_depend>pcl_ros</build_depend>
44 |   <build_depend>roscpp</build_depend>
45 |   <build_depend>sensor_msgs</build_depend>
46 |   <build_depend>visualization_msgs</build_depend>
47 |   <build_depend>libpcl-all-dev</build_depend>
48 |   <build_depend>cv_bridge</build_depend>
49 | 
50 |   <run_depend>libpcl-all</run_depend>
51 |   <run_depend>pcl_ros</run_depend>
52 |   <run_depend>roscpp</run_depend>
53 |   <run_depend>sensor_msgs</run_depend>
54 |   <run_depend>visualization_msgs</run_depend>
55 |   <run_depend>cv_bridge</run_depend>
56 | 
57 | 
58 |   <!-- The export tag contains other, unspecified, tags -->
59 |   <export>
60 |     <!-- Other tools can request additional information be placed here -->
61 | 
62 |   </export>
63 | </package>
64 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Multiple objects detection, tracking and classification from LIDAR scans/point-clouds
 2 | 
 3 | 
 4 | [![DOI](https://zenodo.org/badge/47581608.svg)](https://zenodo.org/badge/latestdoi/47581608)
 5 | 
 6 | ![Sample demo of multiple object tracking using LIDAR scans](https://media.giphy.com/media/3YKG95w9gu263yQwDa/giphy.gif)
 7 | 
 8 | PCL based ROS package to Detect/Cluster --> Track --> Classify static and dynamic objects in real-time from LIDAR scans implemented in C++.
 9 | 
10 | ### Features:
11 | 
12 | - K-D tree based point cloud processing for object feature detection from point clouds
13 | - Unsupervised euclidean cluster extraction (3D) or k-means clustering based on detected features and refinement using RANSAC (2D)
14 | - Stable tracking (object ID & data association) with an ensemble of Kalman Filters 
15 | - Robust compared to k-means clustering with mean-flow tracking
16 | 
17 | ### Usage:
18 | 
19 | Follow the steps below to use this (`multi_object_tracking_lidar`) package:
20 | 
21 | 1. [Create a catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace) (if you do not have one setup already). 
22 | 1. Navigate to the `src` folder in your catkin workspace: `cd ~/catkin_ws/src`
23 | 1. Clone this repository: `git clone https://github.com/praveen-palanisamy/multiple-object-tracking-lidar.git`
24 | 1. Compile and build the package: `cd ~/catkin_ws && catkin_make`
25 | 1. Add the catkin workspace to your ROS environment: `source ~/catkin_ws/devel/setup.bash`
26 | 1. Run the `kf_tracker` ROS node in this package: `rosrun multi_object_tracking_lidar kf_tracker`
27 | 
28 | If all went well, the ROS node should be up and running! As long as you have the point clouds published on to the `filtered_cloud` rostopic, you should see outputs from this node published onto the `obj_id`, `cluster_0`, `cluster_1`, …, `cluster_5` topics along with the markers on `viz` topic which you can visualize using RViz.
29 | 
30 | ### Supported point-cloud streams/sources:
31 | The input point-clouds can be from:
32 | 1. A real LiDAR or 
33 | 2. A simulated LiDAR or 
34 | 3. A point cloud dataset or 
35 | 4. Any other data source that produces point clouds
36 | 
37 | **Note:** This package expects valid point cloud data as input. The point clouds you publish to the "`filtered_cloud`" is **not** expected to contain NaNs. The point cloud filtering is somewhat task and application dependent and therefore it is not done by this module. 
38 | PCL library provides `pcl::removeNaNFromPointCloud (...)` method  to filter out NaN points. You can refer to [this example code snippet](https://github.com/praveen-palanisamy/multiple-object-tracking-lidar/issues/29#issuecomment-672098760) to easily filter out NaN points in your point cloud.
39 | 
40 | ## Citing
41 | 
42 | If you use the code or snippets from this repository in your work, please cite:
43 | 
44 | ```bibtex
45 | @software{praveen_palanisamy_2019_3559187,
46 |   author       = {Praveen Palanisamy},
47 |   title        = {{praveen-palanisamy/multiple-object-tracking-lidar: 
48 |                    Multiple-Object-Tracking-from-Point-Clouds_v1.0.2}},
49 |   month        = dec,
50 |   year         = 2019,
51 |   publisher    = {Zenodo},
52 |   version      = {1.0.2},
53 |   doi          = {10.5281/zenodo.3559187},
54 |   url          = {https://doi.org/10.5281/zenodo.3559186}
55 | }
56 | ```
57 | 
58 | ### Wiki
59 | 
60 | [Checkout the Wiki pages](https://github.com/praveen-palanisamy/multiple-object-tracking-lidar/wiki)
61 | 
62 | 1. [Multiple-object tracking from pointclouds using a Velodyne VLP-16](https://github.com/praveen-palanisamy/multiple-object-tracking-lidar/wiki/velodyne_vlp16)
63 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 2.8.3)
  2 | project(multi_object_tracking_lidar)
  3 | 
  4 | set(CMAKE_CXX_STANDARD 14)
  5 | ## Find catkin macros and libraries
  6 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
  7 | ## is used, also find other catkin packages
  8 | find_package(catkin REQUIRED COMPONENTS
  9 |   pcl_ros
 10 |   roscpp
 11 |   sensor_msgs
 12 | )
 13 | find_package( OpenCV REQUIRED )
 14 | 
 15 | ## System dependencies are found with CMake's conventions
 16 | # find_package(Boost REQUIRED COMPONENTS system)
 17 | 
 18 | 
 19 | ## Uncomment this if the package has a setup.py. This macro ensures
 20 | ## modules and global scripts declared therein get installed
 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
 22 | # catkin_python_setup()
 23 | 
 24 | ################################################
 25 | ## Declare ROS messages, services and actions ##
 26 | ################################################
 27 | 
 28 | ## To declare and build messages, services or actions from within this
 29 | ## package, follow these steps:
 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in
 31 | ##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
 32 | ## * In the file package.xml:
 33 | ##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
 34 | ##   * If MSG_DEP_SET isn't empty the following dependencies might have been
 35 | ##     pulled in transitively but can be declared for certainty nonetheless:
 36 | ##     * add a build_depend tag for "message_generation"
 37 | ##     * add a run_depend tag for "message_runtime"
 38 | ## * In this file (CMakeLists.txt):
 39 | ##   * add "message_generation" and every package in MSG_DEP_SET to
 40 | ##     find_package(catkin REQUIRED COMPONENTS ...)
 41 | ##   * add "message_runtime" and every package in MSG_DEP_SET to
 42 | ##     catkin_package(CATKIN_DEPENDS ...)
 43 | ##   * uncomment the add_*_files sections below as needed
 44 | ##     and list every .msg/.srv/.action file to be processed
 45 | ##   * uncomment the generate_messages entry below
 46 | ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 47 | 
 48 | ## Generate messages in the 'msg' folder
 49 | # add_message_files(
 50 | #   FILES
 51 | #   Message1.msg
 52 | #   Message2.msg
 53 | # )
 54 | 
 55 | ## Generate services in the 'srv' folder
 56 | # add_service_files(
 57 | #   FILES
 58 | #   Service1.srv
 59 | #   Service2.srv
 60 | # )
 61 | 
 62 | ## Generate actions in the 'action' folder
 63 | # add_action_files(
 64 | #   FILES
 65 | #   Action1.action
 66 | #   Action2.action
 67 | # )
 68 | 
 69 | ## Generate added messages and services with any dependencies listed here
 70 | # generate_messages(
 71 | #   DEPENDENCIES
 72 | #   sensor_msgs
 73 | # )
 74 | 
 75 | ###################################
 76 | ## catkin specific configuration ##
 77 | ###################################
 78 | ## The catkin_package macro generates cmake config files for your package
 79 | ## Declare things to be passed to dependent projects
 80 | ## INCLUDE_DIRS: uncomment this if you package contains header files
 81 | ## LIBRARIES: libraries you create in this project that dependent projects also need
 82 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 83 | ## DEPENDS: system dependencies of this project that dependent projects also need
 84 | catkin_package(
 85 | #  INCLUDE_DIRS include
 86 | #  LIBRARIES kf_tracker
 87 | #  CATKIN_DEPENDS pck_ros roscpp sensor_msgs
 88 | #  DEPENDS system_lib
 89 | )
 90 | 
 91 | ###########
 92 | ## Build ##
 93 | ###########
 94 | 
 95 | ## Specify additional locations of header files
 96 | ## Your package locations should be listed before other locations
 97 | # include_directories(include)
 98 | include_directories(
 99 |   ${catkin_INCLUDE_DIRS}
100 |   ${OpenCV_INCLUDE_DIRS}
101 |   include
102 | )
103 | 
104 | 
105 | ## Declare a cpp library
106 | # add_library(kf_tracker
107 | #   src/${PROJECT_NAME}/kf_tracker.cpp
108 | # )
109 | 
110 | ## Declare a cpp executable
111 | # add_executable(kf_tracker_node src/kf_tracker_node.cpp)
112 | add_executable( kf_tracker src/main.cpp )
113 | target_link_libraries ( kf_tracker ${OpenCV_LIBRARIES} ${catkin_LIBRARIES})
114 |  
115 | ## Add cmake target dependencies of the executable/library
116 | ## as an example, message headers may need to be generated before nodes
117 | # add_dependencies(kf_tracker_node kf_tracker_generate_messages_cpp)
118 | 
119 | ## Specify libraries to link a library or executable target against
120 | # target_link_libraries(kf_tracker_node
121 | #   ${catkin_LIBRARIES}
122 | # )
123 | 
124 | #############
125 | ## Install ##
126 | #############
127 | 
128 | # all install targets should use catkin DESTINATION variables
129 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
130 | 
131 | ## Mark executable scripts (Python etc.) for installation
132 | ## in contrast to setup.py, you can choose the destination
133 | # install(PROGRAMS
134 | #   scripts/my_python_script
135 | #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
136 | # )
137 | 
138 | ## Mark executables and/or libraries for installation
139 | install(TARGETS kf_tracker
140 | 	ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
141 | 	LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
142 | 	RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
143 | )
144 | 
145 | ## Mark cpp header files for installation
146 | # install(DIRECTORY include/${PROJECT_NAME}/
147 | #   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
148 | #   FILES_MATCHING PATTERN "*.h"
149 | #   PATTERN ".svn" EXCLUDE
150 | # )
151 | 
152 | ## Mark other files for installation (e.g. launch and bag files, etc.)
153 | # install(FILES
154 | #   # myfile1
155 | #   # myfile2
156 | #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
157 | # )
158 | 
159 | #############
160 | ## Testing ##
161 | #############
162 | 
163 | ## Add gtest based cpp test target and link libraries
164 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_kf_tracker.cpp)
165 | # if(TARGET ${PROJECT_NAME}-test)
166 | #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
167 | # endif()
168 | 
169 | ## Add folders to be run by python nosetests
170 | # catkin_add_nosetests(test)
171 | 


--------------------------------------------------------------------------------
/src/featureDetection.cpp:
--------------------------------------------------------------------------------
  1 | #include <iostream>
  2 | #include <string.h>
  3 | #include "kf_tracker/featureDetection.h"
  4 | #include "kf_tracker/CKalmanFilter.h"
  5 | 
  6 | using namespace cv;
  7 | 
  8 | featureDetection::featureDetection(){
  9 | 
 10 | 	_detectedEdges = Mat();
 11 | 	_width = 800;
 12 | 	_height = 600;
 13 | 	_LMWidth = 10;
 14 | 	_thres = 40;
 15 | 	_rho = 1;
 16 | 	_theta = CV_PI/180.0;
 17 | 	_houghThres =100;
 18 | 	_ransacThres = 0.01;
 19 | }
 20 | 
 21 | featureDetection::~featureDetection(){
 22 | 
 23 | }
 24 | 
 25 | //////////
 26 | 
 27 | // This is just one approach. Other approaches can be Edge detection, Connected Components, etc.
 28 | void featureDetection::filteringPipeLine(Mat src){
 29 | 	Mat img;
 30 | 	
 31 | 	///// Generating the mask to mask the top half of the image
 32 | 	Mat mask = Mat(src.size(), CV_8UC1, Scalar(1));
 33 | 	for(int i = 0;i < mask.rows/2; i++){
 34 | 		for(int j = 0;j < mask.cols;j++){
 35 | 			mask.at<uchar>(Point(j,i)) = 0;
 36 | 		}
 37 | 	}
 38 | 	src.copyTo(img,mask);
 39 | 	/////
 40 | 	
 41 | 	_detectedEdges = Mat(img.size(),CV_8UC1); // detectedEdges
 42 | 	_detectedEdges.setTo(0);
 43 | 
 44 | 	int val = 0;
 45 | 	// iterating through each row
 46 | 	for (int j = img.rows/2;j<img.rows;j++){
 47 | 		unsigned char *imgptr = img.ptr<uchar>(j);
 48 | 		unsigned char *detEdgesptr = _detectedEdges.ptr<uchar>(j);
 49 | 
 50 | 		// iterating through each column seeing the difference among columns which are "width" apart
 51 | 		for (int i = _LMWidth;i < img.cols - _LMWidth; ++i){
 52 | 			if(imgptr[i]!= 0){
 53 | 				val = 2*imgptr[i];
 54 | 				val += -imgptr[i - _LMWidth];
 55 | 				val += -imgptr[i + _LMWidth];
 56 | 				val += -abs((int)(imgptr[i - _LMWidth] - imgptr[i + _LMWidth]));
 57 | 
 58 | 				val = (val<0)?0:val;
 59 | 				val = (val>255)?255:val;
 60 | 
 61 | 				detEdgesptr[i] = (unsigned char) val;
 62 | 			}
 63 | 		}
 64 | 	}
 65 | 
 66 | 	// Thresholding
 67 | 	threshold(_detectedEdges,_detectedEdges,_thres,255,0);
 68 | 
 69 | }
 70 | //////////
 71 | 
 72 | // Performing Hough Transform
 73 | vector<Vec2f> featureDetection::houghTransform(){
 74 | 
 75 | 	Mat _detectedEdgesRGB;
 76 | 	cvtColor(_detectedEdges,_detectedEdgesRGB, CV_GRAY2BGR); // converting to RGB
 77 | 	HoughLines(_detectedEdges,_lines,_rho,_theta,_houghThres); // Finding the hough lines
 78 | 	vector<Vec2f> retVar;
 79 | 	
 80 | 	if (_lines.size() > 1){
 81 | 		Mat labels,centers;
 82 | 		Mat samples = Mat(_lines.size(),2,CV_32F);
 83 | 
 84 | 		for (int i = 0;i < _lines.size();i++){
 85 | 			samples.at<float>(i,0) = _lines[i][0];
 86 | 			samples.at<float>(i,1) = _lines[i][1];
 87 | 		}
 88 | 		// K means clustering to get two lines
 89 | 		kmeans(samples, 2, labels, TermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 1000, 0.001), 5, KMEANS_PP_CENTERS, centers );
 90 | 
 91 | 		////////////////// Using RANSAC to get rid of outliers
 92 | 		_lines.clear();
 93 | 		
 94 | 		vector<Point2f> left;
 95 | 		vector<Point2f> right;
 96 | 		for(int i = 0;i < labels.rows; i++){
 97 | 			if (labels.at<int>(i) == 0) left.push_back(Point2f(samples.at<float>(i,0), samples.at<float>(i,1)));
 98 | 			else right.push_back(Point2f(samples.at<float>(i,0), samples.at<float>(i,1)));
 99 | 		}
100 | 		// Performing Ransac
101 | 		vector<Point2f> leftR = ransac(left); 
102 | 		vector<Point2f> rightR = ransac(right);
103 | 		//////////////////
104 | 
105 | 		if (leftR.size() < 1 || rightR.size() < 1 || 
106 | 		   (float)(cos((leftR[0].y + leftR[1].y)/2) * cos((rightR[0].y + rightR[1].y)/2)) >= 0) return retVar;
107 | 
108 | 		// pushing the end points of the line to _lines
109 | 		_lines.push_back(Vec2f((leftR[0].x + leftR[1].x)/2, (leftR[0].y + leftR[1].y)/2));
110 | 		_lines.push_back(Vec2f((rightR[0].x + rightR[1].x)/2, (rightR[0].y + rightR[1].y)/2));
111 | 
112 | 	}
113 | 
114 | 
115 | 	return _lines;
116 | }
117 | 
118 | // Implementing RANSAC to remove outlier lines
119 | // Picking the best estimate having maximum number of inliers
120 | // TO DO: Better implementation 
121 | vector<Point2f> featureDetection::ransac(vector<Point2f> data){
122 | 
123 | 	vector<Point2f> res;
124 | 	int maxInliers = 0;
125 | 
126 | 	// Picking up the first sample
127 | 	for(int i = 0;i < data.size();i++){
128 | 		Point2f p1 = data[i];
129 | 	
130 | 		// Picking up the second sample
131 | 		for(int j = i + 1;j < data.size();j++){
132 | 			Point2f p2 = data[j];
133 | 			int n = 0;
134 | 			
135 | 			// Finding the total number of inliers
136 | 			for (int k = 0;k < data.size();k++){
137 | 				Point2f p3 = data[k];
138 | 				float normalLength = norm(p2 - p1);
139 | 				float distance = abs((float)((p3.x - p1.x) * (p2.y - p1.y) - (p3.y - p1.y) * (p2.x - p1.x)) / normalLength);
140 | 				if (distance < _ransacThres) n++;
141 | 			}
142 | 			
143 | 			// if the current selection has more inliers, update the result and maxInliers
144 | 			if (n > maxInliers) {
145 | 				res.clear();
146 | 				maxInliers = n;			
147 | 				res.push_back(p1);
148 | 				res.push_back(p2);
149 | 			}
150 | 		
151 | 		}
152 | 		
153 | 	}
154 | 
155 | 	return res;
156 | }
157 | 
158 | 
159 | Mat featureDetection::lineItr(Mat img, vector<Vec2f> lines, string name){
160 | 
161 | 	Mat imgRGB;
162 | 	cvtColor(img,imgRGB,CV_GRAY2RGB); // converting the image to RGB for display
163 | 	vector<Point> endPoints;
164 | 
165 | 	// Here, I convert the polar coordinates to Cartesian coordinates.
166 | 	// Then, I extend the line to meet the boundary of the image.
167 | 	for (int i = 0;i < lines.size();i++){
168 | 		float r = lines[i][0];
169 | 		float t = lines[i][1];
170 | 		
171 | 		float x = r*cos(t);
172 | 		float y = r*sin(t);
173 | 
174 | 		Point p1(cvRound(x - 1.0*sin(t)*1000), cvRound(y + cos(t)*1000));
175 | 		Point p2(cvRound(x + 1.0*sin(t)*1000), cvRound(y - cos(t)*1000));
176 | 
177 | 		clipLine(img.size(),p1,p2);
178 | 		if (p1.y > p2.y){
179 | 			endPoints.push_back(p1);
180 | 			endPoints.push_back(p2);
181 | 		}
182 | 		else{
183 | 			endPoints.push_back(p2);
184 | 			endPoints.push_back(p1);
185 | 		}
186 | 
187 | 	}
188 | 
189 | 	
190 | 	Point pint;
191 | 	bool check = findIntersection(endPoints,pint);
192 | 
193 | 	if (check){
194 | 		line(imgRGB,endPoints[0],pint,Scalar(0,0,255),5);
195 | 		line(imgRGB,endPoints[2],pint,Scalar(0,0,255),5);
196 | 	}	
197 | 	/////
198 | 
199 | 	
200 | 	float xIntercept = min(endPoints[0].x,endPoints[2].x);
201 | 	myfile << name << "," << xIntercept * 2 << "," << pint.x * 2 << endl;
202 | 
203 | 	visualize(imgRGB); // Visualize the final result
204 | 
205 | 	return imgRGB;
206 | }
207 | 
208 | // Finding the Vanishing Point
209 | bool featureDetection::findIntersection(vector<Point> endP, Point& pi){
210 | 
211 | 	Point x = endP[2] - endP[0];
212 | 	Point d1 = endP[1] - endP[0];
213 | 	Point d2 = endP[3] - endP[2];
214 | 	
215 | 	float cross = d1.x*d2.y - d1.y*d2.x;
216 | 	if (abs(cross) < 1e-8) // No intersection
217 |         return false;
218 | 
219 |     double t1 = (x.x * d2.y - x.y * d2.x)/cross;
220 |     pi = endP[0] + d1 * t1;
221 |     return true;
222 | 
223 | 
224 | }
225 | 
226 | // Visualize
227 | void featureDetection::visualize(Mat imgRGB){
228 | 	
229 | 	namedWindow("detectedFeatures");
230 | 	imshow("detectedFeatures",imgRGB);
231 | 	waitKey(100);
232 | 
233 | }
234 | 
235 | #ifdef testT
236 | int main()
237 | {
238 | 	featureDetection detect; // object of featureDetection class
239 | 
240 | 
241 | 	ippath += imname;
242 | 	Mat img1 = imread(ippath,0); // Read the image
243 | 	resize(img1,img1,Size(detect._width,detect._height));
244 | 	
245 | 	detect.filteringPipeLine(img1); 
246 | 	vector<Vec2f> lines = detect.houghTransform(); // Hough Transform
247 | 	Mat imgFinal = detect.lineItr(img1, lines, imname); 
248 | 	
249 | 	oppath += imname;
250 | 	imwrite(oppath,imgFinal); 
251 | 
252 | 	while ( getline (imageNames,imname) ){
253 | 		ippath = "./images/";
254 | 		oppath = "./output/";
255 | 		ippath += imname;
256 | 
257 | 		Mat img2 = imread(ippath,0); // Read the image
258 | 		resize(img2,img2,Size(detect._width,detect._height));
259 | 		
260 | 		detect.filteringPipeLine(img2); 
261 | 		vector<Vec2f> lines2 = detect.houghTransform(); // Hough Transform
262 | 		
263 | 		
264 | 		
265 | 		if (lines2.size() < 2) {
266 | 			imgFinal = detect.lineItr(img2,lines, imname);
267 | 			oppath += imname;
268 | 			imwrite(oppath,imgFinal); 
269 | 			continue;
270 | 		}
271 | 		
272 | 		///// Kalman Filter to predict the next state
273 | 		CKalmanFilter KF2(lines);
274 | 		vector<Vec2f> pp = KF2.predict();
275 | 
276 | 		vector<Vec2f> lines2Final = KF2.update(lines2);
277 | 		lines = lines2Final;
278 | 		imgFinal = detect.lineItr(img2,lines2, imname); 
279 | 		/////
280 | 		
281 | 		oppath += imname;
282 | 		imwrite(oppath,imgFinal);
283 | 	}
284 | 	
285 | 
286 | }
287 | #endif
288 | 


--------------------------------------------------------------------------------
/src/main_naive.cpp:
--------------------------------------------------------------------------------
  1 | #include "kf_tracker/CKalmanFilter.h"
  2 | #include "kf_tracker/featureDetection.h"
  3 | #include "opencv2/video/tracking.hpp"
  4 | #include "pcl_ros/point_cloud.h"
  5 | #include <algorithm>
  6 | #include <fstream>
  7 | #include <geometry_msgs/Point.h>
  8 | #include <iostream>
  9 | #include <iterator>
 10 | #include <opencv2/core/core.hpp>
 11 | #include <opencv2/highgui/highgui.hpp>
 12 | #include <opencv2/imgproc/imgproc.hpp>
 13 | #include <opencv2/video/video.hpp>
 14 | #include <pcl/io/pcd_io.h>
 15 | #include <pcl/point_types.h>
 16 | #include <ros/ros.h>
 17 | #include <std_msgs/Float32MultiArray.h>
 18 | #include <std_msgs/Int32MultiArray.h>
 19 | #include <string.h>
 20 | 
 21 | #include <pcl/common/geometry.h>
 22 | #include <pcl/features/normal_3d.h>
 23 | #include <pcl/filters/extract_indices.h>
 24 | #include <pcl/filters/voxel_grid.h>
 25 | #include <pcl/kdtree/kdtree.h>
 26 | #include <pcl/point_cloud.h>
 27 | #include <pcl/point_types.h>
 28 | #include <pcl/sample_consensus/method_types.h>
 29 | #include <pcl/sample_consensus/model_types.h>
 30 | #include <pcl/segmentation/extract_clusters.h>
 31 | #include <pcl/segmentation/sac_segmentation.h>
 32 | #include <pcl_conversions/pcl_conversions.h>
 33 | #include <sensor_msgs/PointCloud2.h>
 34 | 
 35 | using namespace std;
 36 | using namespace cv;
 37 | 
 38 | ros::Publisher objID_pub;
 39 | 
 40 | // KF init
 41 | int stateDim = 4; // [x,y,v_x,v_y]//,w,h]
 42 | int measDim = 2;  // [z_x,z_y,z_w,z_h]
 43 | int ctrlDim = 0;
 44 | cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F);
 45 | cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F);
 46 | cv::KalmanFilter KF2(stateDim, measDim, ctrlDim, CV_32F);
 47 | cv::KalmanFilter KF3(stateDim, measDim, ctrlDim, CV_32F);
 48 | cv::KalmanFilter KF4(stateDim, measDim, ctrlDim, CV_32F);
 49 | cv::KalmanFilter KF5(stateDim, measDim, ctrlDim, CV_32F);
 50 | 
 51 | ros::Publisher pub_cluster0;
 52 | ros::Publisher pub_cluster1;
 53 | ros::Publisher pub_cluster2;
 54 | ros::Publisher pub_cluster3;
 55 | ros::Publisher pub_cluster4;
 56 | ros::Publisher pub_cluster5;
 57 | 
 58 | std::vector<geometry_msgs::Point> prevClusterCenters;
 59 | 
 60 | cv::Mat state(stateDim, 1, CV_32F);
 61 | cv::Mat_<float> measurement(2, 1);
 62 | 
 63 | std::vector<int> objID; // Output of the data association using KF
 64 |                         // measurement.setTo(Scalar(0));
 65 | 
 66 | bool firstFrame = true;
 67 | 
 68 | // calculate euclidean distance of two points
 69 | double euclidean_distance(geometry_msgs::Point &p1, geometry_msgs::Point &p2) {
 70 |   return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) +
 71 |               (p1.z - p2.z) * (p1.z - p2.z));
 72 | }
 73 | /*
 74 | //Count unique object IDs. just to make sure same ID has not been assigned to
 75 | two KF_Trackers. int countIDs(vector<int> v)
 76 | {
 77 |     transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n =
 78 | distance(v.end(), v.begin()) sort(v.begin(), v.end()); // Average case O(n log
 79 | n), worst case O(n^2) (usually implemented as quicksort.
 80 |     // To guarantee worst case O(n log n) replace with make_heap, then
 81 | sort_heap.
 82 | 
 83 |     // Unique will take a sorted range, and move things around to get duplicated
 84 |     // items to the back and returns an iterator to the end of the unique
 85 | section of the range auto unique_end = unique(v.begin(), v.end()); // Again n
 86 | comparisons return distance(unique_end, v.begin()); // Constant time for random
 87 | access iterators (like vector's)
 88 | }
 89 | */
 90 | 
 91 | /*
 92 | 
 93 | objID: vector containing the IDs of the clusters that should be associated with
 94 | each KF_Tracker objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc.
 95 | */
 96 | void KFT(const std_msgs::Float32MultiArray ccs) {
 97 | 
 98 |   // First predict, to update the internal statePre variable
 99 | 
100 |   std::vector<cv::Mat> pred{KF0.predict(), KF1.predict(), KF2.predict(),
101 |                             KF3.predict(), KF4.predict(), KF5.predict()};
102 |   // cout<<"Pred successfull\n";
103 | 
104 |   // cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
105 |   // cout<<"Prediction 1
106 |   // ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";
107 | 
108 |   // Get measurements
109 |   // Extract the position of the clusters forom the multiArray. To check if the
110 |   // data coming in, check the .z (every third) coordinate and that will be 0.0
111 |   std::vector<geometry_msgs::Point> clusterCenters; // clusterCenters
112 | 
113 |   int i = 0;
114 |   for (std::vector<float>::const_iterator it = ccs.data.begin();
115 |        it != ccs.data.end(); it += 3) {
116 |     geometry_msgs::Point pt;
117 |     pt.x = *it;
118 |     pt.y = *(it + 1);
119 |     pt.z = *(it + 2);
120 | 
121 |     clusterCenters.push_back(pt);
122 |   }
123 | 
124 |   //  cout<<"CLusterCenters Obtained"<<"\n";
125 |   std::vector<geometry_msgs::Point> KFpredictions;
126 |   i = 0;
127 |   for (auto it = pred.begin(); it != pred.end(); it++) {
128 |     geometry_msgs::Point pt;
129 |     pt.x = (*it).at<float>(0);
130 |     pt.y = (*it).at<float>(1);
131 |     pt.z = (*it).at<float>(2);
132 | 
133 |     KFpredictions.push_back(pt);
134 |   }
135 |   // cout<<"Got predictions"<<"\n";
136 | 
137 |   /* Original Version using Kalman filter prediction
138 | 
139 |   // Find the cluster that is more probable to be belonging to a given KF.
140 |   objID.clear();//Clear the objID vector
141 |   for(int filterN=0;filterN<6;filterN++)
142 |   {
143 |       std::vector<float> distVec;
144 |       for(int n=0;n<6;n++)
145 |           distVec.push_back(euclidean_distance(KFpredictions[filterN],clusterCenters[n]));
146 | 
147 |     //
148 |  cout<<"distVec[="<<distVec[0]<<","<<distVec[1]<<","<<distVec[2]<<","<<distVec[3]<<","<<distVec[4]<<","<<distVec[5]<<"\n";
149 |       objID.push_back(std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end())));
150 |      // cout<<"MinD for
151 |  filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
152 | 
153 |   }
154 | 
155 |  // cout<<"Got object IDs"<<"\n";
156 |   //countIDs(objID);// for verif/corner cases
157 |     Original version using kalman filter prediction
158 |   */
159 |   // display objIDs
160 |   /* DEBUG
161 |     cout<<"objID= ";
162 |     for(auto it=objID.begin();it!=objID.end();it++)
163 |         cout<<*it<<" ,";
164 |     cout<<"\n";
165 |     */
166 | 
167 |   /* Naive version without using kalman filter */
168 |   objID.clear(); // Clear the objID vector
169 |   for (int filterN = 0; filterN < 6; filterN++) {
170 |     std::vector<float> distVec;
171 |     for (int n = 0; n < 6; n++)
172 |       distVec.push_back(
173 |           euclidean_distance(prevClusterCenters[n], clusterCenters[n]));
174 | 
175 |     // cout<<"distVec[="<<distVec[0]<<","<<distVec[1]<<","<<distVec[2]<<","<<distVec[3]<<","<<distVec[4]<<","<<distVec[5]<<"\n";
176 |     objID.push_back(std::distance(distVec.begin(),
177 |                                   min_element(distVec.begin(), distVec.end())));
178 |     // cout<<"MinD for
179 |     // filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
180 |   }
181 |   /* Naive version without kalman filter */
182 |   // prevClusterCenters.clear();
183 |   // Set the current associated clusters to the prevClusterCenters
184 |   for (int i = 0; i < 6; i++) {
185 |     prevClusterCenters[objID.at(i)] = clusterCenters.at(i);
186 |   }
187 | 
188 |   /* Naive version without kalman filter */
189 | 
190 |   /* Naive version without using kalman filter */
191 | 
192 |   std_msgs::Int32MultiArray obj_id;
193 |   for (auto it = objID.begin(); it != objID.end(); it++)
194 |     obj_id.data.push_back(*it);
195 |   objID_pub.publish(obj_id);
196 |   // convert clusterCenters from geometry_msgs::Point to floats
197 |   std::vector<std::vector<float>> cc;
198 |   for (int i = 0; i < clusterCenters.size(); i++) {
199 |     vector<float> pt;
200 |     pt.push_back(clusterCenters[i].x);
201 |     pt.push_back(clusterCenters[i].y);
202 |     pt.push_back(clusterCenters[i].z);
203 | 
204 |     cc.push_back(pt);
205 |   }
206 |   // cout<<"cc[5][0]="<<cc[5].at(0)<<"cc[5][1]="<<cc[5].at(1)<<"cc[5][2]="<<cc[5].at(2)<<"\n";
207 |   float meas0[3] = {cc[0].at(0), cc[0].at(1)};
208 |   float meas1[3] = {cc[1].at(0), cc[1].at(1)};
209 |   float meas2[3] = {cc[2].at(0), cc[2].at(1)};
210 |   float meas3[3] = {cc[3].at(0), cc[3].at(1)};
211 |   float meas4[3] = {cc[4].at(0), cc[4].at(1)};
212 |   float meas5[3] = {cc[5].at(0), cc[5].at(1)};
213 | 
214 |   // The update phase
215 |   cv::Mat meas0Mat = cv::Mat(2, 1, CV_32F, meas0);
216 |   cv::Mat meas1Mat = cv::Mat(2, 1, CV_32F, meas1);
217 |   cv::Mat meas2Mat = cv::Mat(2, 1, CV_32F, meas2);
218 |   cv::Mat meas3Mat = cv::Mat(2, 1, CV_32F, meas3);
219 |   cv::Mat meas4Mat = cv::Mat(2, 1, CV_32F, meas4);
220 |   cv::Mat meas5Mat = cv::Mat(2, 1, CV_32F, meas5);
221 | 
222 |   // cout<<"meas0Mat"<<meas0Mat<<"\n";
223 | 
224 |   Mat estimated0 = KF0.correct(meas0Mat);
225 |   Mat estimated1 = KF0.correct(meas1Mat);
226 |   Mat estimated2 = KF0.correct(meas2Mat);
227 |   Mat estimated3 = KF0.correct(meas3Mat);
228 |   Mat estimated4 = KF0.correct(meas4Mat);
229 |   Mat estimated5 = KF0.correct(meas5Mat);
230 | 
231 |   // Publish the point clouds belonging to each clusters
232 | 
233 |   // cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
234 |   // Point statePt(estimated.at<float>(0),estimated.at<float>(1));
235 |   // cout<<"DONE KF_TRACKER\n";
236 | }
237 | void publish_cloud(ros::Publisher &pub,
238 |                    pcl::PointCloud<pcl::PointXYZ>::Ptr cluster) {
239 |   sensor_msgs::PointCloud2::Ptr clustermsg(new sensor_msgs::PointCloud2);
240 |   pcl::toROSMsg(*cluster, *clustermsg);
241 |   clustermsg->header.frame_id = "map";
242 |   clustermsg->header.stamp = ros::Time::now();
243 |   pub.publish(*clustermsg);
244 | }
245 | 
246 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
247 | 
248 | {
249 |   cout << "IF firstFrame=" << firstFrame << "\n";
250 |   // If this is the first frame, initialize kalman filters for the clustered
251 |   // objects
252 |   if (firstFrame) {
253 |     // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset.
254 |     // Could be made generic by creating a Kalman Filter only when a new object
255 |     // is detected
256 |     KF0.transitionMatrix =
257 |         *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
258 |     KF1.transitionMatrix =
259 |         *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
260 |     KF2.transitionMatrix =
261 |         *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
262 |     KF3.transitionMatrix =
263 |         *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
264 |     KF4.transitionMatrix =
265 |         *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
266 |     KF5.transitionMatrix =
267 |         *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
268 | 
269 |     cv::setIdentity(KF0.measurementMatrix);
270 |     cv::setIdentity(KF1.measurementMatrix);
271 |     cv::setIdentity(KF2.measurementMatrix);
272 |     cv::setIdentity(KF3.measurementMatrix);
273 |     cv::setIdentity(KF4.measurementMatrix);
274 |     cv::setIdentity(KF5.measurementMatrix);
275 |     // Process Noise Covariance Matrix Q
276 |     // [ Ex 0  0    0 0    0 ]
277 |     // [ 0  Ey 0    0 0    0 ]
278 |     // [ 0  0  Ev_x 0 0    0 ]
279 |     // [ 0  0  0    1 Ev_y 0 ]
280 |     //// [ 0  0  0    0 1    Ew ]
281 |     //// [ 0  0  0    0 0    Eh ]
282 |     setIdentity(KF0.processNoiseCov, Scalar::all(1e-4));
283 |     setIdentity(KF1.processNoiseCov, Scalar::all(1e-4));
284 |     setIdentity(KF2.processNoiseCov, Scalar::all(1e-4));
285 |     setIdentity(KF3.processNoiseCov, Scalar::all(1e-4));
286 |     setIdentity(KF4.processNoiseCov, Scalar::all(1e-4));
287 |     setIdentity(KF5.processNoiseCov, Scalar::all(1e-4));
288 |     // Meas noise cov matrix R
289 |     cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(1e-1));
290 |     cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(1e-1));
291 |     cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(1e-1));
292 |     cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(1e-1));
293 |     cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(1e-1));
294 |     cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(1e-1));
295 | 
296 |     // Process the point cloud
297 |     pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(
298 |         new pcl::PointCloud<pcl::PointXYZ>);
299 |     pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(
300 |         new pcl::PointCloud<pcl::PointXYZ>);
301 |     /* Creating the KdTree from input point cloud*/
302 |     pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
303 |         new pcl::search::KdTree<pcl::PointXYZ>);
304 | 
305 |     pcl::fromROSMsg(*input, *input_cloud);
306 | 
307 |     tree->setInputCloud(input_cloud);
308 | 
309 |     /* Here we are creating a vector of PointIndices, which contains the actual
310 |      * index information in a vector<int>. The indices of each detected cluster
311 |      * are saved here. Cluster_indices is a vector containing one instance of
312 |      * PointIndices for each detected cluster. Cluster_indices[0] contain all
313 |      * indices of the first cluster in input point cloud.
314 |      */
315 |     std::vector<pcl::PointIndices> cluster_indices;
316 |     pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
317 |     ec.setClusterTolerance(0.08);
318 |     ec.setMinClusterSize(10);
319 |     ec.setMaxClusterSize(600);
320 |     ec.setSearchMethod(tree);
321 |     ec.setInputCloud(input_cloud);
322 |     /* Extract the clusters out of pc and save indices in cluster_indices.*/
323 |     ec.extract(cluster_indices);
324 | 
325 |     /* To separate each cluster out of the vector<PointIndices> we have to
326 |      * iterate through cluster_indices, create a new PointCloud for each
327 |      * entry and write all points of the current cluster in the PointCloud.
328 |      */
329 |     // pcl::PointXYZ origin (0,0,0);
330 |     // float mindist_this_cluster = 1000;
331 |     // float dist_this_point = 1000;
332 | 
333 |     std::vector<pcl::PointIndices>::const_iterator it;
334 |     std::vector<int>::const_iterator pit;
335 |     // Vector of cluster pointclouds
336 |     std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_vec;
337 | 
338 |     for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
339 |       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(
340 |           new pcl::PointCloud<pcl::PointXYZ>);
341 |       for (pit = it->indices.begin(); pit != it->indices.end(); pit++) {
342 | 
343 |         cloud_cluster->points.push_back(input_cloud->points[*pit]);
344 |         // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
345 |         //                                          origin);
346 |         // mindist_this_cluster = std::min(dist_this_point,
347 |         // mindist_this_cluster);
348 |       }
349 | 
350 |       cluster_vec.push_back(cloud_cluster);
351 |     }
352 | 
353 |     // Ensure at least 6 clusters exist to publish (later clusters may be empty)
354 |     while (cluster_vec.size() < 6) {
355 |       pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster(
356 |           new pcl::PointCloud<pcl::PointXYZ>);
357 |       empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0));
358 |       cluster_vec.push_back(empty_cluster);
359 |     }
360 | 
361 |     // Set initial state
362 |     KF0.statePre.at<float>(0) =
363 |         cluster_vec[0]->points[cluster_vec[0]->points.size() / 2].x;
364 | 
365 |     KF0.statePre.at<float>(1) =
366 |         cluster_vec[0]->points[cluster_vec[0]->points.size() / 2].y;
367 |     KF0.statePre.at<float>(2) = 0; // initial v_x
368 |     KF0.statePre.at<float>(3) = 0; // initial v_y
369 | 
370 |     // Set initial state
371 |     KF1.statePre.at<float>(0) =
372 |         cluster_vec[1]->points[cluster_vec[1]->points.size() / 2].x;
373 |     KF1.statePre.at<float>(1) =
374 |         cluster_vec[1]->points[cluster_vec[1]->points.size() / 2].y;
375 |     KF1.statePre.at<float>(2) = 0; // initial v_x
376 |     KF1.statePre.at<float>(3) = 0; // initial v_y
377 | 
378 |     // Set initial state
379 |     KF2.statePre.at<float>(0) =
380 |         cluster_vec[2]->points[cluster_vec[2]->points.size() / 2].x;
381 |     KF2.statePre.at<float>(1) =
382 |         cluster_vec[2]->points[cluster_vec[2]->points.size() / 2].y;
383 |     KF2.statePre.at<float>(2) = 0; // initial v_x
384 |     KF2.statePre.at<float>(3) = 0; // initial v_y
385 | 
386 |     // Set initial state
387 |     KF3.statePre.at<float>(0) =
388 |         cluster_vec[3]->points[cluster_vec[3]->points.size() / 2].x;
389 |     KF3.statePre.at<float>(1) =
390 |         cluster_vec[3]->points[cluster_vec[3]->points.size() / 2].y;
391 |     KF3.statePre.at<float>(2) = 0; // initial v_x
392 |     KF3.statePre.at<float>(3) = 0; // initial v_y
393 | 
394 |     // Set initial state
395 |     KF4.statePre.at<float>(0) =
396 |         cluster_vec[4]->points[cluster_vec[4]->points.size() / 2].x;
397 |     KF4.statePre.at<float>(1) =
398 |         cluster_vec[4]->points[cluster_vec[4]->points.size() / 2].y;
399 |     KF4.statePre.at<float>(2) = 0; // initial v_x
400 |     KF4.statePre.at<float>(3) = 0; // initial v_y
401 | 
402 |     // Set initial state
403 |     KF5.statePre.at<float>(0) =
404 |         cluster_vec[5]->points[cluster_vec[5]->points.size() / 2].x;
405 |     KF5.statePre.at<float>(1) =
406 |         cluster_vec[5]->points[cluster_vec[5]->points.size() / 2].y;
407 |     KF5.statePre.at<float>(2) = 0; // initial v_x
408 |     KF5.statePre.at<float>(3) = 0; // initial v_y
409 | 
410 |     firstFrame = false;
411 | 
412 |     // Print the initial state of the kalman filter for debugging
413 |     cout << "KF0.satePre=" << KF0.statePre.at<float>(0) << ","
414 |          << KF0.statePre.at<float>(1) << "\n";
415 |     cout << "KF1.satePre=" << KF1.statePre.at<float>(0) << ","
416 |          << KF1.statePre.at<float>(1) << "\n";
417 |     cout << "KF2.satePre=" << KF2.statePre.at<float>(0) << ","
418 |          << KF2.statePre.at<float>(1) << "\n";
419 |     cout << "KF3.satePre=" << KF3.statePre.at<float>(0) << ","
420 |          << KF3.statePre.at<float>(1) << "\n";
421 |     cout << "KF4.satePre=" << KF4.statePre.at<float>(0) << ","
422 |          << KF4.statePre.at<float>(1) << "\n";
423 |     cout << "KF5.satePre=" << KF5.statePre.at<float>(0) << ","
424 |          << KF5.statePre.at<float>(1) << "\n";
425 | 
426 |     // cin.ignore();// To be able to see the printed initial state of the
427 |     // KalmanFilter
428 | 
429 |     /* Naive version without kalman filter */
430 |     for (int i = 0; i < 6; i++) {
431 |       geometry_msgs::Point pt;
432 |       pt.x = cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].x;
433 |       pt.y = cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].y;
434 |       prevClusterCenters.push_back(pt);
435 |     }
436 | 
437 |     /* Naive version without kalman filter */
438 |   }
439 | 
440 |   else {
441 |     cout << "ELSE firstFrame=" << firstFrame << "\n";
442 |     pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(
443 |         new pcl::PointCloud<pcl::PointXYZ>);
444 |     pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(
445 |         new pcl::PointCloud<pcl::PointXYZ>);
446 |     /* Creating the KdTree from input point cloud*/
447 |     pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
448 |         new pcl::search::KdTree<pcl::PointXYZ>);
449 | 
450 |     pcl::fromROSMsg(*input, *input_cloud);
451 | 
452 |     tree->setInputCloud(input_cloud);
453 | 
454 |     /* Here we are creating a vector of PointIndices, which contains the actual
455 |      * index information in a vector<int>. The indices of each detected cluster
456 |      * are saved here. Cluster_indices is a vector containing one instance of
457 |      * PointIndices for each detected cluster. Cluster_indices[0] contain all
458 |      * indices of the first cluster in input point cloud.
459 |      */
460 |     std::vector<pcl::PointIndices> cluster_indices;
461 |     pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
462 |     ec.setClusterTolerance(0.08);
463 |     ec.setMinClusterSize(10);
464 |     ec.setMaxClusterSize(600);
465 |     ec.setSearchMethod(tree);
466 |     ec.setInputCloud(input_cloud);
467 |     cout << "PCL init successfull\n";
468 |     /* Extract the clusters out of pc and save indices in cluster_indices.*/
469 |     ec.extract(cluster_indices);
470 |     cout << "PCL extract successfull\n";
471 |     /* To separate each cluster out of the vector<PointIndices> we have to
472 |      * iterate through cluster_indices, create a new PointCloud for each
473 |      * entry and write all points of the current cluster in the PointCloud.
474 |      */
475 |     // pcl::PointXYZ origin (0,0,0);
476 |     // float mindist_this_cluster = 1000;
477 |     // float dist_this_point = 1000;
478 | 
479 |     std::vector<pcl::PointIndices>::const_iterator it;
480 |     std::vector<int>::const_iterator pit;
481 |     // Vector of cluster pointclouds
482 |     std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_vec;
483 | 
484 |     for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
485 |       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(
486 |           new pcl::PointCloud<pcl::PointXYZ>);
487 |       for (pit = it->indices.begin(); pit != it->indices.end(); pit++) {
488 | 
489 |         cloud_cluster->points.push_back(input_cloud->points[*pit]);
490 |         // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
491 |         //                                          origin);
492 |         // mindist_this_cluster = std::min(dist_this_point,
493 |         // mindist_this_cluster);
494 |       }
495 | 
496 |       cluster_vec.push_back(cloud_cluster);
497 |     }
498 |     // cout<<"cluster_vec got some clusters\n";
499 | 
500 |     // Ensure at least 6 clusters exist to publish (later clusters may be empty)
501 |     while (cluster_vec.size() < 6) {
502 |       pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster(
503 |           new pcl::PointCloud<pcl::PointXYZ>);
504 |       empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0));
505 |       cluster_vec.push_back(empty_cluster);
506 |     }
507 |     std_msgs::Float32MultiArray cc;
508 |     for (int i = 0; i < 6; i++) {
509 |       cc.data.push_back(
510 |           cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].x);
511 |       cc.data.push_back(
512 |           cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].y);
513 |       cc.data.push_back(
514 |           cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].z);
515 |     }
516 |     cout << "6 clusters initialized\n";
517 | 
518 |     // cc_pos.publish(cc);// Publish cluster mid-points.
519 |     KFT(cc);
520 |     int i = 0;
521 |     bool publishedCluster[6];
522 |     for (auto it = objID.begin(); it != objID.end(); it++) {
523 |       cout << "Inside the for loop\n";
524 | 
525 |       switch (*it) {
526 |         cout << "Inside the switch case\n";
527 |       case 0: {
528 |         publish_cloud(pub_cluster0, cluster_vec[i]);
529 |         publishedCluster[i] =
530 |             true; // Use this flag to publish only once for a given obj ID
531 |         i++;
532 |         break;
533 |       }
534 |       case 1: {
535 |         publish_cloud(pub_cluster1, cluster_vec[i]);
536 |         publishedCluster[i] =
537 |             true; // Use this flag to publish only once for a given obj ID
538 |         i++;
539 |         break;
540 |       }
541 |       case 2: {
542 |         publish_cloud(pub_cluster2, cluster_vec[i]);
543 |         publishedCluster[i] =
544 |             true; // Use this flag to publish only once for a given obj ID
545 |         i++;
546 |         break;
547 |       }
548 |       case 3: {
549 |         publish_cloud(pub_cluster3, cluster_vec[i]);
550 |         publishedCluster[i] =
551 |             true; // Use this flag to publish only once for a given obj ID
552 |         i++;
553 |         break;
554 |       }
555 |       case 4: {
556 |         publish_cloud(pub_cluster4, cluster_vec[i]);
557 |         publishedCluster[i] =
558 |             true; // Use this flag to publish only once for a given obj ID
559 |         i++;
560 |         break;
561 |       }
562 | 
563 |       case 5: {
564 |         publish_cloud(pub_cluster5, cluster_vec[i]);
565 |         publishedCluster[i] =
566 |             true; // Use this flag to publish only once for a given obj ID
567 |         i++;
568 |         break;
569 |       }
570 |       default:
571 |         break;
572 |       }
573 |     }
574 |   }
575 | }
576 | 
577 | int main(int argc, char **argv) {
578 |   // ROS init
579 |   ros::init(argc, argv, "KFTracker");
580 |   ros::NodeHandle nh;
581 | 
582 |   // Publishers to publish the state of the objects (pos and vel)
583 |   // objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
584 | 
585 |   cout << "About to setup callback\n";
586 | 
587 |   // Create a ROS subscriber for the input point cloud
588 |   ros::Subscriber sub = nh.subscribe("filtered_cloud", 1, cloud_cb);
589 |   // Create a ROS publisher for the output point cloud
590 |   pub_cluster0 = nh.advertise<sensor_msgs::PointCloud2>("cluster_0", 1);
591 |   pub_cluster1 = nh.advertise<sensor_msgs::PointCloud2>("cluster_1", 1);
592 |   pub_cluster2 = nh.advertise<sensor_msgs::PointCloud2>("cluster_2", 1);
593 |   pub_cluster3 = nh.advertise<sensor_msgs::PointCloud2>("cluster_3", 1);
594 |   pub_cluster4 = nh.advertise<sensor_msgs::PointCloud2>("cluster_4", 1);
595 |   pub_cluster5 = nh.advertise<sensor_msgs::PointCloud2>("cluster_5", 1);
596 |   // Subscribe to the clustered pointclouds
597 |   // ros::Subscriber c1=nh.subscribe("ccs",100,KFT);
598 |   objID_pub = nh.advertise<std_msgs::Int32MultiArray>("obj_id", 1);
599 |   /* Point cloud clustering
600 |    */
601 | 
602 |   // cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1
603 | 
604 |   /* Point cloud clustering
605 |    */
606 | 
607 |   ros::spin();
608 | }
609 | 


--------------------------------------------------------------------------------
/src/main.cpp:
--------------------------------------------------------------------------------
  1 | #include "kf_tracker/CKalmanFilter.h"
  2 | #include "kf_tracker/featureDetection.h"
  3 | #include "opencv2/video/tracking.hpp"
  4 | #include "pcl_ros/point_cloud.h"
  5 | #include <algorithm>
  6 | #include <fstream>
  7 | #include <geometry_msgs/Point.h>
  8 | #include <iostream>
  9 | #include <iterator>
 10 | #include <opencv2/core/core.hpp>
 11 | #include <opencv2/highgui/highgui.hpp>
 12 | #include <opencv2/imgproc/imgproc.hpp>
 13 | #include <opencv2/video/video.hpp>
 14 | #include <pcl/io/pcd_io.h>
 15 | #include <pcl/point_types.h>
 16 | #include <ros/ros.h>
 17 | #include <std_msgs/Float32MultiArray.h>
 18 | #include <std_msgs/Int32MultiArray.h>
 19 | #include <string.h>
 20 | 
 21 | #include <pcl/common/centroid.h>
 22 | #include <pcl/common/geometry.h>
 23 | #include <pcl/features/normal_3d.h>
 24 | #include <pcl/filters/extract_indices.h>
 25 | #include <pcl/filters/voxel_grid.h>
 26 | #include <pcl/kdtree/kdtree.h>
 27 | #include <pcl/point_cloud.h>
 28 | #include <pcl/point_types.h>
 29 | #include <pcl/sample_consensus/method_types.h>
 30 | #include <pcl/sample_consensus/model_types.h>
 31 | #include <pcl/segmentation/extract_clusters.h>
 32 | #include <pcl/segmentation/sac_segmentation.h>
 33 | #include <pcl_conversions/pcl_conversions.h>
 34 | #include <sensor_msgs/PointCloud2.h>
 35 | 
 36 | #include <limits>
 37 | #include <utility>
 38 | #include <visualization_msgs/Marker.h>
 39 | #include <visualization_msgs/MarkerArray.h>
 40 | 
 41 | using namespace std;
 42 | using namespace cv;
 43 | 
 44 | ros::Publisher objID_pub;
 45 | 
 46 | // KF init
 47 | int stateDim = 4; // [x,y,v_x,v_y]//,w,h]
 48 | int measDim = 2;  // [z_x,z_y,z_w,z_h]
 49 | int ctrlDim = 0;
 50 | cv::KalmanFilter KF0(stateDim, measDim, ctrlDim, CV_32F);
 51 | cv::KalmanFilter KF1(stateDim, measDim, ctrlDim, CV_32F);
 52 | cv::KalmanFilter KF2(stateDim, measDim, ctrlDim, CV_32F);
 53 | cv::KalmanFilter KF3(stateDim, measDim, ctrlDim, CV_32F);
 54 | cv::KalmanFilter KF4(stateDim, measDim, ctrlDim, CV_32F);
 55 | cv::KalmanFilter KF5(stateDim, measDim, ctrlDim, CV_32F);
 56 | 
 57 | ros::Publisher pub_cluster0;
 58 | ros::Publisher pub_cluster1;
 59 | ros::Publisher pub_cluster2;
 60 | ros::Publisher pub_cluster3;
 61 | ros::Publisher pub_cluster4;
 62 | ros::Publisher pub_cluster5;
 63 | 
 64 | ros::Publisher markerPub;
 65 | 
 66 | std::vector<geometry_msgs::Point> prevClusterCenters;
 67 | 
 68 | cv::Mat state(stateDim, 1, CV_32F);
 69 | cv::Mat_<float> measurement(2, 1);
 70 | 
 71 | std::vector<int> objID; // Output of the data association using KF
 72 |                         // measurement.setTo(Scalar(0));
 73 | 
 74 | bool firstFrame = true;
 75 | 
 76 | // calculate euclidean distance of two points
 77 | double euclidean_distance(geometry_msgs::Point &p1, geometry_msgs::Point &p2) {
 78 |   return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) +
 79 |               (p1.z - p2.z) * (p1.z - p2.z));
 80 | }
 81 | /*
 82 | //Count unique object IDs. just to make sure same ID has not been assigned to
 83 | two KF_Trackers. int countIDs(vector<int> v)
 84 | {
 85 |     transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n =
 86 | distance(v.end(), v.begin()) sort(v.begin(), v.end()); // Average case O(n log
 87 | n), worst case O(n^2) (usually implemented as quicksort.
 88 |     // To guarantee worst case O(n log n) replace with make_heap, then
 89 | sort_heap.
 90 | 
 91 |     // Unique will take a sorted range, and move things around to get duplicated
 92 |     // items to the back and returns an iterator to the end of the unique
 93 | section of the range auto unique_end = unique(v.begin(), v.end()); // Again n
 94 | comparisons return distance(unique_end, v.begin()); // Constant time for random
 95 | access iterators (like vector's)
 96 | }
 97 | */
 98 | 
 99 | /*
100 | 
101 | objID: vector containing the IDs of the clusters that should be associated with
102 | each KF_Tracker objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc.
103 | */
104 | 
105 | std::pair<int, int> findIndexOfMin(std::vector<std::vector<float>> distMat) {
106 |   cout << "findIndexOfMin cALLED\n";
107 |   std::pair<int, int> minIndex;
108 |   float minEl = std::numeric_limits<float>::max();
109 |   cout << "minEl=" << minEl << "\n";
110 |   for (int i = 0; i < distMat.size(); i++)
111 |     for (int j = 0; j < distMat.at(0).size(); j++) {
112 |       if (distMat[i][j] < minEl) {
113 |         minEl = distMat[i][j];
114 |         minIndex = std::make_pair(i, j);
115 |       }
116 |     }
117 |   cout << "minIndex=" << minIndex.first << "," << minIndex.second << "\n";
118 |   return minIndex;
119 | }
120 | void KFT(const std_msgs::Float32MultiArray ccs) {
121 | 
122 |   // First predict, to update the internal statePre variable
123 | 
124 |   std::vector<cv::Mat> pred{KF0.predict(), KF1.predict(), KF2.predict(),
125 |                             KF3.predict(), KF4.predict(), KF5.predict()};
126 |   // cout<<"Pred successfull\n";
127 | 
128 |   // cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
129 |   // cout<<"Prediction 1
130 |   // ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";
131 | 
132 |   // Get measurements
133 |   // Extract the position of the clusters forom the multiArray. To check if the
134 |   // data coming in, check the .z (every third) coordinate and that will be 0.0
135 |   std::vector<geometry_msgs::Point> clusterCenters; // clusterCenters
136 | 
137 |   int i = 0;
138 |   for (std::vector<float>::const_iterator it = ccs.data.begin();
139 |        it != ccs.data.end(); it += 3) {
140 |     geometry_msgs::Point pt;
141 |     pt.x = *it;
142 |     pt.y = *(it + 1);
143 |     pt.z = *(it + 2);
144 | 
145 |     clusterCenters.push_back(pt);
146 |   }
147 | 
148 |   //  cout<<"CLusterCenters Obtained"<<"\n";
149 |   std::vector<geometry_msgs::Point> KFpredictions;
150 |   i = 0;
151 |   for (auto it = pred.begin(); it != pred.end(); it++) {
152 |     geometry_msgs::Point pt;
153 |     pt.x = (*it).at<float>(0);
154 |     pt.y = (*it).at<float>(1);
155 |     pt.z = (*it).at<float>(2);
156 | 
157 |     KFpredictions.push_back(pt);
158 |   }
159 |   // cout<<"Got predictions"<<"\n";
160 | 
161 |   // Find the cluster that is more probable to be belonging to a given KF.
162 |   objID.clear();   // Clear the objID vector
163 |   objID.resize(6); // Allocate default elements so that [i] doesnt segfault.
164 |                    // Should be done better
165 |   // Copy clusterCentres for modifying it and preventing multiple assignments of
166 |   // the same ID
167 |   std::vector<geometry_msgs::Point> copyOfClusterCenters(clusterCenters);
168 |   std::vector<std::vector<float>> distMat;
169 | 
170 |   for (int filterN = 0; filterN < 6; filterN++) {
171 |     std::vector<float> distVec;
172 |     for (int n = 0; n < 6; n++) {
173 |       distVec.push_back(
174 |           euclidean_distance(KFpredictions[filterN], copyOfClusterCenters[n]));
175 |     }
176 | 
177 |     distMat.push_back(distVec);
178 |     /*// Based on distVec instead of distMat (global min). Has problems with the
179 |     person's leg going out of scope int
180 |     ID=std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end()));
181 |      //cout<<"finterlN="<<filterN<<"   minID="<<ID
182 |      objID.push_back(ID);
183 |     // Prevent assignment of the same object ID to multiple clusters
184 |      copyOfClusterCenters[ID].x=100000;// A large value so that this center is
185 |     not assigned to another cluster copyOfClusterCenters[ID].y=10000;
186 |      copyOfClusterCenters[ID].z=10000;
187 |     */
188 |     cout << "filterN=" << filterN << "\n";
189 |   }
190 | 
191 |   cout << "distMat.size()" << distMat.size() << "\n";
192 |   cout << "distMat[0].size()" << distMat.at(0).size() << "\n";
193 |   // DEBUG: print the distMat
194 |   for (const auto &row : distMat) {
195 |     for (const auto &s : row)
196 |       std::cout << s << ' ';
197 |     std::cout << std::endl;
198 |   }
199 | 
200 |   for (int clusterCount = 0; clusterCount < 6; clusterCount++) {
201 |     // 1. Find min(distMax)==> (i,j);
202 |     std::pair<int, int> minIndex(findIndexOfMin(distMat));
203 |     cout << "Received minIndex=" << minIndex.first << "," << minIndex.second
204 |          << "\n";
205 |     // 2. objID[i]=clusterCenters[j]; counter++
206 |     objID[minIndex.first] = minIndex.second;
207 | 
208 |     // 3. distMat[i,:]=10000; distMat[:,j]=10000
209 |     distMat[minIndex.first] =
210 |         std::vector<float>(6, 10000.0); // Set the row to a high number.
211 |     for (int row = 0; row < distMat.size();
212 |          row++) // set the column to a high number
213 |     {
214 |       distMat[row][minIndex.second] = 10000.0;
215 |     }
216 |     // 4. if(counter<6) got to 1.
217 |     cout << "clusterCount=" << clusterCount << "\n";
218 |   }
219 | 
220 |   // cout<<"Got object IDs"<<"\n";
221 |   // countIDs(objID);// for verif/corner cases
222 | 
223 |   // display objIDs
224 |   /* DEBUG
225 |     cout<<"objID= ";
226 |     for(auto it=objID.begin();it!=objID.end();it++)
227 |         cout<<*it<<" ,";
228 |     cout<<"\n";
229 |     */
230 | 
231 |   visualization_msgs::MarkerArray clusterMarkers;
232 | 
233 |   for (int i = 0; i < 6; i++) {
234 |     visualization_msgs::Marker m;
235 | 
236 |     m.id = i;
237 |     m.type = visualization_msgs::Marker::CUBE;
238 |     m.header.frame_id = "map";
239 |     m.scale.x = 0.3;
240 |     m.scale.y = 0.3;
241 |     m.scale.z = 0.3;
242 |     m.action = visualization_msgs::Marker::ADD;
243 |     m.color.a = 1.0;
244 |     m.color.r = i % 2 ? 1 : 0;
245 |     m.color.g = i % 3 ? 1 : 0;
246 |     m.color.b = i % 4 ? 1 : 0;
247 | 
248 |     // geometry_msgs::Point clusterC(clusterCenters.at(objID[i]));
249 |     geometry_msgs::Point clusterC(KFpredictions[i]);
250 |     m.pose.position.x = clusterC.x;
251 |     m.pose.position.y = clusterC.y;
252 |     m.pose.position.z = clusterC.z;
253 | 
254 |     clusterMarkers.markers.push_back(m);
255 |   }
256 | 
257 |   prevClusterCenters = clusterCenters;
258 | 
259 |   markerPub.publish(clusterMarkers);
260 | 
261 |   std_msgs::Int32MultiArray obj_id;
262 |   for (auto it = objID.begin(); it != objID.end(); it++)
263 |     obj_id.data.push_back(*it);
264 |   // Publish the object IDs
265 |   objID_pub.publish(obj_id);
266 |   // convert clusterCenters from geometry_msgs::Point to floats
267 |   std::vector<std::vector<float>> cc;
268 |   for (int i = 0; i < 6; i++) {
269 |     vector<float> pt;
270 |     pt.push_back(clusterCenters[objID[i]].x);
271 |     pt.push_back(clusterCenters[objID[i]].y);
272 |     pt.push_back(clusterCenters[objID[i]].z);
273 | 
274 |     cc.push_back(pt);
275 |   }
276 |   // cout<<"cc[5][0]="<<cc[5].at(0)<<"cc[5][1]="<<cc[5].at(1)<<"cc[5][2]="<<cc[5].at(2)<<"\n";
277 |   float meas0[2] = {cc[0].at(0), cc[0].at(1)};
278 |   float meas1[2] = {cc[1].at(0), cc[1].at(1)};
279 |   float meas2[2] = {cc[2].at(0), cc[2].at(1)};
280 |   float meas3[2] = {cc[3].at(0), cc[3].at(1)};
281 |   float meas4[2] = {cc[4].at(0), cc[4].at(1)};
282 |   float meas5[2] = {cc[5].at(0), cc[5].at(1)};
283 | 
284 |   // The update phase
285 |   cv::Mat meas0Mat = cv::Mat(2, 1, CV_32F, meas0);
286 |   cv::Mat meas1Mat = cv::Mat(2, 1, CV_32F, meas1);
287 |   cv::Mat meas2Mat = cv::Mat(2, 1, CV_32F, meas2);
288 |   cv::Mat meas3Mat = cv::Mat(2, 1, CV_32F, meas3);
289 |   cv::Mat meas4Mat = cv::Mat(2, 1, CV_32F, meas4);
290 |   cv::Mat meas5Mat = cv::Mat(2, 1, CV_32F, meas5);
291 | 
292 |   // cout<<"meas0Mat"<<meas0Mat<<"\n";
293 |   if (!(meas0Mat.at<float>(0, 0) == 0.0f || meas0Mat.at<float>(1, 0) == 0.0f))
294 |     Mat estimated0 = KF0.correct(meas0Mat);
295 |   if (!(meas1[0] == 0.0f || meas1[1] == 0.0f))
296 |     Mat estimated1 = KF1.correct(meas1Mat);
297 |   if (!(meas2[0] == 0.0f || meas2[1] == 0.0f))
298 |     Mat estimated2 = KF2.correct(meas2Mat);
299 |   if (!(meas3[0] == 0.0f || meas3[1] == 0.0f))
300 |     Mat estimated3 = KF3.correct(meas3Mat);
301 |   if (!(meas4[0] == 0.0f || meas4[1] == 0.0f))
302 |     Mat estimated4 = KF4.correct(meas4Mat);
303 |   if (!(meas5[0] == 0.0f || meas5[1] == 0.0f))
304 |     Mat estimated5 = KF5.correct(meas5Mat);
305 | 
306 |   // Publish the point clouds belonging to each clusters
307 | 
308 |   // cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
309 |   // Point statePt(estimated.at<float>(0),estimated.at<float>(1));
310 |   // cout<<"DONE KF_TRACKER\n";
311 | }
312 | void publish_cloud(ros::Publisher &pub,
313 |                    pcl::PointCloud<pcl::PointXYZ>::Ptr cluster) {
314 |   sensor_msgs::PointCloud2::Ptr clustermsg(new sensor_msgs::PointCloud2);
315 |   pcl::toROSMsg(*cluster, *clustermsg);
316 |   clustermsg->header.frame_id = "map";
317 |   clustermsg->header.stamp = ros::Time::now();
318 |   pub.publish(*clustermsg);
319 | }
320 | 
321 | void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
322 | 
323 | {
324 |   // cout<<"IF firstFrame="<<firstFrame<<"\n";
325 |   // If this is the first frame, initialize kalman filters for the clustered
326 |   // objects
327 |   if (firstFrame) {
328 |     // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset.
329 |     // Could be made generic by creating a Kalman Filter only when a new object
330 |     // is detected
331 | 
332 |     float dvx = 0.01f; // 1.0
333 |     float dvy = 0.01f; // 1.0
334 |     float dx = 1.0f;
335 |     float dy = 1.0f;
336 |     KF0.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
337 |                             dvx, 0, 0, 0, 0, dvy);
338 |     KF1.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
339 |                             dvx, 0, 0, 0, 0, dvy);
340 |     KF2.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
341 |                             dvx, 0, 0, 0, 0, dvy);
342 |     KF3.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
343 |                             dvx, 0, 0, 0, 0, dvy);
344 |     KF4.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
345 |                             dvx, 0, 0, 0, 0, dvy);
346 |     KF5.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
347 |                             dvx, 0, 0, 0, 0, dvy);
348 | 
349 |     cv::setIdentity(KF0.measurementMatrix);
350 |     cv::setIdentity(KF1.measurementMatrix);
351 |     cv::setIdentity(KF2.measurementMatrix);
352 |     cv::setIdentity(KF3.measurementMatrix);
353 |     cv::setIdentity(KF4.measurementMatrix);
354 |     cv::setIdentity(KF5.measurementMatrix);
355 |     // Process Noise Covariance Matrix Q
356 |     // [ Ex 0  0    0 0    0 ]
357 |     // [ 0  Ey 0    0 0    0 ]
358 |     // [ 0  0  Ev_x 0 0    0 ]
359 |     // [ 0  0  0    1 Ev_y 0 ]
360 |     //// [ 0  0  0    0 1    Ew ]
361 |     //// [ 0  0  0    0 0    Eh ]
362 |     float sigmaP = 0.01;
363 |     float sigmaQ = 0.1;
364 |     setIdentity(KF0.processNoiseCov, Scalar::all(sigmaP));
365 |     setIdentity(KF1.processNoiseCov, Scalar::all(sigmaP));
366 |     setIdentity(KF2.processNoiseCov, Scalar::all(sigmaP));
367 |     setIdentity(KF3.processNoiseCov, Scalar::all(sigmaP));
368 |     setIdentity(KF4.processNoiseCov, Scalar::all(sigmaP));
369 |     setIdentity(KF5.processNoiseCov, Scalar::all(sigmaP));
370 |     // Meas noise cov matrix R
371 |     cv::setIdentity(KF0.measurementNoiseCov, cv::Scalar(sigmaQ)); // 1e-1
372 |     cv::setIdentity(KF1.measurementNoiseCov, cv::Scalar(sigmaQ));
373 |     cv::setIdentity(KF2.measurementNoiseCov, cv::Scalar(sigmaQ));
374 |     cv::setIdentity(KF3.measurementNoiseCov, cv::Scalar(sigmaQ));
375 |     cv::setIdentity(KF4.measurementNoiseCov, cv::Scalar(sigmaQ));
376 |     cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ));
377 | 
378 |     // Process the point cloud
379 |     pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(
380 |         new pcl::PointCloud<pcl::PointXYZ>);
381 |     pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(
382 |         new pcl::PointCloud<pcl::PointXYZ>);
383 |     /* Creating the KdTree from input point cloud*/
384 |     pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
385 |         new pcl::search::KdTree<pcl::PointXYZ>);
386 | 
387 |     pcl::fromROSMsg(*input, *input_cloud);
388 | 
389 |     tree->setInputCloud(input_cloud);
390 | 
391 |     std::vector<pcl::PointIndices> cluster_indices;
392 |     pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
393 |     ec.setClusterTolerance(0.08);
394 |     ec.setMinClusterSize(10);
395 |     ec.setMaxClusterSize(600);
396 |     ec.setSearchMethod(tree);
397 |     ec.setInputCloud(input_cloud);
398 |     /* Extract the clusters out of pc and save indices in cluster_indices.*/
399 |     ec.extract(cluster_indices);
400 | 
401 |     std::vector<pcl::PointIndices>::const_iterator it;
402 |     std::vector<int>::const_iterator pit;
403 |     // Vector of cluster pointclouds
404 |     std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_vec;
405 |     // Cluster centroids
406 |     std::vector<pcl::PointXYZ> clusterCentroids;
407 | 
408 |     for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
409 | 
410 |       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(
411 |           new pcl::PointCloud<pcl::PointXYZ>);
412 |       float x = 0.0;
413 |       float y = 0.0;
414 |       int numPts = 0;
415 |       for (pit = it->indices.begin(); pit != it->indices.end(); pit++) {
416 | 
417 |         cloud_cluster->points.push_back(input_cloud->points[*pit]);
418 |         x += input_cloud->points[*pit].x;
419 |         y += input_cloud->points[*pit].y;
420 |         numPts++;
421 | 
422 |         // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
423 |         //                                          origin);
424 |         // mindist_this_cluster = std::min(dist_this_point,
425 |         // mindist_this_cluster);
426 |       }
427 | 
428 |       pcl::PointXYZ centroid;
429 |       centroid.x = x / numPts;
430 |       centroid.y = y / numPts;
431 |       centroid.z = 0.0;
432 | 
433 |       cluster_vec.push_back(cloud_cluster);
434 | 
435 |       // Get the centroid of the cluster
436 |       clusterCentroids.push_back(centroid);
437 |     }
438 | 
439 |     // Ensure at least 6 clusters exist to publish (later clusters may be empty)
440 |     while (cluster_vec.size() < 6) {
441 |       pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster(
442 |           new pcl::PointCloud<pcl::PointXYZ>);
443 |       empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0));
444 |       cluster_vec.push_back(empty_cluster);
445 |     }
446 | 
447 |     while (clusterCentroids.size() < 6) {
448 |       pcl::PointXYZ centroid;
449 |       centroid.x = 0.0;
450 |       centroid.y = 0.0;
451 |       centroid.z = 0.0;
452 | 
453 |       clusterCentroids.push_back(centroid);
454 |     }
455 | 
456 |     // Set initial state
457 |     KF0.statePre.at<float>(0) = clusterCentroids.at(0).x;
458 |     KF0.statePre.at<float>(1) = clusterCentroids.at(0).y;
459 |     KF0.statePre.at<float>(2) = 0; // initial v_x
460 |     KF0.statePre.at<float>(3) = 0; // initial v_y
461 | 
462 |     // Set initial state
463 |     KF1.statePre.at<float>(0) = clusterCentroids.at(1).x;
464 |     KF1.statePre.at<float>(1) = clusterCentroids.at(1).y;
465 |     KF1.statePre.at<float>(2) = 0; // initial v_x
466 |     KF1.statePre.at<float>(3) = 0; // initial v_y
467 | 
468 |     // Set initial state
469 |     KF2.statePre.at<float>(0) = clusterCentroids.at(2).x;
470 |     KF2.statePre.at<float>(1) = clusterCentroids.at(2).y;
471 |     KF2.statePre.at<float>(2) = 0; // initial v_x
472 |     KF2.statePre.at<float>(3) = 0; // initial v_y
473 | 
474 |     // Set initial state
475 |     KF3.statePre.at<float>(0) = clusterCentroids.at(3).x;
476 |     KF3.statePre.at<float>(1) = clusterCentroids.at(3).y;
477 |     KF3.statePre.at<float>(2) = 0; // initial v_x
478 |     KF3.statePre.at<float>(3) = 0; // initial v_y
479 | 
480 |     // Set initial state
481 |     KF4.statePre.at<float>(0) = clusterCentroids.at(4).x;
482 |     KF4.statePre.at<float>(1) = clusterCentroids.at(4).y;
483 |     KF4.statePre.at<float>(2) = 0; // initial v_x
484 |     KF4.statePre.at<float>(3) = 0; // initial v_y
485 | 
486 |     // Set initial state
487 |     KF5.statePre.at<float>(0) = clusterCentroids.at(5).x;
488 |     KF5.statePre.at<float>(1) = clusterCentroids.at(5).y;
489 |     KF5.statePre.at<float>(2) = 0; // initial v_x
490 |     KF5.statePre.at<float>(3) = 0; // initial v_y
491 | 
492 |     firstFrame = false;
493 | 
494 |     for (int i = 0; i < 6; i++) {
495 |       geometry_msgs::Point pt;
496 |       pt.x = clusterCentroids.at(i).x;
497 |       pt.y = clusterCentroids.at(i).y;
498 |       prevClusterCenters.push_back(pt);
499 |     }
500 |     /*  // Print the initial state of the kalman filter for debugging
501 |       cout<<"KF0.satePre="<<KF0.statePre.at<float>(0)<<","<<KF0.statePre.at<float>(1)<<"\n";
502 |       cout<<"KF1.satePre="<<KF1.statePre.at<float>(0)<<","<<KF1.statePre.at<float>(1)<<"\n";
503 |       cout<<"KF2.satePre="<<KF2.statePre.at<float>(0)<<","<<KF2.statePre.at<float>(1)<<"\n";
504 |       cout<<"KF3.satePre="<<KF3.statePre.at<float>(0)<<","<<KF3.statePre.at<float>(1)<<"\n";
505 |       cout<<"KF4.satePre="<<KF4.statePre.at<float>(0)<<","<<KF4.statePre.at<float>(1)<<"\n";
506 |       cout<<"KF5.satePre="<<KF5.statePre.at<float>(0)<<","<<KF5.statePre.at<float>(1)<<"\n";
507 | 
508 |       //cin.ignore();// To be able to see the printed initial state of the
509 |       KalmanFilter
510 |       */
511 |   }
512 | 
513 |   else {
514 |     // cout<<"ELSE firstFrame="<<firstFrame<<"\n";
515 |     pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(
516 |         new pcl::PointCloud<pcl::PointXYZ>);
517 |     pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(
518 |         new pcl::PointCloud<pcl::PointXYZ>);
519 |     /* Creating the KdTree from input point cloud*/
520 |     pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
521 |         new pcl::search::KdTree<pcl::PointXYZ>);
522 | 
523 |     pcl::fromROSMsg(*input, *input_cloud);
524 | 
525 |     tree->setInputCloud(input_cloud);
526 | 
527 |     /* Here we are creating a vector of PointIndices, which contains the actual
528 |      * index information in a vector<int>. The indices of each detected cluster
529 |      * are saved here. Cluster_indices is a vector containing one instance of
530 |      * PointIndices for each detected cluster. Cluster_indices[0] contain all
531 |      * indices of the first cluster in input point cloud.
532 |      */
533 |     std::vector<pcl::PointIndices> cluster_indices;
534 |     pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
535 |     ec.setClusterTolerance(0.3);
536 |     ec.setMinClusterSize(10);
537 |     ec.setMaxClusterSize(600);
538 |     ec.setSearchMethod(tree);
539 |     ec.setInputCloud(input_cloud);
540 |     // cout<<"PCL init successfull\n";
541 |     /* Extract the clusters out of pc and save indices in cluster_indices.*/
542 |     ec.extract(cluster_indices);
543 |     // cout<<"PCL extract successfull\n";
544 |     /* To separate each cluster out of the vector<PointIndices> we have to
545 |      * iterate through cluster_indices, create a new PointCloud for each
546 |      * entry and write all points of the current cluster in the PointCloud.
547 |      */
548 |     // pcl::PointXYZ origin (0,0,0);
549 |     // float mindist_this_cluster = 1000;
550 |     // float dist_this_point = 1000;
551 | 
552 |     std::vector<pcl::PointIndices>::const_iterator it;
553 |     std::vector<int>::const_iterator pit;
554 |     // Vector of cluster pointclouds
555 |     std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_vec;
556 | 
557 |     // Cluster centroids
558 |     std::vector<pcl::PointXYZ> clusterCentroids;
559 | 
560 |     for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
561 |       float x = 0.0;
562 |       float y = 0.0;
563 |       int numPts = 0;
564 |       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(
565 |           new pcl::PointCloud<pcl::PointXYZ>);
566 |       for (pit = it->indices.begin(); pit != it->indices.end(); pit++) {
567 | 
568 |         cloud_cluster->points.push_back(input_cloud->points[*pit]);
569 | 
570 |         x += input_cloud->points[*pit].x;
571 |         y += input_cloud->points[*pit].y;
572 |         numPts++;
573 | 
574 |         // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
575 |         //                                          origin);
576 |         // mindist_this_cluster = std::min(dist_this_point,
577 |         // mindist_this_cluster);
578 |       }
579 | 
580 |       pcl::PointXYZ centroid;
581 |       centroid.x = x / numPts;
582 |       centroid.y = y / numPts;
583 |       centroid.z = 0.0;
584 | 
585 |       cluster_vec.push_back(cloud_cluster);
586 | 
587 |       // Get the centroid of the cluster
588 |       clusterCentroids.push_back(centroid);
589 |     }
590 |     // cout<<"cluster_vec got some clusters\n";
591 | 
592 |     // Ensure at least 6 clusters exist to publish (later clusters may be empty)
593 |     while (cluster_vec.size() < 6) {
594 |       pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster(
595 |           new pcl::PointCloud<pcl::PointXYZ>);
596 |       empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0));
597 |       cluster_vec.push_back(empty_cluster);
598 |     }
599 | 
600 |     while (clusterCentroids.size() < 6) {
601 |       pcl::PointXYZ centroid;
602 |       centroid.x = 0.0;
603 |       centroid.y = 0.0;
604 |       centroid.z = 0.0;
605 | 
606 |       clusterCentroids.push_back(centroid);
607 |     }
608 | 
609 |     std_msgs::Float32MultiArray cc;
610 |     for (int i = 0; i < 6; i++) {
611 |       cc.data.push_back(clusterCentroids.at(i).x);
612 |       cc.data.push_back(clusterCentroids.at(i).y);
613 |       cc.data.push_back(clusterCentroids.at(i).z);
614 |     }
615 |     // cout<<"6 clusters initialized\n";
616 | 
617 |     // cc_pos.publish(cc);// Publish cluster mid-points.
618 |     KFT(cc);
619 |     int i = 0;
620 |     bool publishedCluster[6];
621 |     for (auto it = objID.begin(); it != objID.end();
622 |          it++) { // cout<<"Inside the for loop\n";
623 | 
624 |       switch (i) {
625 |         cout << "Inside the switch case\n";
626 |       case 0: {
627 |         publish_cloud(pub_cluster0, cluster_vec[*it]);
628 |         publishedCluster[i] =
629 |             true; // Use this flag to publish only once for a given obj ID
630 |         i++;
631 |         break;
632 |       }
633 |       case 1: {
634 |         publish_cloud(pub_cluster1, cluster_vec[*it]);
635 |         publishedCluster[i] =
636 |             true; // Use this flag to publish only once for a given obj ID
637 |         i++;
638 |         break;
639 |       }
640 |       case 2: {
641 |         publish_cloud(pub_cluster2, cluster_vec[*it]);
642 |         publishedCluster[i] =
643 |             true; // Use this flag to publish only once for a given obj ID
644 |         i++;
645 |         break;
646 |       }
647 |       case 3: {
648 |         publish_cloud(pub_cluster3, cluster_vec[*it]);
649 |         publishedCluster[i] =
650 |             true; // Use this flag to publish only once for a given obj ID
651 |         i++;
652 |         break;
653 |       }
654 |       case 4: {
655 |         publish_cloud(pub_cluster4, cluster_vec[*it]);
656 |         publishedCluster[i] =
657 |             true; // Use this flag to publish only once for a given obj ID
658 |         i++;
659 |         break;
660 |       }
661 | 
662 |       case 5: {
663 |         publish_cloud(pub_cluster5, cluster_vec[*it]);
664 |         publishedCluster[i] =
665 |             true; // Use this flag to publish only once for a given obj ID
666 |         i++;
667 |         break;
668 |       }
669 |       default:
670 |         break;
671 |       }
672 |     }
673 |   }
674 | }
675 | 
676 | int main(int argc, char **argv) {
677 |   // ROS init
678 |   ros::init(argc, argv, "kf_tracker");
679 |   ros::NodeHandle nh;
680 | 
681 |   // Publishers to publish the state of the objects (pos and vel)
682 |   // objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
683 | 
684 |   cout << "About to setup callback\n";
685 | 
686 |   // Create a ROS subscriber for the input point cloud
687 |   ros::Subscriber sub = nh.subscribe("filtered_cloud", 1, cloud_cb);
688 |   // Create a ROS publisher for the output point cloud
689 |   pub_cluster0 = nh.advertise<sensor_msgs::PointCloud2>("cluster_0", 1);
690 |   pub_cluster1 = nh.advertise<sensor_msgs::PointCloud2>("cluster_1", 1);
691 |   pub_cluster2 = nh.advertise<sensor_msgs::PointCloud2>("cluster_2", 1);
692 |   pub_cluster3 = nh.advertise<sensor_msgs::PointCloud2>("cluster_3", 1);
693 |   pub_cluster4 = nh.advertise<sensor_msgs::PointCloud2>("cluster_4", 1);
694 |   pub_cluster5 = nh.advertise<sensor_msgs::PointCloud2>("cluster_5", 1);
695 |   // Subscribe to the clustered pointclouds
696 |   // ros::Subscriber c1=nh.subscribe("ccs",100,KFT);
697 |   objID_pub = nh.advertise<std_msgs::Int32MultiArray>("obj_id", 1);
698 |   /* Point cloud clustering
699 |    */
700 | 
701 |   // cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1
702 |   markerPub = nh.advertise<visualization_msgs::MarkerArray>("viz", 1);
703 | 
704 |   /* Point cloud clustering
705 |    */
706 | 
707 |   ros::spin();
708 | }
709 | 


--------------------------------------------------------------------------------