ClusterSet::ClusterSet(const FeatureSetPtr &fs, const std::vector<int> & clusters)
{
if (fs->getNumberOfFeatures() != clusters.size())
{
ROS_ERROR("[ClusterSet::ClusterSet] Number of Features in the Set and the number of Id's in the vector of integers does NOT match. Number of features: "
"%d Number of Id's: %d", fs->getNumberOfFeatures(), clusters.size());
throw std::string(
"[ClusterSet::ClusterSet] Number of Features in the Set and the number of Id's in the vector of integers does NOT match.");
}
std::map<int, ClusterPtr> clusters_map;
std::vector<int>::const_iterator int_it = clusters.begin();
std::vector<int>::const_iterator int_it_end = clusters.end();
FeatureSet::iterator fs_it = fs->begin();
FeatureSet::iterator fs_it_end = fs->end();
for (; int_it != int_it_end && fs_it != fs_it_end; int_it++, fs_it++)
{
if ((*int_it) != -1)
{
std::map<int, ClusterPtr>::iterator cs_find = clusters_map.find(*(int_it));
if (cs_find == clusters_map.end())
{
ClusterPtr new_cluster = ClusterPtr(new Cluster(fs->getFrame(), *(int_it)));
new_cluster->addFeature((*fs_it)->clone());
clusters_map[*(int_it)] = new_cluster;
}
else
{
cs_find->second->addFeature((*fs_it)->clone());
}
}
}
std::map<int, ClusterPtr>::iterator c_map_it = clusters_map.begin();
std::map<int, ClusterPtr>::iterator c_map_it_end = clusters_map.end();
for (; c_map_it != c_map_it_end; c_map_it++)
{
this->_clusters.push_back(c_map_it->second->clone());
}
}
int nnClustering( vector<PointPtr>& points, double threshold , vector<ClusterPtr>& clusters_nn)
{
/* Note to myself:
The make_pair STL (Standard Template Library) function creates a pair structure that contains two data elements of any type
*/
//Create empty point associations
// ros::Time tic = ros::Time::now();
vector<pair<ClusterPtr,bool> > point_association;
point_association.resize( points.size(), make_pair(ClusterPtr(), false));
int idc = 0;
//Determining the number of clusters
for(uint idx = 1; idx < points.size(); idx++)
{
//If the point wasn't yet associated with any cluster
if( point_association[idx].second == false )
{
//Create new cluster_nn
ClusterPtr cluster_nn(new Cluster);
idc++;
cluster_nn->ranges.push_back(points[idx]->range);
cluster_nn->support_points.push_back(points[idx]);
cluster_nn->id = idc;
point_association[idx].first = cluster_nn;
point_association[idx].second = true;
clusters_nn.push_back(cluster_nn);
}
//Go though all the other points and see which ones associate with this measurement
recursiveClustering(points, point_association , threshold , idx );
} //end for
for(uint m=0; m<clusters_nn.size() ;m++)
{
clusters_nn[m]->centroid = calculateClusterCentroid(clusters_nn[m]->support_points);
clusters_nn[m]->central_point = calculateClusterMedian( clusters_nn[m]->support_points );
}
//----------------------------------------------------------------------------
// ros::Time toc = ros::Time::now();
// double duration = (toc-tic).toSec();
// stringstream ss;
// ss << "src/durations/SNN_DUR.txt";
// ofstream fpc(ss.str().c_str(), ios::app);
//
// if (!fpc.is_open())
// {
// cout << "Couldn't open fpc" << endl;
// return 1;
// }
// fpc<< duration << endl;
// fpc.close();
//----------------------------------------------------------------------------
//cout<<"number of clusters nearest neighbour: "<<clusters_nn.size()<<endl;
return clusters_nn.size();
} //end function
