12345qiupeng
/
multiple-object-tracking-lidar
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #46 from praveen-palanisamy/rm-topic-slash-prefix 

Remove topic slash prefix
master
Praveen Palanisamy 2021-08-29 10:41:25 -07:00 committed by GitHub
parent f1aec75f7d a9aa364d33
commit aa39c9fe3e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 1016 additions and 1045 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

356
src/main.cpp
View File

@ -1,47 +1,46 @@
#include <iostream>
#include <string.h>
#include <fstream>
#include <algorithm>
#include <iterator>
#include "kf_tracker/featureDetection.h"
#include "kf_tracker/CKalmanFilter.h" #include "kf_tracker/CKalmanFilter.h"
#include "kf_tracker/featureDetection.h"
#include "opencv2/video/tracking.hpp"
#include "pcl_ros/point_cloud.h"
#include <algorithm>
#include <fstream>
#include <geometry_msgs/Point.h>
#include <iostream>
#include <iterator>
#include <opencv2/core/core.hpp> #include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp> #include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp> #include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/video.hpp> #include <opencv2/video/video.hpp>
#include "opencv2/video/tracking.hpp"
#include <ros/ros.h>
#include <pcl/io/pcd_io.h> #include <pcl/io/pcd_io.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include "pcl_ros/point_cloud.h" #include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/Float32MultiArray.h> #include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Int32MultiArray.h> #include <std_msgs/Int32MultiArray.h>
#include <string.h>
#include <sensor_msgs/PointCloud2.h> #include <pcl/common/centroid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/geometry.h> #include <pcl/common/geometry.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h> #include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h> #include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h> #include <pcl/kdtree/kdtree.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h> #include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h> #include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h> #include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/centroid.h> #include <pcl/segmentation/sac_segmentation.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <limits> #include <limits>
#include <utility> #include <utility>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
using namespace std; using namespace std;
using namespace cv; using namespace cv;
ros::Publisher objID_pub; ros::Publisher objID_pub;
// KF init // KF init
@ -66,7 +65,6 @@ ros::Publisher objID_pub;
std::vector<geometry_msgs::Point> prevClusterCenters; std::vector<geometry_msgs::Point> prevClusterCenters;
cv::Mat state(stateDim, 1, CV_32F); cv::Mat state(stateDim, 1, CV_32F);
cv::Mat_<float> measurement(2, 1); cv::Mat_<float> measurement(2, 1);
@ -76,156 +74,147 @@ ros::Publisher objID_pub;
bool firstFrame = true; bool firstFrame = true;
// calculate euclidean distance of two points // calculate euclidean distance of two points
double euclidean_distance(geometry_msgs::Point& p1, geometry_msgs::Point& p2) double euclidean_distance(geometry_msgs::Point &p1, geometry_msgs::Point &p2) {
{ return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) +
return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z)); (p1.z - p2.z) * (p1.z - p2.z));
} }
/* /*
//Count unique object IDs. just to make sure same ID has not been assigned to two KF_Trackers. //Count unique object IDs. just to make sure same ID has not been assigned to
int countIDs(vector<int> v) two KF_Trackers. int countIDs(vector<int> v)
{ {
transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n = distance(v.end(), v.begin()) transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n =
sort(v.begin(), v.end()); // Average case O(n log n), worst case O(n^2) (usually implemented as quicksort. distance(v.end(), v.begin()) sort(v.begin(), v.end()); // Average case O(n log
// To guarantee worst case O(n log n) replace with make_heap, then sort_heap. n), worst case O(n^2) (usually implemented as quicksort.
// To guarantee worst case O(n log n) replace with make_heap, then
sort_heap.
// Unique will take a sorted range, and move things around to get duplicated // Unique will take a sorted range, and move things around to get duplicated
// items to the back and returns an iterator to the end of the unique section of the range // items to the back and returns an iterator to the end of the unique
auto unique_end = unique(v.begin(), v.end()); // Again n comparisons section of the range auto unique_end = unique(v.begin(), v.end()); // Again n
return distance(unique_end, v.begin()); // Constant time for random access iterators (like vector's) comparisons return distance(unique_end, v.begin()); // Constant time for random
access iterators (like vector's)
} }
*/ */
/* /*
objID: vector containing the IDs of the clusters that should be associated with each KF_Tracker objID: vector containing the IDs of the clusters that should be associated with
objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc. each KF_Tracker objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc.
*/ */
std::pair<int,int> findIndexOfMin(std::vector<std::vector<float> > distMat) std::pair<int, int> findIndexOfMin(std::vector<std::vector<float>> distMat) {
{
cout << "findIndexOfMin cALLED\n"; cout << "findIndexOfMin cALLED\n";
std::pair<int, int> minIndex; std::pair<int, int> minIndex;
float minEl = std::numeric_limits<float>::max(); float minEl = std::numeric_limits<float>::max();
cout << "minEl=" << minEl << "\n"; cout << "minEl=" << minEl << "\n";
for (int i = 0; i < distMat.size(); i++) for (int i = 0; i < distMat.size(); i++)
for(int j=0;j<distMat.at(0).size();j++) for (int j = 0; j < distMat.at(0).size(); j++) {
{ if (distMat[i][j] < minEl) {
if( distMat[i][j]<minEl)
{
minEl = distMat[i][j]; minEl = distMat[i][j];
minIndex = std::make_pair(i, j); minIndex = std::make_pair(i, j);
} }
} }
cout << "minIndex=" << minIndex.first << "," << minIndex.second << "\n"; cout << "minIndex=" << minIndex.first << "," << minIndex.second << "\n";
return minIndex; return minIndex;
} }
void KFT(const std_msgs::Float32MultiArray ccs) void KFT(const std_msgs::Float32MultiArray ccs) {
{
// First predict, to update the internal statePre variable // First predict, to update the internal statePre variable
std::vector<cv::Mat> pred{KF0.predict(),KF1.predict(),KF2.predict(),KF3.predict(),KF4.predict(),KF5.predict()}; std::vector<cv::Mat> pred{KF0.predict(), KF1.predict(), KF2.predict(),
KF3.predict(), KF4.predict(), KF5.predict()};
// cout<<"Pred successfull\n"; // cout<<"Pred successfull\n";
// cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1)); // cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
// cout<<"Prediction 1 ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n"; // cout<<"Prediction 1
// ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";
// Get measurements // Get measurements
// Extract the position of the clusters forom the multiArray. To check if the data // Extract the position of the clusters forom the multiArray. To check if the
// coming in, check the .z (every third) coordinate and that will be 0.0 // data coming in, check the .z (every third) coordinate and that will be 0.0
std::vector<geometry_msgs::Point> clusterCenters; // clusterCenters std::vector<geometry_msgs::Point> clusterCenters; // clusterCenters
int i = 0; int i = 0;
for (std::vector<float>::const_iterator it=ccs.data.begin();it!=ccs.data.end();it+=3) for (std::vector<float>::const_iterator it = ccs.data.begin();
{ it != ccs.data.end(); it += 3) {
geometry_msgs::Point pt; geometry_msgs::Point pt;
pt.x = *it; pt.x = *it;
pt.y = *(it + 1); pt.y = *(it + 1);
pt.z = *(it + 2); pt.z = *(it + 2);
clusterCenters.push_back(pt); clusterCenters.push_back(pt);
} }
// cout<<"CLusterCenters Obtained"<<"\n"; // cout<<"CLusterCenters Obtained"<<"\n";
std::vector<geometry_msgs::Point> KFpredictions; std::vector<geometry_msgs::Point> KFpredictions;
i = 0; i = 0;
for (auto it=pred.begin();it!=pred.end();it++) for (auto it = pred.begin(); it != pred.end(); it++) {
{
geometry_msgs::Point pt; geometry_msgs::Point pt;
pt.x = (*it).at<float>(0); pt.x = (*it).at<float>(0);
pt.y = (*it).at<float>(1); pt.y = (*it).at<float>(1);
pt.z = (*it).at<float>(2); pt.z = (*it).at<float>(2);
KFpredictions.push_back(pt); KFpredictions.push_back(pt);
} }
// cout<<"Got predictions"<<"\n"; // cout<<"Got predictions"<<"\n";
// Find the cluster that is more probable to be belonging to a given KF. // Find the cluster that is more probable to be belonging to a given KF.
objID.clear(); // Clear the objID vector objID.clear(); // Clear the objID vector
objID.resize(6);//Allocate default elements so that [i] doesnt segfault. Should be done better objID.resize(6); // Allocate default elements so that [i] doesnt segfault.
// Copy clusterCentres for modifying it and preventing multiple assignments of the same ID // Should be done better
// Copy clusterCentres for modifying it and preventing multiple assignments of
// the same ID
std::vector<geometry_msgs::Point> copyOfClusterCenters(clusterCenters); std::vector<geometry_msgs::Point> copyOfClusterCenters(clusterCenters);
std::vector<std::vector<float>> distMat; std::vector<std::vector<float>> distMat;
for(int filterN=0;filterN<6;filterN++) for (int filterN = 0; filterN < 6; filterN++) {
{
std::vector<float> distVec; std::vector<float> distVec;
for(int n=0;n<6;n++) for (int n = 0; n < 6; n++) {
{ distVec.push_back(
distVec.push_back(euclidean_distance(KFpredictions[filterN],copyOfClusterCenters[n])); euclidean_distance(KFpredictions[filterN], copyOfClusterCenters[n]));
} }
distMat.push_back(distVec); distMat.push_back(distVec);
/*// Based on distVec instead of distMat (global min). Has problems with the person's leg going out of scope /*// Based on distVec instead of distMat (global min). Has problems with the
int ID=std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end())); person's leg going out of scope int
ID=std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end()));
//cout<<"finterlN="<<filterN<<" minID="<<ID //cout<<"finterlN="<<filterN<<" minID="<<ID
objID.push_back(ID); objID.push_back(ID);
// Prevent assignment of the same object ID to multiple clusters // Prevent assignment of the same object ID to multiple clusters
copyOfClusterCenters[ID].x=100000;// A large value so that this center is not assigned to another cluster copyOfClusterCenters[ID].x=100000;// A large value so that this center is
copyOfClusterCenters[ID].y=10000; not assigned to another cluster copyOfClusterCenters[ID].y=10000;
copyOfClusterCenters[ID].z=10000; copyOfClusterCenters[ID].z=10000;
*/ */
cout << "filterN=" << filterN << "\n"; cout << "filterN=" << filterN << "\n";
} }
cout << "distMat.size()" << distMat.size() << "\n"; cout << "distMat.size()" << distMat.size() << "\n";
cout << "distMat[0].size()" << distMat.at(0).size() << "\n"; cout << "distMat[0].size()" << distMat.at(0).size() << "\n";
// DEBUG: print the distMat // DEBUG: print the distMat
for ( const auto &row : distMat ) for (const auto &row : distMat) {
{ for (const auto &s : row)
for ( const auto &s : row ) std::cout << s << ' '; std::cout << s << ' ';
std::cout << std::endl; std::cout << std::endl;
} }
for (int clusterCount = 0; clusterCount < 6; clusterCount++) {
for(int clusterCount=0;clusterCount<6;clusterCount++)
{
// 1. Find min(distMax)==> (i,j); // 1. Find min(distMax)==> (i,j);
std::pair<int, int> minIndex(findIndexOfMin(distMat)); std::pair<int, int> minIndex(findIndexOfMin(distMat));
cout<<"Received minIndex="<<minIndex.first<<","<<minIndex.second<<"\n"; cout << "Received minIndex=" << minIndex.first << "," << minIndex.second
<< "\n";
// 2. objID[i]=clusterCenters[j]; counter++ // 2. objID[i]=clusterCenters[j]; counter++
objID[minIndex.first] = minIndex.second; objID[minIndex.first] = minIndex.second;
// 3. distMat[i,:]=10000; distMat[:,j]=10000 // 3. distMat[i,:]=10000; distMat[:,j]=10000
distMat[minIndex.first]=std::vector<float>(6,10000.0);// Set the row to a high number. distMat[minIndex.first] =
for(int row=0;row<distMat.size();row++)//set the column to a high number std::vector<float>(6, 10000.0); // Set the row to a high number.
for (int row = 0; row < distMat.size();
row++) // set the column to a high number
{ {
distMat[row][minIndex.second] = 10000.0; distMat[row][minIndex.second] = 10000.0;
} }
// 4. if(counter<6) got to 1. // 4. if(counter<6) got to 1.
cout << "clusterCount=" << clusterCount << "\n"; cout << "clusterCount=" << clusterCount << "\n";
} }
// cout<<"Got object IDs"<<"\n"; // cout<<"Got object IDs"<<"\n";
@ -241,14 +230,15 @@ void KFT(const std_msgs::Float32MultiArray ccs)
visualization_msgs::MarkerArray clusterMarkers; visualization_msgs::MarkerArray clusterMarkers;
for (int i=0;i<6;i++) for (int i = 0; i < 6; i++) {
{
visualization_msgs::Marker m; visualization_msgs::Marker m;
m.id = i; m.id = i;
m.type = visualization_msgs::Marker::CUBE; m.type = visualization_msgs::Marker::CUBE;
m.header.frame_id="/map"; m.header.frame_id = "map";
m.scale.x=0.3; m.scale.y=0.3; m.scale.z=0.3; m.scale.x = 0.3;
m.scale.y = 0.3;
m.scale.z = 0.3;
m.action = visualization_msgs::Marker::ADD; m.action = visualization_msgs::Marker::ADD;
m.color.a = 1.0; m.color.a = 1.0;
m.color.r = i % 2 ? 1 : 0; m.color.r = i % 2 ? 1 : 0;
@ -268,9 +258,6 @@ void KFT(const std_msgs::Float32MultiArray ccs)
markerPub.publish(clusterMarkers); markerPub.publish(clusterMarkers);
std_msgs::Int32MultiArray obj_id; std_msgs::Int32MultiArray obj_id;
for (auto it = objID.begin(); it != objID.end(); it++) for (auto it = objID.begin(); it != objID.end(); it++)
obj_id.data.push_back(*it); obj_id.data.push_back(*it);
@ -278,8 +265,7 @@ void KFT(const std_msgs::Float32MultiArray ccs)
objID_pub.publish(obj_id); objID_pub.publish(obj_id);
// convert clusterCenters from geometry_msgs::Point to floats // convert clusterCenters from geometry_msgs::Point to floats
std::vector<std::vector<float>> cc; std::vector<std::vector<float>> cc;
for (int i=0;i<6;i++) for (int i = 0; i < 6; i++) {
{
vector<float> pt; vector<float> pt;
pt.push_back(clusterCenters[objID[i]].x); pt.push_back(clusterCenters[objID[i]].x);
pt.push_back(clusterCenters[objID[i]].y); pt.push_back(clusterCenters[objID[i]].y);
@ -295,8 +281,6 @@ void KFT(const std_msgs::Float32MultiArray ccs)
float meas4[2] = {cc[4].at(0), cc[4].at(1)}; float meas4[2] = {cc[4].at(0), cc[4].at(1)};
float meas5[2] = {cc[5].at(0), cc[5].at(1)}; float meas5[2] = {cc[5].at(0), cc[5].at(1)};
// The update phase // The update phase
cv::Mat meas0Mat = cv::Mat(2, 1, CV_32F, meas0); cv::Mat meas0Mat = cv::Mat(2, 1, CV_32F, meas0);
cv::Mat meas1Mat = cv::Mat(2, 1, CV_32F, meas1); cv::Mat meas1Mat = cv::Mat(2, 1, CV_32F, meas1);
@ -319,45 +303,48 @@ if (!(meas4[0]==0.0f || meas4[1]==0.0f))
if (!(meas5[0] == 0.0f || meas5[1] == 0.0f)) if (!(meas5[0] == 0.0f || meas5[1] == 0.0f))
Mat estimated5 = KF5.correct(meas5Mat); Mat estimated5 = KF5.correct(meas5Mat);
// Publish the point clouds belonging to each clusters // Publish the point clouds belonging to each clusters
// cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n"; // cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
// Point statePt(estimated.at<float>(0),estimated.at<float>(1)); // Point statePt(estimated.at<float>(0),estimated.at<float>(1));
// cout<<"DONE KF_TRACKER\n"; // cout<<"DONE KF_TRACKER\n";
} }
void publish_cloud(ros::Publisher& pub, pcl::PointCloud<pcl::PointXYZ>::Ptr cluster){ void publish_cloud(ros::Publisher &pub,
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster) {
sensor_msgs::PointCloud2::Ptr clustermsg(new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr clustermsg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*cluster, *clustermsg); pcl::toROSMsg(*cluster, *clustermsg);
clustermsg->header.frame_id = "/map"; clustermsg->header.frame_id = "map";
clustermsg->header.stamp = ros::Time::now(); clustermsg->header.stamp = ros::Time::now();
pub.publish(*clustermsg); pub.publish(*clustermsg);
} }
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input) void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
{ {
// cout<<"IF firstFrame="<<firstFrame<<"\n"; // cout<<"IF firstFrame="<<firstFrame<<"\n";
// If this is the first frame, initialize kalman filters for the clustered objects // If this is the first frame, initialize kalman filters for the clustered
if (firstFrame) // objects
{ if (firstFrame) {
// Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset. // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset.
// Could be made generic by creating a Kalman Filter only when a new object is detected // Could be made generic by creating a Kalman Filter only when a new object
// is detected
float dvx = 0.01f; // 1.0 float dvx = 0.01f; // 1.0
float dvy = 0.01f; // 1.0 float dvy = 0.01f; // 1.0
float dx = 1.0f; float dx = 1.0f;
float dy = 1.0f; float dy = 1.0f;
KF0.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0, 0,dy,0,1, 0,0,dvx,0, 0,0,0,dvy); KF0.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
KF1.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0, 0,dy,0,1, 0,0,dvx,0, 0,0,0,dvy); dvx, 0, 0, 0, 0, dvy);
KF2.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0, 0,dy,0,1, 0,0,dvx,0, 0,0,0,dvy); KF1.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
KF3.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0, 0,dy,0,1, 0,0,dvx,0, 0,0,0,dvy); dvx, 0, 0, 0, 0, dvy);
KF4.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0, 0,dy,0,1, 0,0,dvx,0, 0,0,0,dvy); KF2.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
KF5.transitionMatrix = (Mat_<float>(4, 4) << dx,0,1,0, 0,dy,0,1, 0,0,dvx,0, 0,0,0,dvy); dvx, 0, 0, 0, 0, dvy);
KF3.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
dvx, 0, 0, 0, 0, dvy);
KF4.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
dvx, 0, 0, 0, 0, dvy);
KF5.transitionMatrix = (Mat_<float>(4, 4) << dx, 0, 1, 0, 0, dy, 0, 1, 0, 0,
dvx, 0, 0, 0, 0, dvy);
cv::setIdentity(KF0.measurementMatrix); cv::setIdentity(KF0.measurementMatrix);
cv::setIdentity(KF1.measurementMatrix); cv::setIdentity(KF1.measurementMatrix);
@ -389,16 +376,18 @@ if (firstFrame)
cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ)); cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(sigmaQ));
// Process the point cloud // Process the point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>); new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(
new pcl::PointCloud<pcl::PointXYZ>);
/* Creating the KdTree from input point cloud*/ /* Creating the KdTree from input point cloud*/
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
new pcl::search::KdTree<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *input_cloud); pcl::fromROSMsg(*input, *input_cloud);
tree->setInputCloud(input_cloud); tree->setInputCloud(input_cloud);
std::vector<pcl::PointIndices> cluster_indices; std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.08); ec.setClusterTolerance(0.08);
@ -409,7 +398,6 @@ if (firstFrame)
/* Extract the clusters out of pc and save indices in cluster_indices.*/ /* Extract the clusters out of pc and save indices in cluster_indices.*/
ec.extract(cluster_indices); ec.extract(cluster_indices);
std::vector<pcl::PointIndices>::const_iterator it; std::vector<pcl::PointIndices>::const_iterator it;
std::vector<int>::const_iterator pit; std::vector<int>::const_iterator pit;
// Vector of cluster pointclouds // Vector of cluster pointclouds
@ -419,24 +407,24 @@ if (firstFrame)
for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(
float x=0.0; float y=0.0; new pcl::PointCloud<pcl::PointXYZ>);
float x = 0.0;
float y = 0.0;
int numPts = 0; int numPts = 0;
for(pit = it->indices.begin(); pit != it->indices.end(); pit++) for (pit = it->indices.begin(); pit != it->indices.end(); pit++) {
{
cloud_cluster->points.push_back(input_cloud->points[*pit]); cloud_cluster->points.push_back(input_cloud->points[*pit]);
x += input_cloud->points[*pit].x; x += input_cloud->points[*pit].x;
y += input_cloud->points[*pit].y; y += input_cloud->points[*pit].y;
numPts++; numPts++;
// dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
// origin); // origin);
//mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster); // mindist_this_cluster = std::min(dist_this_point,
// mindist_this_cluster);
} }
pcl::PointXYZ centroid; pcl::PointXYZ centroid;
centroid.x = x / numPts; centroid.x = x / numPts;
centroid.y = y / numPts; centroid.y = y / numPts;
@ -446,19 +434,17 @@ if (firstFrame)
// Get the centroid of the cluster // Get the centroid of the cluster
clusterCentroids.push_back(centroid); clusterCentroids.push_back(centroid);
} }
// Ensure at least 6 clusters exist to publish (later clusters may be empty) // Ensure at least 6 clusters exist to publish (later clusters may be empty)
while (cluster_vec.size() < 6) { while (cluster_vec.size() < 6) {
pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster(
new pcl::PointCloud<pcl::PointXYZ>);
empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0)); empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0));
cluster_vec.push_back(empty_cluster); cluster_vec.push_back(empty_cluster);
} }
while (clusterCentroids.size()<6) while (clusterCentroids.size() < 6) {
{
pcl::PointXYZ centroid; pcl::PointXYZ centroid;
centroid.x = 0.0; centroid.x = 0.0;
centroid.y = 0.0; centroid.y = 0.0;
@ -467,7 +453,6 @@ if (firstFrame)
clusterCentroids.push_back(centroid); clusterCentroids.push_back(centroid);
} }
// Set initial state // Set initial state
KF0.statePre.at<float>(0) = clusterCentroids.at(0).x; KF0.statePre.at<float>(0) = clusterCentroids.at(0).x;
KF0.statePre.at<float>(1) = clusterCentroids.at(0).y; KF0.statePre.at<float>(1) = clusterCentroids.at(0).y;
@ -486,7 +471,6 @@ if (firstFrame)
KF2.statePre.at<float>(2) = 0; // initial v_x KF2.statePre.at<float>(2) = 0; // initial v_x
KF2.statePre.at<float>(3) = 0; // initial v_y KF2.statePre.at<float>(3) = 0; // initial v_y
// Set initial state // Set initial state
KF3.statePre.at<float>(0) = clusterCentroids.at(3).x; KF3.statePre.at<float>(0) = clusterCentroids.at(3).x;
KF3.statePre.at<float>(1) = clusterCentroids.at(3).y; KF3.statePre.at<float>(1) = clusterCentroids.at(3).y;
@ -507,8 +491,7 @@ if (firstFrame)
firstFrame = false; firstFrame = false;
for (int i=0;i<6;i++) for (int i = 0; i < 6; i++) {
{
geometry_msgs::Point pt; geometry_msgs::Point pt;
pt.x = clusterCentroids.at(i).x; pt.x = clusterCentroids.at(i).x;
pt.y = clusterCentroids.at(i).y; pt.y = clusterCentroids.at(i).y;
@ -522,27 +505,30 @@ if (firstFrame)
cout<<"KF4.satePre="<<KF4.statePre.at<float>(0)<<","<<KF4.statePre.at<float>(1)<<"\n"; cout<<"KF4.satePre="<<KF4.statePre.at<float>(0)<<","<<KF4.statePre.at<float>(1)<<"\n";
cout<<"KF5.satePre="<<KF5.statePre.at<float>(0)<<","<<KF5.statePre.at<float>(1)<<"\n"; cout<<"KF5.satePre="<<KF5.statePre.at<float>(0)<<","<<KF5.statePre.at<float>(1)<<"\n";
//cin.ignore();// To be able to see the printed initial state of the KalmanFilter //cin.ignore();// To be able to see the printed initial state of the
KalmanFilter
*/ */
} }
else {
else
{
// cout<<"ELSE firstFrame="<<firstFrame<<"\n"; // cout<<"ELSE firstFrame="<<firstFrame<<"\n";
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>); new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(
new pcl::PointCloud<pcl::PointXYZ>);
/* Creating the KdTree from input point cloud*/ /* Creating the KdTree from input point cloud*/
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
new pcl::search::KdTree<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *input_cloud); pcl::fromROSMsg(*input, *input_cloud);
tree->setInputCloud(input_cloud); tree->setInputCloud(input_cloud);
/* Here we are creating a vector of PointIndices, which contains the actual index /* Here we are creating a vector of PointIndices, which contains the actual
* information in a vector<int>. The indices of each detected cluster are saved here. * index information in a vector<int>. The indices of each detected cluster
* Cluster_indices is a vector containing one instance of PointIndices for each detected * are saved here. Cluster_indices is a vector containing one instance of
* cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud. * PointIndices for each detected cluster. Cluster_indices[0] contain all
* indices of the first cluster in input point cloud.
*/ */
std::vector<pcl::PointIndices> cluster_indices; std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
@ -571,26 +557,24 @@ else
// Cluster centroids // Cluster centroids
std::vector<pcl::PointXYZ> clusterCentroids; std::vector<pcl::PointXYZ> clusterCentroids;
for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
float x = 0.0;
for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) float y = 0.0;
{
float x=0.0; float y=0.0;
int numPts = 0; int numPts = 0;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(
for(pit = it->indices.begin(); pit != it->indices.end(); pit++) new pcl::PointCloud<pcl::PointXYZ>);
{ for (pit = it->indices.begin(); pit != it->indices.end(); pit++) {
cloud_cluster->points.push_back(input_cloud->points[*pit]); cloud_cluster->points.push_back(input_cloud->points[*pit]);
x += input_cloud->points[*pit].x; x += input_cloud->points[*pit].x;
y += input_cloud->points[*pit].y; y += input_cloud->points[*pit].y;
numPts++; numPts++;
// dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
// origin); // origin);
//mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster); // mindist_this_cluster = std::min(dist_this_point,
// mindist_this_cluster);
} }
pcl::PointXYZ centroid; pcl::PointXYZ centroid;
@ -602,19 +586,18 @@ else
// Get the centroid of the cluster // Get the centroid of the cluster
clusterCentroids.push_back(centroid); clusterCentroids.push_back(centroid);
} }
// cout<<"cluster_vec got some clusters\n"; // cout<<"cluster_vec got some clusters\n";
// Ensure at least 6 clusters exist to publish (later clusters may be empty) // Ensure at least 6 clusters exist to publish (later clusters may be empty)
while (cluster_vec.size() < 6) { while (cluster_vec.size() < 6) {
pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster(
new pcl::PointCloud<pcl::PointXYZ>);
empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0)); empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0));
cluster_vec.push_back(empty_cluster); cluster_vec.push_back(empty_cluster);
} }
while (clusterCentroids.size()<6) while (clusterCentroids.size() < 6) {
{
pcl::PointXYZ centroid; pcl::PointXYZ centroid;
centroid.x = 0.0; centroid.x = 0.0;
centroid.y = 0.0; centroid.y = 0.0;
@ -623,14 +606,11 @@ else
clusterCentroids.push_back(centroid); clusterCentroids.push_back(centroid);
} }
std_msgs::Float32MultiArray cc; std_msgs::Float32MultiArray cc;
for(int i=0;i<6;i++) for (int i = 0; i < 6; i++) {
{
cc.data.push_back(clusterCentroids.at(i).x); cc.data.push_back(clusterCentroids.at(i).x);
cc.data.push_back(clusterCentroids.at(i).y); cc.data.push_back(clusterCentroids.at(i).y);
cc.data.push_back(clusterCentroids.at(i).z); cc.data.push_back(clusterCentroids.at(i).z);
} }
// cout<<"6 clusters initialized\n"; // cout<<"6 clusters initialized\n";
@ -638,80 +618,69 @@ else
KFT(cc); KFT(cc);
int i = 0; int i = 0;
bool publishedCluster[6]; bool publishedCluster[6];
for(auto it=objID.begin();it!=objID.end();it++) for (auto it = objID.begin(); it != objID.end();
{ //cout<<"Inside the for loop\n"; it++) { // cout<<"Inside the for loop\n";
switch (i) {
switch(i)
{
cout << "Inside the switch case\n"; cout << "Inside the switch case\n";
case 0: { case 0: {
publish_cloud(pub_cluster0, cluster_vec[*it]); publish_cloud(pub_cluster0, cluster_vec[*it]);
publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID publishedCluster[i] =
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 1: { case 1: {
publish_cloud(pub_cluster1, cluster_vec[*it]); publish_cloud(pub_cluster1, cluster_vec[*it]);
publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID publishedCluster[i] =
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 2: { case 2: {
publish_cloud(pub_cluster2, cluster_vec[*it]); publish_cloud(pub_cluster2, cluster_vec[*it]);
publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID publishedCluster[i] =
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 3: { case 3: {
publish_cloud(pub_cluster3, cluster_vec[*it]); publish_cloud(pub_cluster3, cluster_vec[*it]);
publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID publishedCluster[i] =
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 4: { case 4: {
publish_cloud(pub_cluster4, cluster_vec[*it]); publish_cloud(pub_cluster4, cluster_vec[*it]);
publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID publishedCluster[i] =
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 5: { case 5: {
publish_cloud(pub_cluster5, cluster_vec[*it]); publish_cloud(pub_cluster5, cluster_vec[*it]);
publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID publishedCluster[i] =
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
default: break; default:
break;
}
}
}
} }
} int main(int argc, char **argv) {
}
}
int main(int argc, char** argv)
{
// ROS init // ROS init
ros::init(argc, argv, "kf_tracker"); ros::init(argc, argv, "kf_tracker");
ros::NodeHandle nh; ros::NodeHandle nh;
// Publishers to publish the state of the objects (pos and vel) // Publishers to publish the state of the objects (pos and vel)
// objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1); // objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
cout << "About to setup callback\n"; cout << "About to setup callback\n";
// Create a ROS subscriber for the input point cloud // Create a ROS subscriber for the input point cloud
@ -735,8 +704,5 @@ cout<<"About to setup callback\n";
/* Point cloud clustering /* Point cloud clustering
*/ */
ros::spin(); ros::spin();
} }

353
src/main_naive.cpp
View File

@ -1,41 +1,40 @@
#include <iostream>
#include <string.h>
#include <fstream>
#include <algorithm>
#include <iterator>
#include "kf_tracker/featureDetection.h"
#include "kf_tracker/CKalmanFilter.h" #include "kf_tracker/CKalmanFilter.h"
#include "kf_tracker/featureDetection.h"
#include "opencv2/video/tracking.hpp"
#include "pcl_ros/point_cloud.h"
#include <algorithm>
#include <fstream>
#include <geometry_msgs/Point.h>
#include <iostream>
#include <iterator>
#include <opencv2/core/core.hpp> #include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp> #include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp> #include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/video.hpp> #include <opencv2/video/video.hpp>
#include "opencv2/video/tracking.hpp"
#include <ros/ros.h>
#include <pcl/io/pcd_io.h> #include <pcl/io/pcd_io.h>
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include "pcl_ros/point_cloud.h" #include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/Float32MultiArray.h> #include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Int32MultiArray.h> #include <std_msgs/Int32MultiArray.h>
#include <string.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/geometry.h> #include <pcl/common/geometry.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h> #include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h> #include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h> #include <pcl/kdtree/kdtree.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h> #include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h> #include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h> #include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
using namespace std; using namespace std;
using namespace cv; using namespace cv;
ros::Publisher objID_pub; ros::Publisher objID_pub;
// KF init // KF init
@ -58,7 +57,6 @@ ros::Publisher objID_pub;
std::vector<geometry_msgs::Point> prevClusterCenters; std::vector<geometry_msgs::Point> prevClusterCenters;
cv::Mat state(stateDim, 1, CV_32F); cv::Mat state(stateDim, 1, CV_32F);
cv::Mat_<float> measurement(2, 1); cv::Mat_<float> measurement(2, 1);
@ -68,72 +66,71 @@ ros::Publisher objID_pub;
bool firstFrame = true; bool firstFrame = true;
// calculate euclidean distance of two points // calculate euclidean distance of two points
double euclidean_distance(geometry_msgs::Point& p1, geometry_msgs::Point& p2) double euclidean_distance(geometry_msgs::Point &p1, geometry_msgs::Point &p2) {
{ return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) +
return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z)); (p1.z - p2.z) * (p1.z - p2.z));
} }
/* /*
//Count unique object IDs. just to make sure same ID has not been assigned to two KF_Trackers. //Count unique object IDs. just to make sure same ID has not been assigned to
int countIDs(vector<int> v) two KF_Trackers. int countIDs(vector<int> v)
{ {
transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n = distance(v.end(), v.begin()) transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n =
sort(v.begin(), v.end()); // Average case O(n log n), worst case O(n^2) (usually implemented as quicksort. distance(v.end(), v.begin()) sort(v.begin(), v.end()); // Average case O(n log
// To guarantee worst case O(n log n) replace with make_heap, then sort_heap. n), worst case O(n^2) (usually implemented as quicksort.
// To guarantee worst case O(n log n) replace with make_heap, then
sort_heap.
// Unique will take a sorted range, and move things around to get duplicated // Unique will take a sorted range, and move things around to get duplicated
// items to the back and returns an iterator to the end of the unique section of the range // items to the back and returns an iterator to the end of the unique
auto unique_end = unique(v.begin(), v.end()); // Again n comparisons section of the range auto unique_end = unique(v.begin(), v.end()); // Again n
return distance(unique_end, v.begin()); // Constant time for random access iterators (like vector's) comparisons return distance(unique_end, v.begin()); // Constant time for random
access iterators (like vector's)
} }
*/ */
/* /*
objID: vector containing the IDs of the clusters that should be associated with each KF_Tracker objID: vector containing the IDs of the clusters that should be associated with
objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc. each KF_Tracker objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc.
*/ */
void KFT(const std_msgs::Float32MultiArray ccs) void KFT(const std_msgs::Float32MultiArray ccs) {
{
// First predict, to update the internal statePre variable // First predict, to update the internal statePre variable
std::vector<cv::Mat> pred{KF0.predict(),KF1.predict(),KF2.predict(),KF3.predict(),KF4.predict(),KF5.predict()}; std::vector<cv::Mat> pred{KF0.predict(), KF1.predict(), KF2.predict(),
KF3.predict(), KF4.predict(), KF5.predict()};
// cout<<"Pred successfull\n"; // cout<<"Pred successfull\n";
// cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1)); // cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
// cout<<"Prediction 1 ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n"; // cout<<"Prediction 1
// ="<<prediction.at<float>(0)<<","<<prediction.at<float>(1)<<"\n";
// Get measurements // Get measurements
// Extract the position of the clusters forom the multiArray. To check if the data // Extract the position of the clusters forom the multiArray. To check if the
// coming in, check the .z (every third) coordinate and that will be 0.0 // data coming in, check the .z (every third) coordinate and that will be 0.0
std::vector<geometry_msgs::Point> clusterCenters; // clusterCenters std::vector<geometry_msgs::Point> clusterCenters; // clusterCenters
int i = 0; int i = 0;
for (std::vector<float>::const_iterator it=ccs.data.begin();it!=ccs.data.end();it+=3) for (std::vector<float>::const_iterator it = ccs.data.begin();
{ it != ccs.data.end(); it += 3) {
geometry_msgs::Point pt; geometry_msgs::Point pt;
pt.x = *it; pt.x = *it;
pt.y = *(it + 1); pt.y = *(it + 1);
pt.z = *(it + 2); pt.z = *(it + 2);
clusterCenters.push_back(pt); clusterCenters.push_back(pt);
} }
// cout<<"CLusterCenters Obtained"<<"\n"; // cout<<"CLusterCenters Obtained"<<"\n";
std::vector<geometry_msgs::Point> KFpredictions; std::vector<geometry_msgs::Point> KFpredictions;
i = 0; i = 0;
for (auto it=pred.begin();it!=pred.end();it++) for (auto it = pred.begin(); it != pred.end(); it++) {
{
geometry_msgs::Point pt; geometry_msgs::Point pt;
pt.x = (*it).at<float>(0); pt.x = (*it).at<float>(0);
pt.y = (*it).at<float>(1); pt.y = (*it).at<float>(1);
pt.z = (*it).at<float>(2); pt.z = (*it).at<float>(2);
KFpredictions.push_back(pt); KFpredictions.push_back(pt);
} }
// cout<<"Got predictions"<<"\n"; // cout<<"Got predictions"<<"\n";
@ -147,9 +144,11 @@ void KFT(const std_msgs::Float32MultiArray ccs)
for(int n=0;n<6;n++) for(int n=0;n<6;n++)
distVec.push_back(euclidean_distance(KFpredictions[filterN],clusterCenters[n])); distVec.push_back(euclidean_distance(KFpredictions[filterN],clusterCenters[n]));
// cout<<"distVec[="<<distVec[0]<<","<<distVec[1]<<","<<distVec[2]<<","<<distVec[3]<<","<<distVec[4]<<","<<distVec[5]<<"\n"; //
cout<<"distVec[="<<distVec[0]<<","<<distVec[1]<<","<<distVec[2]<<","<<distVec[3]<<","<<distVec[4]<<","<<distVec[5]<<"\n";
objID.push_back(std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end()))); objID.push_back(std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end())));
// cout<<"MinD for filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n"; // cout<<"MinD for
filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
} }
@ -167,39 +166,36 @@ void KFT(const std_msgs::Float32MultiArray ccs)
/* Naive version without using kalman filter */ /* Naive version without using kalman filter */
objID.clear(); // Clear the objID vector objID.clear(); // Clear the objID vector
for(int filterN=0;filterN<6;filterN++) for (int filterN = 0; filterN < 6; filterN++) {
{
std::vector<float> distVec; std::vector<float> distVec;
for (int n = 0; n < 6; n++) for (int n = 0; n < 6; n++)
distVec.push_back(euclidean_distance(prevClusterCenters[n],clusterCenters[n])); distVec.push_back(
euclidean_distance(prevClusterCenters[n], clusterCenters[n]));
// cout<<"distVec[="<<distVec[0]<<","<<distVec[1]<<","<<distVec[2]<<","<<distVec[3]<<","<<distVec[4]<<","<<distVec[5]<<"\n"; // cout<<"distVec[="<<distVec[0]<<","<<distVec[1]<<","<<distVec[2]<<","<<distVec[3]<<","<<distVec[4]<<","<<distVec[5]<<"\n";
objID.push_back(std::distance(distVec.begin(),min_element(distVec.begin(),distVec.end()))); objID.push_back(std::distance(distVec.begin(),
// cout<<"MinD for filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n"; min_element(distVec.begin(), distVec.end())));
// cout<<"MinD for
// filter"<<filterN<<"="<<*min_element(distVec.begin(),distVec.end())<<"\n";
} }
/* Naive version without kalman filter */ /* Naive version without kalman filter */
// prevClusterCenters.clear(); // prevClusterCenters.clear();
// Set the current associated clusters to the prevClusterCenters // Set the current associated clusters to the prevClusterCenters
for (int i=0;i<6;i++) for (int i = 0; i < 6; i++) {
{
prevClusterCenters[objID.at(i)] = clusterCenters.at(i); prevClusterCenters[objID.at(i)] = clusterCenters.at(i);
} }
/* Naive version without kalman filter */ /* Naive version without kalman filter */
/* Naive version without using kalman filter */ /* Naive version without using kalman filter */
std_msgs::Int32MultiArray obj_id; std_msgs::Int32MultiArray obj_id;
for (auto it = objID.begin(); it != objID.end(); it++) for (auto it = objID.begin(); it != objID.end(); it++)
obj_id.data.push_back(*it); obj_id.data.push_back(*it);
objID_pub.publish(obj_id); objID_pub.publish(obj_id);
// convert clusterCenters from geometry_msgs::Point to floats // convert clusterCenters from geometry_msgs::Point to floats
std::vector<std::vector<float>> cc; std::vector<std::vector<float>> cc;
for (int i=0;i<clusterCenters.size();i++) for (int i = 0; i < clusterCenters.size(); i++) {
{
vector<float> pt; vector<float> pt;
pt.push_back(clusterCenters[i].x); pt.push_back(clusterCenters[i].x);
pt.push_back(clusterCenters[i].y); pt.push_back(clusterCenters[i].y);
@ -215,8 +211,6 @@ objID.clear();//Clear the objID vector
float meas4[3] = {cc[4].at(0), cc[4].at(1)}; float meas4[3] = {cc[4].at(0), cc[4].at(1)};
float meas5[3] = {cc[5].at(0), cc[5].at(1)}; float meas5[3] = {cc[5].at(0), cc[5].at(1)};
// The update phase // The update phase
cv::Mat meas0Mat = cv::Mat(2, 1, CV_32F, meas0); cv::Mat meas0Mat = cv::Mat(2, 1, CV_32F, meas0);
cv::Mat meas1Mat = cv::Mat(2, 1, CV_32F, meas1); cv::Mat meas1Mat = cv::Mat(2, 1, CV_32F, meas1);
@ -236,37 +230,41 @@ objID.clear();//Clear the objID vector
// Publish the point clouds belonging to each clusters // Publish the point clouds belonging to each clusters
// cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n"; // cout<<"estimate="<<estimated.at<float>(0)<<","<<estimated.at<float>(1)<<"\n";
// Point statePt(estimated.at<float>(0),estimated.at<float>(1)); // Point statePt(estimated.at<float>(0),estimated.at<float>(1));
// cout<<"DONE KF_TRACKER\n"; // cout<<"DONE KF_TRACKER\n";
} }
void publish_cloud(ros::Publisher& pub, pcl::PointCloud<pcl::PointXYZ>::Ptr cluster){ void publish_cloud(ros::Publisher &pub,
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster) {
sensor_msgs::PointCloud2::Ptr clustermsg(new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr clustermsg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*cluster, *clustermsg); pcl::toROSMsg(*cluster, *clustermsg);
clustermsg->header.frame_id = "/map"; clustermsg->header.frame_id = "map";
clustermsg->header.stamp = ros::Time::now(); clustermsg->header.stamp = ros::Time::now();
pub.publish(*clustermsg); pub.publish(*clustermsg);
} }
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input) void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
{ {
cout << "IF firstFrame=" << firstFrame << "\n"; cout << "IF firstFrame=" << firstFrame << "\n";
// If this is the first frame, initialize kalman filters for the clustered objects // If this is the first frame, initialize kalman filters for the clustered
if (firstFrame) // objects
{ if (firstFrame) {
// Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset. // Initialize 6 Kalman Filters; Assuming 6 max objects in the dataset.
// Could be made generic by creating a Kalman Filter only when a new object is detected // Could be made generic by creating a Kalman Filter only when a new object
KF0.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); // is detected
KF1.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); KF0.transitionMatrix =
KF2.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
KF3.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); KF1.transitionMatrix =
KF4.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); *(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
KF5.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1); KF2.transitionMatrix =
*(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
KF3.transitionMatrix =
*(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
KF4.transitionMatrix =
*(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
KF5.transitionMatrix =
*(Mat_<float>(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1);
cv::setIdentity(KF0.measurementMatrix); cv::setIdentity(KF0.measurementMatrix);
cv::setIdentity(KF1.measurementMatrix); cv::setIdentity(KF1.measurementMatrix);
@ -296,19 +294,23 @@ if (firstFrame)
cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(1e-1)); cv::setIdentity(KF5.measurementNoiseCov, cv::Scalar(1e-1));
// Process the point cloud // Process the point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>); new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(
new pcl::PointCloud<pcl::PointXYZ>);
/* Creating the KdTree from input point cloud*/ /* Creating the KdTree from input point cloud*/
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
new pcl::search::KdTree<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *input_cloud); pcl::fromROSMsg(*input, *input_cloud);
tree->setInputCloud(input_cloud); tree->setInputCloud(input_cloud);
/* Here we are creating a vector of PointIndices, which contains the actual index /* Here we are creating a vector of PointIndices, which contains the actual
* information in a vector<int>. The indices of each detected cluster are saved here. * index information in a vector<int>. The indices of each detected cluster
* Cluster_indices is a vector containing one instance of PointIndices for each detected * are saved here. Cluster_indices is a vector containing one instance of
* cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud. * PointIndices for each detected cluster. Cluster_indices[0] contain all
* indices of the first cluster in input point cloud.
*/ */
std::vector<pcl::PointIndices> cluster_indices; std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
@ -334,84 +336,98 @@ if (firstFrame)
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_vec; std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_vec;
for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(
for(pit = it->indices.begin(); pit != it->indices.end(); pit++) new pcl::PointCloud<pcl::PointXYZ>);
{ for (pit = it->indices.begin(); pit != it->indices.end(); pit++) {
cloud_cluster->points.push_back(input_cloud->points[*pit]); cloud_cluster->points.push_back(input_cloud->points[*pit]);
// dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
// origin); // origin);
//mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster); // mindist_this_cluster = std::min(dist_this_point,
// mindist_this_cluster);
} }
cluster_vec.push_back(cloud_cluster); cluster_vec.push_back(cloud_cluster);
} }
// Ensure at least 6 clusters exist to publish (later clusters may be empty) // Ensure at least 6 clusters exist to publish (later clusters may be empty)
while (cluster_vec.size() < 6) { while (cluster_vec.size() < 6) {
pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster(
new pcl::PointCloud<pcl::PointXYZ>);
empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0)); empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0));
cluster_vec.push_back(empty_cluster); cluster_vec.push_back(empty_cluster);
} }
// Set initial state // Set initial state
KF0.statePre.at<float>(0)=cluster_vec[0]->points[cluster_vec[0]->points.size()/2].x; KF0.statePre.at<float>(0) =
cluster_vec[0]->points[cluster_vec[0]->points.size() / 2].x;
KF0.statePre.at<float>(1)=cluster_vec[0]->points[cluster_vec[0]->points.size()/2].y; KF0.statePre.at<float>(1) =
cluster_vec[0]->points[cluster_vec[0]->points.size() / 2].y;
KF0.statePre.at<float>(2) = 0; // initial v_x KF0.statePre.at<float>(2) = 0; // initial v_x
KF0.statePre.at<float>(3) = 0; // initial v_y KF0.statePre.at<float>(3) = 0; // initial v_y
// Set initial state // Set initial state
KF1.statePre.at<float>(0)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].x; KF1.statePre.at<float>(0) =
KF1.statePre.at<float>(1)=cluster_vec[1]->points[cluster_vec[1]->points.size()/2].y; cluster_vec[1]->points[cluster_vec[1]->points.size() / 2].x;
KF1.statePre.at<float>(1) =
cluster_vec[1]->points[cluster_vec[1]->points.size() / 2].y;
KF1.statePre.at<float>(2) = 0; // initial v_x KF1.statePre.at<float>(2) = 0; // initial v_x
KF1.statePre.at<float>(3) = 0; // initial v_y KF1.statePre.at<float>(3) = 0; // initial v_y
// Set initial state // Set initial state
KF2.statePre.at<float>(0)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].x; KF2.statePre.at<float>(0) =
KF2.statePre.at<float>(1)=cluster_vec[2]->points[cluster_vec[2]->points.size()/2].y; cluster_vec[2]->points[cluster_vec[2]->points.size() / 2].x;
KF2.statePre.at<float>(1) =
cluster_vec[2]->points[cluster_vec[2]->points.size() / 2].y;
KF2.statePre.at<float>(2) = 0; // initial v_x KF2.statePre.at<float>(2) = 0; // initial v_x
KF2.statePre.at<float>(3) = 0; // initial v_y KF2.statePre.at<float>(3) = 0; // initial v_y
// Set initial state // Set initial state
KF3.statePre.at<float>(0)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].x; KF3.statePre.at<float>(0) =
KF3.statePre.at<float>(1)=cluster_vec[3]->points[cluster_vec[3]->points.size()/2].y; cluster_vec[3]->points[cluster_vec[3]->points.size() / 2].x;
KF3.statePre.at<float>(1) =
cluster_vec[3]->points[cluster_vec[3]->points.size() / 2].y;
KF3.statePre.at<float>(2) = 0; // initial v_x KF3.statePre.at<float>(2) = 0; // initial v_x
KF3.statePre.at<float>(3) = 0; // initial v_y KF3.statePre.at<float>(3) = 0; // initial v_y
// Set initial state // Set initial state
KF4.statePre.at<float>(0)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].x; KF4.statePre.at<float>(0) =
KF4.statePre.at<float>(1)=cluster_vec[4]->points[cluster_vec[4]->points.size()/2].y; cluster_vec[4]->points[cluster_vec[4]->points.size() / 2].x;
KF4.statePre.at<float>(1) =
cluster_vec[4]->points[cluster_vec[4]->points.size() / 2].y;
KF4.statePre.at<float>(2) = 0; // initial v_x KF4.statePre.at<float>(2) = 0; // initial v_x
KF4.statePre.at<float>(3) = 0; // initial v_y KF4.statePre.at<float>(3) = 0; // initial v_y
// Set initial state // Set initial state
KF5.statePre.at<float>(0)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].x; KF5.statePre.at<float>(0) =
KF5.statePre.at<float>(1)=cluster_vec[5]->points[cluster_vec[5]->points.size()/2].y; cluster_vec[5]->points[cluster_vec[5]->points.size() / 2].x;
KF5.statePre.at<float>(1) =
cluster_vec[5]->points[cluster_vec[5]->points.size() / 2].y;
KF5.statePre.at<float>(2) = 0; // initial v_x KF5.statePre.at<float>(2) = 0; // initial v_x
KF5.statePre.at<float>(3) = 0; // initial v_y KF5.statePre.at<float>(3) = 0; // initial v_y
firstFrame = false; firstFrame = false;
// Print the initial state of the kalman filter for debugging // Print the initial state of the kalman filter for debugging
cout<<"KF0.satePre="<<KF0.statePre.at<float>(0)<<","<<KF0.statePre.at<float>(1)<<"\n"; cout << "KF0.satePre=" << KF0.statePre.at<float>(0) << ","
cout<<"KF1.satePre="<<KF1.statePre.at<float>(0)<<","<<KF1.statePre.at<float>(1)<<"\n"; << KF0.statePre.at<float>(1) << "\n";
cout<<"KF2.satePre="<<KF2.statePre.at<float>(0)<<","<<KF2.statePre.at<float>(1)<<"\n"; cout << "KF1.satePre=" << KF1.statePre.at<float>(0) << ","
cout<<"KF3.satePre="<<KF3.statePre.at<float>(0)<<","<<KF3.statePre.at<float>(1)<<"\n"; << KF1.statePre.at<float>(1) << "\n";
cout<<"KF4.satePre="<<KF4.statePre.at<float>(0)<<","<<KF4.statePre.at<float>(1)<<"\n"; cout << "KF2.satePre=" << KF2.statePre.at<float>(0) << ","
cout<<"KF5.satePre="<<KF5.statePre.at<float>(0)<<","<<KF5.statePre.at<float>(1)<<"\n"; << KF2.statePre.at<float>(1) << "\n";
cout << "KF3.satePre=" << KF3.statePre.at<float>(0) << ","
//cin.ignore();// To be able to see the printed initial state of the KalmanFilter << KF3.statePre.at<float>(1) << "\n";
cout << "KF4.satePre=" << KF4.statePre.at<float>(0) << ","
<< KF4.statePre.at<float>(1) << "\n";
cout << "KF5.satePre=" << KF5.statePre.at<float>(0) << ","
<< KF5.statePre.at<float>(1) << "\n";
// cin.ignore();// To be able to see the printed initial state of the
// KalmanFilter
/* Naive version without kalman filter */ /* Naive version without kalman filter */
for (int i=0;i<6;i++) for (int i = 0; i < 6; i++) {
{
geometry_msgs::Point pt; geometry_msgs::Point pt;
pt.x = cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].x; pt.x = cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].x;
pt.y = cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].y; pt.y = cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].y;
@ -421,23 +437,25 @@ if (firstFrame)
/* Naive version without kalman filter */ /* Naive version without kalman filter */
} }
else {
else
{
cout << "ELSE firstFrame=" << firstFrame << "\n"; cout << "ELSE firstFrame=" << firstFrame << "\n";
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>); new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud(
new pcl::PointCloud<pcl::PointXYZ>);
/* Creating the KdTree from input point cloud*/ /* Creating the KdTree from input point cloud*/
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
new pcl::search::KdTree<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *input_cloud); pcl::fromROSMsg(*input, *input_cloud);
tree->setInputCloud(input_cloud); tree->setInputCloud(input_cloud);
/* Here we are creating a vector of PointIndices, which contains the actual index /* Here we are creating a vector of PointIndices, which contains the actual
* information in a vector<int>. The indices of each detected cluster are saved here. * index information in a vector<int>. The indices of each detected cluster
* Cluster_indices is a vector containing one instance of PointIndices for each detected * are saved here. Cluster_indices is a vector containing one instance of
* cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud. * PointIndices for each detected cluster. Cluster_indices[0] contain all
* indices of the first cluster in input point cloud.
*/ */
std::vector<pcl::PointIndices> cluster_indices; std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
@ -464,35 +482,36 @@ cout<<"PCL extract successfull\n";
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_vec; std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_vec;
for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) { for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(
for(pit = it->indices.begin(); pit != it->indices.end(); pit++) new pcl::PointCloud<pcl::PointXYZ>);
{ for (pit = it->indices.begin(); pit != it->indices.end(); pit++) {
cloud_cluster->points.push_back(input_cloud->points[*pit]); cloud_cluster->points.push_back(input_cloud->points[*pit]);
// dist_this_point = pcl::geometry::distance(input_cloud->points[*pit], // dist_this_point = pcl::geometry::distance(input_cloud->points[*pit],
// origin); // origin);
//mindist_this_cluster = std::min(dist_this_point, mindist_this_cluster); // mindist_this_cluster = std::min(dist_this_point,
// mindist_this_cluster);
} }
cluster_vec.push_back(cloud_cluster); cluster_vec.push_back(cloud_cluster);
} }
// cout<<"cluster_vec got some clusters\n"; // cout<<"cluster_vec got some clusters\n";
// Ensure at least 6 clusters exist to publish (later clusters may be empty) // Ensure at least 6 clusters exist to publish (later clusters may be empty)
while (cluster_vec.size() < 6) { while (cluster_vec.size() < 6) {
pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster (new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr empty_cluster(
new pcl::PointCloud<pcl::PointXYZ>);
empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0)); empty_cluster->points.push_back(pcl::PointXYZ(0, 0, 0));
cluster_vec.push_back(empty_cluster); cluster_vec.push_back(empty_cluster);
} }
std_msgs::Float32MultiArray cc; std_msgs::Float32MultiArray cc;
for(int i=0;i<6;i++) for (int i = 0; i < 6; i++) {
{ cc.data.push_back(
cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].x); cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].x);
cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].y); cc.data.push_back(
cc.data.push_back(cluster_vec[i]->points[cluster_vec[i]->points.size()/2].z); cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].y);
cc.data.push_back(
cluster_vec[i]->points[cluster_vec[i]->points.size() / 2].z);
} }
cout << "6 clusters initialized\n"; cout << "6 clusters initialized\n";
@ -500,79 +519,69 @@ cout<<"PCL extract successfull\n";
KFT(cc); KFT(cc);
int i = 0; int i = 0;
bool publishedCluster[6]; bool publishedCluster[6];
for(auto it=objID.begin();it!=objID.end();it++) for (auto it = objID.begin(); it != objID.end(); it++) {
{ cout<<"Inside the for loop\n"; cout << "Inside the for loop\n";
switch(*it) switch (*it) {
{
cout << "Inside the switch case\n"; cout << "Inside the switch case\n";
case 0: { case 0: {
publish_cloud(pub_cluster0, cluster_vec[i]); publish_cloud(pub_cluster0, cluster_vec[i]);
publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID publishedCluster[i] =
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 1: { case 1: {
publish_cloud(pub_cluster1, cluster_vec[i]); publish_cloud(pub_cluster1, cluster_vec[i]);
publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID publishedCluster[i] =
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 2: { case 2: {
publish_cloud(pub_cluster2, cluster_vec[i]); publish_cloud(pub_cluster2, cluster_vec[i]);
publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID publishedCluster[i] =
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 3: { case 3: {
publish_cloud(pub_cluster3, cluster_vec[i]); publish_cloud(pub_cluster3, cluster_vec[i]);
publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID publishedCluster[i] =
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 4: { case 4: {
publish_cloud(pub_cluster4, cluster_vec[i]); publish_cloud(pub_cluster4, cluster_vec[i]);
publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID publishedCluster[i] =
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
case 5: { case 5: {
publish_cloud(pub_cluster5, cluster_vec[i]); publish_cloud(pub_cluster5, cluster_vec[i]);
publishedCluster[i]=true;//Use this flag to publish only once for a given obj ID publishedCluster[i] =
true; // Use this flag to publish only once for a given obj ID
i++; i++;
break; break;
} }
default: break; default:
break;
}
}
}
} }
} int main(int argc, char **argv) {
}
}
int main(int argc, char** argv)
{
// ROS init // ROS init
ros::init(argc, argv, "KFTracker"); ros::init(argc, argv, "KFTracker");
ros::NodeHandle nh; ros::NodeHandle nh;
// Publishers to publish the state of the objects (pos and vel) // Publishers to publish the state of the objects (pos and vel)
// objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1); // objState1=nh.advertise<geometry_msgs::Twist> ("obj_1",1);
cout << "About to setup callback\n"; cout << "About to setup callback\n";
// Create a ROS subscriber for the input point cloud // Create a ROS subscriber for the input point cloud
@ -592,12 +601,8 @@ cout<<"About to setup callback\n";
// cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1 // cc_pos=nh.advertise<std_msgs::Float32MultiArray>("ccs",100);//clusterCenter1
/* Point cloud clustering /* Point cloud clustering
*/ */
ros::spin(); ros::spin();
} }